use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BObservation in project BoofCV by lessthanoptimal.
the class VisOdomBundlePnPBase method triangulateNotSelectedBundleTracks.
/**
* Triangulate tracks which were not included in the optimization
*/
protected void triangulateNotSelectedBundleTracks() {
final int minObservationsTriangulate = this.minObservationsTriangulate;
for (int trackIdx = 0; trackIdx < bundleViso.tracks.size; trackIdx++) {
final BTrack bt = bundleViso.tracks.data[trackIdx];
// results will be unstable
if (bt.selected || bt.observations.size < minObservationsTriangulate)
continue;
observationsNorm.reset();
listOf_world_to_frame.reset();
for (int obsIdx = 0; obsIdx < bt.observations.size; obsIdx++) {
BObservation bo = bt.observations.get(obsIdx);
CameraModel cm = cameraModels.get(bo.frame.camera.index);
cm.pixelToNorm.compute(bo.pixel.x, bo.pixel.y, observationsNorm.grow());
bo.frame.frame_to_world.invert(listOf_world_to_frame.grow());
// NOTE: This invert could be cached. Doesn't need to be done a million times
}
// NOTE: If there is a homogenous metric triangulation added in the future replace this with that
if (triangulateN.triangulate(observationsNorm.toList(), listOf_world_to_frame.toList(), found3D)) {
bt.worldLoc.x = found3D.x;
bt.worldLoc.y = found3D.y;
bt.worldLoc.z = found3D.z;
bt.worldLoc.w = 1.0;
}
}
}
use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BObservation in project BoofCV by lessthanoptimal.
the class TestVisOdomBundleAdjustment method addObservation.
@Test
void addObservation() {
VisOdomBundleAdjustment<BTrack> alg = createAlgSingleCamera();
BFrame frameA = alg.addFrame(0);
BTrack trackA = alg.addTrack(1, 2, 3, 3);
alg.addObservation(frameA, trackA, 5, 10);
assertEquals(1, frameA.tracks.size);
assertEquals(1, trackA.observations.size);
BObservation obs = trackA.observations.get(0);
assertSame(frameA, obs.frame);
assertEquals(0.0, obs.pixel.distance(5, 10), UtilEjml.TEST_F64);
}
use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BObservation in project BoofCV by lessthanoptimal.
the class MaxGeoKeyFrameManager method selectOldToDiscard.
/**
* Selects an older frame to discard. If a frame has zero features in common with the current frame it
* will be selected. After that it scores frames based on how many features it has in common with the other
* frames, excluding the current frame.
*
* @param totalDiscard How many need to be discarded
*/
protected void selectOldToDiscard(VisOdomBundleAdjustment<?> sba, int totalDiscard) {
if (totalDiscard <= 0 || sba.frames.size < 2)
return;
frameToIndex.clear();
for (int i = 0; i < sba.frames.size; i++) {
frameToIndex.put(sba.frames.get(i).id, i);
}
// Create a histogram showing how observations connect the frames
final int N = sba.frames.size;
histogram.reshape(N, N);
histogram.zero();
// Skip the last row since it's not needed
for (int frameIdxA = 0; frameIdxA < N - 1; frameIdxA++) {
BFrame frame = sba.frames.get(frameIdxA);
for (int trackIdx = 0; trackIdx < frame.tracks.size; trackIdx++) {
BTrack track = frame.tracks.get(trackIdx);
for (int obsIdx = 0; obsIdx < track.observations.size; obsIdx++) {
BObservation o = track.observations.get(obsIdx);
int frameIdxB = frameToIndex.get(o.frame.id);
if (frameIdxA == frameIdxB)
continue;
histogram.increment(frameIdxA, frameIdxB);
}
}
}
if (verbose != null)
histogram.print("%4d");
// See which keyframe the current frame has the best connection with
// Most of the time it will be the previous frame, but if that was experienced a lot of motion blur it might
// not be...
int bestFrame = histogram.maximumRowIdx(N - 1);
if (verbose != null)
verbose.println("Frame with best connection to current " + bestFrame);
// mark it so that it's skipped
histogram.set(bestFrame, bestFrame, Integer.MAX_VALUE);
for (int i = 0; i < totalDiscard; i++) {
// Select the frame with the worst connection to the bestFrame. The reason the bestFrame is used and not
// the current frame is that the current frame could be blurred too and might get discarded
int lowestCount = Integer.MAX_VALUE;
int worstIdx = -1;
// N-1 to avoid the current frame
for (int frameIdx = 0; frameIdx < N - 1; frameIdx++) {
int connection = histogram.get(frameIdx, bestFrame);
if (connection == Integer.MAX_VALUE)
continue;
// influence over the current frame's state and have no direct influence over bestFrame's state
if (connection == 0) {
if (verbose != null)
verbose.println("No connection index " + frameIdx);
lowestCount = 0;
worstIdx = frameIdx;
break;
}
if (connection < lowestCount) {
lowestCount = connection;
worstIdx = frameIdx;
}
}
if (verbose != null)
verbose.println("Worst index " + worstIdx + " count " + lowestCount);
discardKeyIndices.add(worstIdx);
// Mark the worst keyframe so it won't be selected again
histogram.set(worstIdx, bestFrame, Integer.MAX_VALUE);
}
}
Aggregations