Adds features which were triangulated using the stereo pair after the scale factor has been determined.
Don't mark the other view as being processed. It's 3D pose will be estimated later on using PNP with the
new features and features determined later on
321193
void addTriangulatedStereoFeatures(View base , Motion edge , double scale ) {
		View viewA = edge.viewSrc;
		View viewB = edge.viewDst;

		boolean baseIsA = base == viewA;
		View other = baseIsA ? viewB : viewA;

		// Determine transform from other to world
		edge.a_to_b.T.scale(scale);
		Se3_F64 otherToBase = baseIsA ? edge.a_to_b.invert(null) : edge.a_to_b.copy();
		otherToBase.concat(base.viewToWorld, other.viewToWorld);

		// Convert already computed stereo 3D features and turn them into real features
		for (int i = 0; i < edge.stereoTriangulations.size(); i++) {
			Feature3D edge3D = edge.stereoTriangulations.get(i);

			int indexSrc = edge3D.obsIdx.get(0);
			int indexDst = edge3D.obsIdx.get(1);

			Feature3D world3D = baseIsA ? viewA.features3D[indexSrc] : viewB.features3D[indexDst];

			// find the 3D location of the point in world frame
			edge3D.worldPt.scale(scale);
			if( baseIsA ) {
				viewA.viewToWorld.transform(edge3D.worldPt, edge3D.worldPt);
			} else {
				edge.a_to_b.transform(edge3D.worldPt, edge3D.worldPt);
				viewB.viewToWorld.transform(edge3D.worldPt, edge3D.worldPt);
			}

			// See if the feature is already known
			if( world3D != null ) {
				// Add the other view if another feature in the other view was not already associated with this feature
				if( !world3D.views.contains(other) ) {
					world3D.views.add(other);
					world3D.obsIdx.add( baseIsA ? indexDst : indexSrc );
				}

				// Retriangulate the point if it appears that this stereo pair is better than the one which originally
				// computed it
				if( world3D.triangulationAngle >= edge3D.triangulationAngle ) {
					continue;
				}
				world3D.worldPt.set(edge3D.worldPt);
				world3D.triangulationAngle = edge3D.triangulationAngle;
				other.features3D[baseIsA ? indexDst : indexSrc] = edge3D;
			} else {
				graph.features3D.add(edge3D);
				viewA.features3D[indexSrc] = edge3D;
				viewB.features3D[indexDst] = edge3D;
			}
		}

		// free memory
		edge.stereoTriangulations = new ArrayList<>();
	}
876860
private void project3(double[] output) {
		int observationIndex = 0;
		for( int viewIndex = 0; viewIndex < structure.views.length; viewIndex++ ) {
			SceneStructureProjective.View view = structure.views[viewIndex];
			SceneObservations.View obsView = observations.views[viewIndex];
			for (int i = 0; i < obsView.size(); i++) {
				obsView.get(i,observedPixel);
				SceneStructureMetric.Point worldPt = structure.points[observedPixel.index];
				worldPt.get(p3);
				PerspectiveOps.renderPixel(view.worldToView, p3, predictedPixel);
				int outputIndex = observationIndex*2;
				output[outputIndex  ] = predictedPixel.x - observedPixel.x;
				output[outputIndex+1] = predictedPixel.y - observedPixel.y;
				observationIndex++;
			}
		}
	}
8165
public void poseFromBones()
    {
        for (int i = 0; i < getNumBones(); ++i)
        {
            GVRSceneObject bone = mBones[i];
            if (bone == null)
            {
                continue;
            }
            if ((mBoneOptions[i] & BONE_LOCK_ROTATION) != 0)
            {
                continue;
            }
            GVRTransform trans = bone.getTransform();
            mPose.setLocalMatrix(i, trans.getLocalModelMatrix4f());
        }
        mPose.sync();
        updateBonePose();
    }
702688
public void triangulate() {
        double minArea = 1000 * charLength * DOUBLE_PREC;
        newFaces.clear();
        for (Iterator it = faces.iterator(); it.hasNext();) {
            Face face = (Face) it.next();
            if (face.mark == Face.VISIBLE) {
                face.triangulate(newFaces, minArea);
                // splitFace (face);
            }
        }
        for (Face face = newFaces.first(); face != null; face = face.next) {
            faces.add(face);
        }
    }
459259
def getPose3d(self):
        '''
        

        '''	   
        self.lock.acquire()
        pose = self.pose
        self.lock.release()
        return pose
1316494
public void setDepthsFrom3D(int view , List<Point3D_F64> locations ) {
		if( locations.size() != pixels.numCols )
			throw new IllegalArgumentException("Pixel count must be constant and match "+pixels.numCols);

		int N = depths.numCols;
		for (int i = 0; i < N; i++) {
			depths.set(view,i, locations.get(i).z );
		}
	}
457829
private Vector2f[] triangulate(Vector2f[] result) {
		// Step 1: Compute all angles
		contour.computeAngles();
		for (PointBag hole = holes; hole != null; hole = hole.next) {
			hole.computeAngles();
		}

		// Step 2: Connect the holes with the contour (build bridges)
		while (holes != null) {
			Point pHole = holes.first;
			outer: do {
				if (pHole.angle <= 0) {
					Point pContour = contour.first;
					do {
						inner: if (pHole.isInfront(pContour)
								&& pContour.isInfront(pHole)) {
							if (!contour.doesIntersectSegment(pHole.pt,
									pContour.pt)) {
								PointBag hole = holes;
								do {
									if (hole.doesIntersectSegment(pHole.pt,
											pContour.pt)) {
										break inner;
									}
								} while ((hole = hole.next) != null);

								Point newPtContour = getPoint(pContour.pt);
								pContour.insertAfter(newPtContour);

								Point newPtHole = getPoint(pHole.pt);
								pHole.insertBefore(newPtHole);

								pContour.next = pHole;
								pHole.prev = pContour;

								newPtHole.next = newPtContour;
								newPtContour.prev = newPtHole;

								pContour.computeAngle();
								pHole.computeAngle();
								newPtContour.computeAngle();
								newPtHole.computeAngle();

								// detach the points from the hole
								holes.first = null;
								break outer;
							}
						}
					} while ((pContour = pContour.next) != contour.first);
				}
			} while ((pHole = pHole.next) != holes.first);

			// free the hole
			holes = freePointBag(holes);
		}

		// Step 3: Make sure we have enough space for the result
		int numTriangles = contour.countPoints() - 2;
		int neededSpace = numTriangles * 3 + 1; // for the null
		if (result.length < neededSpace) {
			result = (Vector2f[]) Array.newInstance(result.getClass()
					.getComponentType(), neededSpace);
		}

		// Step 4: Extract the triangles
		int idx = 0;
		for (;;) {
			Point pContour = contour.first;

			if (pContour == null) {
				break;
			}
			// Are there 2 or less points left ?
			if (pContour.next == pContour.prev) {
				break;
			}

			outer: do {
				if (pContour.angle > 0) {
					Point prev = pContour.prev;
					Point next = pContour.next;

					if (next.next == prev || prev.isInfront(next) && next.isInfront(prev)) {
						if (!contour.doesIntersectSegment(prev.pt, next.pt)) {
							result[idx++] = pContour.pt;
							result[idx++] = next.pt;
							result[idx++] = prev.pt;
							break outer;
						}
					}
				}
			} while ((pContour = pContour.next) != contour.first);
			
			// remove the point - we do it in every case to prevent endless loop
			Point prev = pContour.prev;
			Point next = pContour.next;

			contour.first = prev;
			pContour.unlink();
			freePoint(pContour);

			next.computeAngle();
			prev.computeAngle();
		}

		// Step 5: Append a null (see Collection.toArray)
		result[idx] = null;

		// Step 6: Free memory
		contour.clear();

		// Finished !
		return result;
	}
198204
def updateframe(t, raw, wavelen, cam, ax, fg):
    showcb = False

    ttxt = f'Cam {cam.name}: '

    if raw.ndim == 3:
        frame = raw[t, ...]
    elif raw.ndim == 2:
        frame = raw
    elif raw.ndim == 1:  # GeoData
        frame = raw.reshape((sqrt(raw.size), -1))
    else:
        raise ValueError('ndim==3 or 2')
# %% filtering (optional) # FIXME provide noise estimate
    
    if 'wiener' in cam.cp:
        # psf = ones((cam.wiener, cam.wiener)) / cam.wiener**2
        frame = wiener(frame, cam.cp['wiener'])

    if 'medfilt2d' in cam.cp:
        frame = medfilt2d(frame.astype(float), cam.cp['medfilt2d'])

    if 'denoise_bilateral' in cam.cp:
        frame = skres.denoise_bilateral(((frame - cam.clim[0]) / (cam.clim[1] - cam.clim[0])).clip(0., 1.),
                                        sigma_color=0.05, sigma_spatial=15, multichannel=False)
    if 'denoise_tv_chambolle' in cam.cp:
        frame = skres.denoise_tv_chambolle(((frame - cam.clim[0]) / (cam.clim[1] - cam.clim[0])).clip(0., 1.),
                                           )
# %% plotting raw uint16 data
    if False:
        v = vis.HistEqStretch(frame)
        NORM = ImageNormalize(stretch=v)

    NORM = None
    #  NORM = LogNorm()
    # NORM = ImageNormalize(stretch=vis.LogStretch())

    hi = ax.imshow(frame,
                   origin='lower', interpolation='none',
                   # aspect='equal',
                   # extent=(0,C.superx,0,C.supery),
                   vmin=cam.clim[0], vmax=cam.clim[1],
                   cmap='gray', norm=NORM)

    # autoscale(False) for case where we put plots on top of image
    # yet still reduces blank space between subplots
    ax.autoscale(False)

    if showcb:  # showing the colorbar makes the plotting go 5-10x more slowly
        hc = fg.colorbar(hi, ax=ax)  # not cax!
        hc.set_label(f'{raw.dtype} data numbers')

    dtframe = datetime.utcfromtimestamp(cam.tKeo[t])

    if cam.name == 'asi':
        dtstr = datetime.strftime(dtframe, '%H:%M:%S')
        if int(wavelen[t]) == 428:
            tcolor = 'blue'
        elif int(wavelen[t]) == 557:
            tcolor = 'limegreen'
        elif int(wavelen[t]) == 630:
            tcolor = 'red'
        else:
            tcolor = 'limegreen'
        ttxt += f'{dtstr} $\lambda$ {wavelen[t]:.1f}'
    else:
        dtstr = datetime.strftime(dtframe, '%H:%M:%S.%f')[:-3]  # millisecond
        tcolor = 'limegreen'
        ttxt += f'{dtstr}'

    ax.set_title(ttxt, color=tcolor)

    ax.set_axis_off()  # no ticks

    if False:
        ax.set_xlabel('x-pixel')
        if cam.name == 0:
            ax.set_ylabel('y-pixel')
# %% plotting 1D cut line
    try:
        ax.plot(cam.cutcol[cam.Lcind], cam.cutrow[cam.Lcind],
                marker='.', linestyle='none', color='blue', markersize=1, alpha=0.5)
        # plot magnetic zenith
        ax.scatter(x=cam.cutcol[cam.angleMagzenind],
                   y=cam.cutrow[cam.angleMagzenind],
                   marker='o', facecolors='none', color='red', s=500, linewidth=2, alpha=0.5)
    except AttributeError:  # asi
        pass
# %% plot cleanup
    ax.grid(False)  # in case Seaborn is used
    return dtframe
1207167
private void addNewTracks() {
		tracker.spawnTracks();
		List<PointTrack> spawned = tracker.getNewTracks(null);

		// estimate 3D coordinate using stereo vision
		for (PointTrack t : spawned) {
//			System.out.println("track spawn "+t.x+" "+t.y);
			VoTrack p = t.getCookie();
			if (p == null) {
				t.cookie = p = new VoTrack();
			}

			// compute normalized image coordinate
			pixelToNorm.compute(t.x, t.y, n);

//			System.out.println("         pointing "+pointing.x+" "+pointing.y+" "+pointing.z);

			// See if the point ever intersects the ground plane or not
			if (planeProjection.normalToPlane(n.x, n.y, p.ground)) {
				// the line above computes the 2D plane position of the point
				p.onPlane = true;
			} else {
				// Handle points at infinity which are off plane.

				// rotate observation pointing vector into plane reference frame
				pointing.set(n.x, n.y, 1);
				GeometryMath_F64.mult(cameraToPlane.getR(), pointing, pointing);
				pointing.normalize();

				// save value of y-axis in pointing vector
				double normXZ = Math.sqrt(pointing.x * pointing.x + pointing.z * pointing.z);
				p.pointingY = pointing.y / normXZ;

				// save the angle as a vector
				p.ground.x = pointing.z;
				p.ground.y = -pointing.x;
				// normalize to make later calculations easier
				p.ground.x /= normXZ;
				p.ground.y /= normXZ;

				p.onPlane = false;
			}

			p.lastInlier = tick;
		}
	}
674542
public static void triangulatePoints(SceneStructureMetric structure , SceneObservations observations )
	{
		TriangulateNViewsMetric triangulation = FactoryMultiView.
				triangulateNViewCalibrated(ConfigTriangulation.GEOMETRIC);

		List<RemoveBrownPtoN_F64> list_p_to_n = new ArrayList<>();
		for (int i = 0; i < structure.cameras.length; i++) {
			RemoveBrownPtoN_F64 p2n = new RemoveBrownPtoN_F64();
			if( structure.cameras[i].model instanceof BundlePinholeSimplified ) {
				BundlePinholeSimplified cam = (BundlePinholeSimplified) structure.cameras[i].model;
				p2n.setK(cam.f, cam.f, 0, 0, 0).setDistortion(new double[]{cam.k1, cam.k2}, 0, 0);
			} else if( structure.cameras[i].model instanceof BundlePinhole) {
				BundlePinhole cam = (BundlePinhole) structure.cameras[i].model;
				p2n.setK(cam.fx, cam.fy, cam.skew, cam.cx, cam.cy).setDistortion(new double[]{0,0}, 0,0);
			} else if( structure.cameras[i].model instanceof BundlePinholeBrown) {
				BundlePinholeBrown cam = (BundlePinholeBrown) structure.cameras[i].model;
				p2n.setK(cam.fx, cam.fy, cam.skew, cam.cx, cam.cy).setDistortion(cam.radial, cam.t1, cam.t2);
			} else {
				throw new RuntimeException("Unknown camera model!");
			}
			list_p_to_n.add(p2n);
		}

		FastQueue<Point2D_F64> normObs = new FastQueue<>(Point2D_F64.class,true);
		normObs.resize(3);

		Point3D_F64 X = new Point3D_F64();

		List<Se3_F64> worldToViews = new ArrayList<>();
		for (int i = 0; i < structure.points.length; i++) {
			normObs.reset();
			worldToViews.clear();
			SceneStructureMetric.Point sp = structure.points[i];
			for (int j = 0; j < sp.views.size; j++) {
				int viewIdx = sp.views.get(j);
				SceneStructureMetric.View v = structure.views[viewIdx];
				worldToViews.add(v.worldToView);

				// get the observation in pixels
				Point2D_F64 n = normObs.grow();
				int pointidx = observations.views[viewIdx].point.indexOf(i);
				observations.views[viewIdx].get(pointidx,n);
				// convert to normalized image coordinates
				list_p_to_n.get(v.camera).compute(n.x,n.y,n);
			}

			triangulation.triangulate(normObs.toList(),worldToViews,X);
			sp.set(X.x,X.y,X.z);
		}
	}
1061656
def update_features(self, poly):
        """"""

        for feature in self.features:
            feature.wavelength = poly(feature.xpos)
646328
@Override
	public boolean process(PairwiseImageGraph pairwiseGraph ) {
		this.graph = new MetricSceneGraph(pairwiseGraph);
		for (int i = 0; i < graph.edges.size(); i++) {
			decomposeEssential(graph.edges.get(i));
		}

		declareModelFitting();

		for (int i = 0; i < graph.edges.size(); i++) {
			Motion e = graph.edges.get(i);
			e.triangulationAngle = medianTriangulationAngle(e);
		}

		if( verbose != null )
			verbose.println("Selecting root");
		// Select the view which will act as the origin
		View origin = selectOriginNode();
		// Select the motion which will define the coordinate system
		Motion baseMotion = selectCoordinateBase( origin );

		this.graph.sanityCheck();

		if( verbose != null )
			verbose.println("Stereo triangulation");
		// Triangulate features in all motions which exceed a certain angle
		for (int i = 0; i < graph.edges.size() && !stopRequested ; i++) {
			Motion e = graph.edges.get(i);
			if( e.triangulationAngle > Math.PI/10 || e == baseMotion) {
				triangulateStereoEdges(e);
				if( verbose != null ) {
					int a = e.viewSrc.index;
					int b = e.viewDst.index;
					verbose.println("   Edge[" + i + "] "+a+"->"+b+"  feat3D="+e.stereoTriangulations.size());
				}
			}
		}
		if( stopRequested )
			return false;

		if( verbose != null )
			verbose.println("Defining the coordinate system");

		// Using the selecting coordinate frames and triangulated points define the coordinate system
		defineCoordinateSystem(origin, baseMotion);
		if( stopRequested )
			return false;

		if( verbose != null )
			verbose.println("Estimate all features");

		// Now estimate all the other view locations and 3D features
		estimateAllFeatures(origin, baseMotion.destination(origin));
		if( stopRequested )
			return false;

		// Convert the graph into the output format
		convertToOutput(origin);

		return viewsAdded.size() >= 2;
	}
503385
private static void checkBehindCamera(SceneStructureMetric structure, SceneObservations observations, BundleAdjustment<SceneStructureMetric> bundleAdjustment) {

		int totalBehind = 0;
		Point3D_F64 X = new Point3D_F64();
		for (int i = 0; i < structure.points.length; i++) {
			structure.points[i].get(X);
			if( X.z < 0 )
				totalBehind++;
		}
		structure.views[1].worldToView.T.print();
		if( totalBehind > structure.points.length/2 ) {
			System.out.println("Flipping because it's reversed. score = "+bundleAdjustment.getFitScore());
			for (int i = 1; i < structure.views.length; i++) {
				Se3_F64 w2v = structure.views[i].worldToView;
				w2v.set(w2v.invert(null));
			}
			triangulatePoints(structure,observations);

			bundleAdjustment.setParameters(structure,observations);
			bundleAdjustment.optimize(structure);
			System.out.println("  after = "+bundleAdjustment.getFitScore());
		} else {
			System.out.println("Points not behind camera. "+totalBehind+" / "+structure.points.length);
		}
	}
1118870
private void addNewTracks() {
		trackerLeft.spawnTracks();
		trackerRight.spawnTracks();

		List<PointTrack> newLeft = trackerLeft.getNewTracks(null);
		List<PointTrack> newRight = trackerRight.getNewTracks(null);

		// get a list of new tracks and their descriptions
		addNewToList(inputLeft, newLeft, pointsLeft, descLeft);
		addNewToList(inputRight,newRight,pointsRight,descRight);

		// associate using L2R
		assocL2R.setSource(pointsLeft,descLeft);
		assocL2R.setDestination(pointsRight, descRight);
		assocL2R.associate();
		FastQueue<AssociatedIndex> matches = assocL2R.getMatches();

		// storage for the triangulated location in the camera frame
		Point3D_F64 cameraP3 = new Point3D_F64();

		for( int i = 0; i < matches.size; i++ ) {
			AssociatedIndex m = matches.get(i);

			PointTrack trackL = newLeft.get(m.src);
			PointTrack trackR = newRight.get(m.dst);

			// declare additional track information stored in each track.  Tracks can be recycled so it
			// might not always need to be declared
			LeftTrackInfo infoLeft = trackL.getCookie();
			if( infoLeft == null )
				trackL.cookie = infoLeft = new LeftTrackInfo();

			RightTrackInfo infoRight = trackR.getCookie();
			if( infoRight == null )
				trackR.cookie = infoRight = new RightTrackInfo();

			Stereo2D3D p2d3d = infoLeft.location;

			// convert pixel observations into normalized image coordinates
			leftImageToNorm.compute(trackL.x,trackL.y,p2d3d.leftObs);
			rightImageToNorm.compute(trackR.x,trackR.y,p2d3d.rightObs);

			// triangulate 3D coordinate in the current camera frame
			if( triangulate.triangulate(p2d3d.leftObs,p2d3d.rightObs,leftToRight,cameraP3) )
			{
				// put the track into the current keyframe coordinate system
				SePointOps_F64.transform(currToKey,cameraP3,p2d3d.location);
				// save a reference to the matching track in the right camera frame
				infoLeft.right = trackR;
				infoLeft.lastConsistent = infoLeft.lastInlier = tick;
				infoRight.left = trackL;
			} else {
				// triangulation failed, drop track
				trackerLeft.dropTrack(trackL);
				// TODO need way to mark right tracks which are unassociated after this loop
				throw new RuntimeException("This special case needs to be handled!");
			}
		}

		// drop tracks that were not associated
		GrowQueue_I32 unassignedRight = assocL2R.getUnassociatedDestination();
		for( int i = 0; i < unassignedRight.size; i++ ) {
			int index = unassignedRight.get(i);
//			System.out.println(" unassigned right "+newRight.get(index).x+" "+newRight.get(index).y);
			trackerRight.dropTrack(newRight.get(index));
		}
		GrowQueue_I32 unassignedLeft = assocL2R.getUnassociatedSource();
		for( int i = 0; i < unassignedLeft.size; i++ ) {
			int index = unassignedLeft.get(i);
			trackerLeft.dropTrack(newLeft.get(index));
		}

//		System.out.println("Total left "+trackerLeft.getAllTracks(null).size()+"  right "+trackerRight.getAllTracks(null).size());

//		System.out.println("Associated: "+matches.size+" new left "+newLeft.size()+" new right "+newRight.size());
//		System.out.println("New Tracks: Total: Left "+trackerLeft.getAllTracks(null).size()+" right "+
//				trackerRight.getAllTracks(null).size());

//		List<PointTrack> temp = trackerLeft.getActiveTracks(null);
//		for( PointTrack t : temp ) {
//			if( t.cookie == null )
//				System.out.println("BUG!");
//		}
//		temp = trackerRight.getActiveTracks(null);
//		for( PointTrack t : temp ) {
//			if( t.cookie == null )
//				System.out.println("BUG!");
//		}
	}
237254
void defineCoordinateSystem(View viewA, Motion motion) {

		View viewB = motion.destination(viewA);
		viewA.viewToWorld.reset(); // identity since it's the origin
		viewB.viewToWorld.set(motion.motionSrcToDst(viewB));
		// translation is only known up to a scale factor so pick a reasonable scale factor
		double scale = viewB.viewToWorld.T.norm();
		viewB.viewToWorld.T.scale(1.0/scale);

		viewsAdded.add(viewA);
		viewsAdded.add(viewB);

		viewA.state = ViewState.PROCESSED;
		viewB.state = ViewState.PROCESSED;

		// Take the already triangulated points and turn them into official 3D features
		boolean originIsDst = viewA == motion.viewDst;
		for (int i = 0; i < motion.stereoTriangulations.size(); i++) {
			Feature3D f = motion.stereoTriangulations.get(i);

			if( f.obsIdx.size != 2 )
				throw new RuntimeException("BUG");

			int indexSrc = f.obsIdx.get(0);
			int indexDst = f.obsIdx.get(1);

			motion.viewSrc.features3D[indexSrc] = f;
			motion.viewDst.features3D[indexDst] = f;

			if( originIsDst ) {
				SePointOps_F64.transform(motion.a_to_b,f.worldPt,f.worldPt);
			}

			f.worldPt.scale(1.0/scale);
			graph.features3D.add(f);
		}

		// free memory and mark as already processed
		motion.stereoTriangulations = new ArrayList<>();

		// All features which can be added using triangulation should now be added
		addTriangulatedFeaturesForAllEdges(viewA);
		addTriangulatedFeaturesForAllEdges(viewB);

		if( verbose != null ) {
			verbose.println("root  = " + viewA.index);
			verbose.println("other = " + viewB.index);
			verbose.println("-------------");
		}
	}
274765
public void set(Vector3D v1, Vector3D v2, Vector3D v3) {
        this.MODE = TRIANGLE_3D;
        this.x1 = v1.getX();
        this.y1 = v1.getY();
        this.z1 = v1.getZ();
        this.x2 = v2.getX();
        this.y2 = v2.getY();
        this.z1 = v2.getZ();
        this.x3 = v3.getX();
        this.y3 = v3.getY();
        this.z1 = v3.getZ();
        calcG();
    }
504422
def addScalarBar3D(
        self,
        pos=(0, 0, 0),
        normal=(0, 0, 1),
        sx=0.1,
        sy=2,
        nlabels=9,
        ncols=256,
        cmap=None,
        c="k",
        alpha=1,
    ):
        
        # book it, it will be created by Plotter.show() later
        self.scalarbar = [pos, normal, sx, sy, nlabels, ncols, cmap, c, alpha]
        return self