Computes the error given the motion model

@param obs Observation in normalized pixel coordinates
@return observation error
487575
@Override
	public double computeDistance(AssociatedPair obs) {

		// triangulate the point in 3D space
		triangulate.triangulate(obs.p1,obs.p2,keyToCurr,p);

		if( p.z < 0 )
			return Double.MAX_VALUE;

		// compute observational error in each view
		double error = errorCam1.errorSq(obs.p1.x,obs.p1.y,p.x/p.z,p.y/p.z);

		SePointOps_F64.transform(keyToCurr,p,p);
		if( p.z < 0 )
			return Double.MAX_VALUE;

		error += errorCam2.errorSq(obs.p2.x,obs.p2.y, p.x/p.z , p.y/p.z);

		return error;
	}
714126
public <T extends CameraModel>T process() {
		if( zhang99 == null )
			throw new IllegalArgumentException("Please call configure first.");
		zhang99.setVerbose(verbose,0);
		if( !zhang99.process(observations) ) {
			throw new RuntimeException("Zhang99 algorithm failed!");
		}

		structure = zhang99.getStructure();

		errors = zhang99.computeErrors();

		foundIntrinsic = zhang99.getCameraModel();
		foundIntrinsic.width = imageWidth;
		foundIntrinsic.height = imageHeight;

		return (T)foundIntrinsic;
	}
43666
private SceneObservations createObservationsForBundleAdjustment(LookupSimilarImages db, View seed, GrowQueue_I32 motions) {
		// seed view + the motions
		SceneObservations observations = new SceneObservations(motions.size+1);

		// Observations for the seed view are a special case
		SceneObservations.View obsView = observations.getView(0);
		for (int i = 0; i < inlierToSeed.size; i++) {
			int id = inlierToSeed.data[i];
			Point2D_F64 o = featsA.get(id); // featsA is never modified after initially loaded

			id = seedToStructure.data[id];
			obsView.add(id,(float)o.x,(float)o.y);
		}

		// Now add observations for edges connected to the seed
		for (int i = 0; i < motions.size(); i++) {
			obsView = observations.getView(i+1);
			Motion m = seed.connections.get(motions.get(i));
			View v = m.other(seed);
			boolean seedIsSrc = m.src == seed;
			db.lookupPixelFeats(v.id,featsB);
			for (int j = 0; j < m.inliers.size; j++) {
				AssociatedIndex a = m.inliers.get(j);
				int id = seedToStructure.data[seedIsSrc?a.src:a.dst];
				if( id < 0 )
					continue;
				Point2D_F64 o = featsB.get(seedIsSrc?a.dst:a.src);
				obsView.add(id,(float)o.x,(float)o.y);
			}
		}
		return observations;
	}
555974
public double getCovariance() {
		if(covariance == null) {
			Observation o = new Observation();
			for(int i=0; i<observationA.getObservationCount(); i++)
				o.addValue(observationA.getValueAt(i)*observationB.getValueAt(i));
			covariance = o.getExpectation()-observationA.getExpectation()*observationB.getExpectation();
		}
		return covariance;
	}
1341241
protected boolean estimate( Quadrilateral_F64 cornersPixels ,
								Quadrilateral_F64 cornersNorm ,
								Se3_F64 foundFiducialToCamera ) {
		// put it into a list to simplify algorithms
		listObs.clear();
		listObs.add( cornersPixels.a );
		listObs.add( cornersPixels.b );
		listObs.add( cornersPixels.c );
		listObs.add( cornersPixels.d );

		// convert observations into normalized image coordinates which P3P requires
		points.get(0).observation.set(cornersNorm.a);
		points.get(1).observation.set(cornersNorm.b);
		points.get(2).observation.set(cornersNorm.c);
		points.get(3).observation.set(cornersNorm.d);

		// estimate pose using all permutations
		bestError = Double.MAX_VALUE;
		estimateP3P(0);
		estimateP3P(1);
		estimateP3P(2);
		estimateP3P(3);

		if( bestError == Double.MAX_VALUE )
			return false;

		// refine the best estimate
		inputP3P.clear();
		for( int i = 0; i < 4; i++ ) {
			inputP3P.add( points.get(i) );
		}

		// got poor or horrible solution the first way, let's try it with EPNP
		// and see if it does better
		if( bestError > 2 ) {
			if (epnp.process(inputP3P, foundEPNP)) {
				if( foundEPNP.T.z > 0 ) {
					double error = computeErrors(foundEPNP);
//					System.out.println("   error EPNP = "+error);
					if (error < bestError) {
						bestPose.set(foundEPNP);
					}
				}
			}
		}

		if( !refine.fitModel(inputP3P,bestPose,foundFiducialToCamera) ) {
			// us the previous estimate instead
			foundFiducialToCamera.set(bestPose);
			return true;
		}

		return true;
	}
915830
double medianTriangulationAngle( Motion edge ) {

		GrowQueue_F64 angles = new GrowQueue_F64(edge.associated.size());
		angles.size = edge.associated.size();

		for (int i = 0; i < edge.associated.size(); i++) {
			AssociatedIndex a = edge.associated.get(i);
			Point2D_F64 normA = edge.viewSrc.observationNorm.get( a.src );
			Point2D_F64 normB = edge.viewDst.observationNorm.get( a.dst );

			double acute = triangulationAngle(normA,normB,edge.a_to_b);
			angles.data[i] = acute;
		}

		angles.sort();
		return angles.getFraction(0.5);
	}
372529
protected boolean linearEstimate(List<CalibrationObservation> observations  )
	{
		status("Estimating Homographies");
		List<DMatrixRMaj> homographies = new ArrayList<>();
		List<Se3_F64> motions = new ArrayList<>();

		for( CalibrationObservation obs : observations ) {
			if( !computeHomography.computeHomography(obs) )
				return false;

			DMatrixRMaj H = computeHomography.getHomography();

			homographies.add(H);
		}

		status("Estimating Calibration Matrix");
		computeK.process(homographies);

		DMatrixRMaj K = computeK.getCalibrationMatrix();

		decomposeH.setCalibrationMatrix(K);
		for( DMatrixRMaj H : homographies ) {
			motions.add(decomposeH.decompose(H));
		}

		status("Estimating Radial Distortion");
		computeRadial.process(K, homographies, observations);

		double distort[] = computeRadial.getParameters();

		convertIntoBundleStructure(motions, K,distort,observations);
		return true;
	}
862749
protected static void computeCosineWindow( GrayF64 cosine ) {
		double cosX[] = new double[ cosine.width ];
		for( int x = 0; x < cosine.width; x++ ) {
			cosX[x] = 0.5*(1 - Math.cos( 2.0*Math.PI*x/(cosine.width-1) ));
		}
		for( int y = 0; y < cosine.height; y++ ) {
			int index = cosine.startIndex + y*cosine.stride;
			double cosY = 0.5*(1 - Math.cos( 2.0*Math.PI*y/(cosine.height-1) ));
			for( int x = 0; x < cosine.width; x++ ) {
				cosine.data[index++] = cosX[x]*cosY;
			}
		}
	}
1015633
public static <T> ObservableTransformer<T,T> doOnErrorCountFx(Consumer<Integer> onError) {
        return obs -> obs.compose(doOnErrorCount(i -> runOnFx(i,onError)));
    }
826864
public void convertIntoBundleStructure(List<Se3_F64> motions,
										   DMatrixRMaj K,
										   double[] distort,
										   List<CalibrationObservation> obs ) {

		structure = new SceneStructureMetric(false);
		observations = new SceneObservations(motions.size(),true);

		structure.initialize(1,motions.size(),layout.size(),1);

		// A single camera is assumed, that's what is being calibrated!
		structure.setCamera(0,false,cameraGenerator.initalizeCamera(K,distort));
		// A single rigid planar target is being viewed. It is assumed to be centered at the origin
		structure.setRigid(0,true,new Se3_F64(),layout.size());
		// Where the points are on the calibration target
		SceneStructureMetric.Rigid rigid = structure.rigids[0];
		for (int i = 0; i < layout.size(); i++) {
			rigid.setPoint(i,layout.get(i).x,layout.get(i).y,0);
		}

		// Add the initial estimate of each view's location and the points observed
		for (int viewIdx = 0; viewIdx < motions.size(); viewIdx++) {
			structure.setView(viewIdx,false,motions.get(viewIdx));
			SceneObservations.View v = observations.getViewRigid(viewIdx);
			structure.connectViewToCamera(viewIdx,0);
			CalibrationObservation ca = obs.get(viewIdx);
			for (int j = 0; j < ca.size(); j++) {
				PointIndex2D_F64 p = ca.get(j);
				v.add(p.index, (float)p.x, (float)p.y);
				structure.connectPointToView(p.index,viewIdx);
			}
		}
	}
214975
public void computeErrorVector( DMatrixRMaj A , DMatrixRMaj errors ) {
		errors.reshape(A.numRows,1);
		CommonOps_DDRM.mult(A,vectorT,errors);
	}
1126708
def res_from_obseravtion_data(observation_data):
    
    res_df = observation_data.copy()
    res_df.loc[:, "name"] = res_df.pop("obsnme")
    res_df.loc[:, "measured"] = res_df.pop("obsval")
    res_df.loc[:, "group"] = res_df.pop("obgnme")
    res_df.loc[:, "modelled"] = np.NaN
    res_df.loc[:, "residual"] = np.NaN
    return res_df
391304
public static <T> ObservableTransformer<T,T> doOnErrorCount(Consumer<Integer> onError) {
        return obs -> obs.lift(new OperatorEmissionCounter<>(new CountObserver(null,null,onError)));
    }
465133
def nnz_obs(self):
        
        nnz = 0
        for w in self.observation_data.weight:
            if w > 0.0:
                nnz += 1
        return nnz
878780
void computeProjectionTable( int width , int height ) {
		output.reshape(width,height);
		depthMap.reshape(width,height);

		ImageMiscOps.fill(depthMap,-1);

		pointing = new float[width*height*3];

		for (int y = 0; y < output.height; y++) {
			for (int x = 0; x < output.width; x++) {
				// Should this add 0.5 so that the ray goes out of the pixel's center? Seems to increase reprojection
				// error in calibration tests if I do...
				pixelTo3.compute(x, y, p3);
				if(UtilEjml.isUncountable(p3.x)) {
					depthMap.unsafe_set(x,y,Float.NaN);
				} else {
					pointing[(y*output.width+x)*3   ] = (float)p3.x;
					pointing[(y*output.width+x)*3+1 ] = (float)p3.y;
					pointing[(y*output.width+x)*3+2 ] = (float)p3.z;
				}
			}
		}
	}
204866
@Override
	public void motionDetected(WebcamMotionEvent wme) {
		for (Point p : wme.getPoints()) {
			motionPoints.put(p, 0);
		}
	}
14170
def render_obs(self, obs):
    """"""
    start_time = time.time()
    self._obs = obs
    self.check_valid_queued_action()
    self._update_camera(point.Point.build(
        self._obs.observation.raw_data.player.camera))

    for surf in self._surfaces:
      # Render that surface.
      surf.draw(surf)

    mouse_pos = self.get_mouse_pos()
    if mouse_pos:
      # Draw a small mouse cursor
      self.all_surfs(_Surface.draw_circle, colors.green, mouse_pos.world_pos,
                     0.1)

    self.draw_actions()

    with sw("flip"):
      pygame.display.flip()

    self._render_times.append(time.time() - start_time)