boofcv.alg.sfm.d3.VisOdomMonoPlaneInfinity Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-sfm Show documentation
Show all versions of boofcv-sfm Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* Copyright (c) 2011-2019, Peter Abeles. All Rights Reserved.
*
* This file is part of BoofCV (http://boofcv.org).
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package boofcv.alg.sfm.d3;
import boofcv.abst.feature.tracker.PointTrack;
import boofcv.abst.feature.tracker.PointTracker;
import boofcv.alg.sfm.overhead.CameraPlaneProjection;
import boofcv.factory.distort.LensDistortionFactory;
import boofcv.struct.calib.CameraPinholeBrown;
import boofcv.struct.distort.Point2Transform2_F64;
import boofcv.struct.image.ImageBase;
import boofcv.struct.sfm.PlanePtPixel;
import georegression.geometry.GeometryMath_F64;
import georegression.metric.UtilAngle;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se2_F64;
import georegression.struct.se.Se3_F64;
import georegression.transform.se.SePointOps_F64;
import org.ddogleg.fitting.modelset.ModelMatcher;
import org.ddogleg.struct.FastQueue;
import org.ddogleg.struct.GrowQueue_F64;
import org.ejml.data.DMatrixRMaj;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
/**
* Estimates camera ego-motion by assuming the camera is viewing a flat plane and that off plane points are at infinity.
* These assumptions are often true for robots traveling over flat terrain outdoors. There are no assumptions for
* orientation of the camera, e.g. it could be upside down and at an angle relative to the plane.
*
* A point feature tracker is used to extract sparse optical flow from the image sequence. Bad tracks are removed
* first through the use of geometric constraints and then through robust estimation techniques which search for
* maximum inlier sets. Before a track is discarded it has to be not used for X frames in a row, where X is user
* specified. Determination of inliers is done through a user specified threshold in pixels.
*
* Points on the plane are used to estimate translation and rotation, while points at infinity are just used to estimate
* rotation. The two rotation estimates are merged together using a weighted vector average. Motion estimation does not
* require any off plane points at infinity, but does require points on the plane. All motion estimation is done
* internally in 2D, but can be converted into 3D.
*
* It is possible to turn on a more rigid test for off plane points at infinity. If 'strictFar' is set to true then
* any points which contains motions that could not be generated by a rotation about the y-axis are discarded. This
* appears to be too strict and discard an excessive number of points when vehicle pitch up and down. Pitching motions
* will often not affect motion estimates from projected vectors on to the plane.
*
* @author Peter Abeles
*/
public class VisOdomMonoPlaneInfinity> {
// Motion estimator for points on plane. Motion estimated is from key-frame to current-frame
private ModelMatcher planeMotion;
// storage for data passed into planeMotion
private FastQueue planeSamples = new FastQueue<>(PlanePtPixel.class, true);
// when the inlier set is less than this number new features are detected
private int thresholdAdd;
// discard tracks after they have not been in the inlier set for this many updates in a row
private int thresholdRetire;
// maximum allowed pixel error. Used for determining which tracks are inliers/outliers
private double thresholdPixelError;
// Should it use the more strict rule for pruning far away points?
private boolean strictFar = false;
// maximum allowed angle error for far pixels computed using pixel threshold and intrinsic parameters
private double thresholdFarAngleError;
// transform from the plane to the camera
private Se3_F64 planeToCamera;
private Se3_F64 cameraToPlane = new Se3_F64();
// code for projection to/from plane
private CameraPlaneProjection planeProjection = new CameraPlaneProjection();
// tracks point features
private PointTracker tracker;
// converts pixels between normalized image coordinates and pixel coordinates
private Point2Transform2_F64 normToPixel;
private Point2Transform2_F64 pixelToNorm;
// tracks which are assumed to be at an infinite distance away
private List tracksFar = new ArrayList<>();
// trans which lie on the ground plane
private List tracksOnPlane = new ArrayList<>();
// storage for normalized image coordinate
private Point2D_F64 n = new Point2D_F64();
// storage for image pixel coordinate
private Point2D_F64 pixel = new Point2D_F64();
// 3D pointing vector of pixel observation
private Vector3D_F64 pointing = new Vector3D_F64();
// Adjusted pointing vector which removes off plane rotation
private Vector3D_F64 pointingAdj = new Vector3D_F64();
// pointing vector on ground in current frame
private Point2D_F64 groundCurr = new Point2D_F64();
// transform from key frame to world frame
private Se2_F64 keyToWorld = new Se2_F64();
// transform from the current camera view to the key frame
private Se2_F64 currToKey = new Se2_F64();
// transform from the current camera view to the world frame
private Se2_F64 currToWorld = new Se2_F64();
// storage for 3D transform
private Se3_F64 currPlaneToWorld3D = new Se3_F64();
private Se3_F64 worldToCurrPlane3D = new Se3_F64();
private Se3_F64 worldToCurrCam3D = new Se3_F64();
// local variable used in concating transforms
private Se2_F64 temp = new Se2_F64();
// angles of rotation computed from points far away
private GrowQueue_F64 farAngles = new GrowQueue_F64();
private GrowQueue_F64 farAnglesCopy = new GrowQueue_F64();
// select angle from points far and the number of points used to estimate it
private double farAngle;
private int farInlierCount; // inlier count for points far away
// found motion for close points. From key-frame to current-frame
private Se2_F64 closeMotionKeyToCurr;
private int closeInlierCount; // inliers count for close points on plane
// number of frames processed. used to decide when tracks should get dropped
private int tick = 0;
// is this the first frame being processed?
private boolean first = true;
/**
* Configures motion estimation.
*
* @param thresholdAdd New points are spawned when the number of on plane inliers drops below this value.
* @param thresholdRetire Tracks are dropped when they are not contained in the inlier set for this many frames
* in a row. Try 2
* @param thresholdPixelError Threshold used to determine inliers. Try 1.5
* @param planeMotion Motion estimator for points on plane
* @param tracker Image feature tracker
*/
public VisOdomMonoPlaneInfinity(int thresholdAdd, int thresholdRetire, double thresholdPixelError,
ModelMatcher planeMotion, PointTracker tracker) {
this.thresholdAdd = thresholdAdd;
this.thresholdRetire = thresholdRetire;
this.thresholdPixelError = thresholdPixelError;
this.planeMotion = planeMotion;
this.tracker = tracker;
}
/**
* Camera the camera's intrinsic parameters. Can be called at any time.
*
* @param intrinsic Intrinsic camera parameters
*/
public void setIntrinsic(CameraPinholeBrown intrinsic) {
planeProjection.setIntrinsic(intrinsic);
normToPixel = LensDistortionFactory.narrow(intrinsic).distort_F64(false,true);
pixelToNorm = LensDistortionFactory.narrow(intrinsic).undistort_F64(true,false);
// Find the change in angle caused by a pixel error in the image center. The same angle error will induce a
// larger change in pixel values towards the outside of the image edge. For fish-eyes lenses this could
// become significant. Not sure what a better way to handle it would be
thresholdFarAngleError = Math.atan2(thresholdPixelError, intrinsic.fx);
}
/**
* Camera the camera's extrinsic parameters. Can be called at any time.
*
* @param planeToCamera Transform from the plane to camera.
*/
public void setExtrinsic(Se3_F64 planeToCamera) {
this.planeToCamera = planeToCamera;
planeToCamera.invert(cameraToPlane);
planeProjection.setPlaneToCamera(planeToCamera, true);
}
/**
* Resets the algorithm into its initial state
*/
public void reset() {
tick = 0;
first = true;
tracker.reset();
keyToWorld.reset();
currToKey.reset();
currToWorld.reset();
worldToCurrCam3D.reset();
}
/**
* Estimates the motion which the camera undergoes relative to the first frame processed.
*
* @param image Most recent camera image.
* @return true if motion was estimated or false if a fault occurred. Should reset after a fault.
*/
public boolean process(T image) {
// update feature tracks
tracker.process(image);
tick++;
if (first) {
// start motion estimation by spawning tracks and estimating their pose
addNewTracks();
first = false;
} else {
// Use updated tracks to update motion estimate and track states
// use geometry to prune tracks and perform pre-processing for motion estimation
sortTracksForEstimation();
// estimate rotation using points at infinity
estimateFar();
// estimate rotation and translation using points on the plane
if (!estimateClose()) {
// if this fails it can't do anything
return false;
}
// merge the two motion estimates together
fuseEstimates();
// discard tracks which aren't being used
dropUnusedTracks();
// see if new tracks need to be added
if (thresholdAdd <= 0 || closeInlierCount < thresholdAdd) {
changeCurrToReference();
addNewTracks();
}
}
return true;
}
/**
* Requests that new tracks are spawned, determines if they are on the plane or not, and computes other required
* data structures.
*/
private void addNewTracks() {
tracker.spawnTracks();
List 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;
}
}
/**
* Removes tracks which have not been included in the inlier set recently
*
* @return Number of dropped tracks
*/
private int dropUnusedTracks() {
List all = tracker.getAllTracks(null);
int num = 0;
for (PointTrack t : all) {
VoTrack p = t.getCookie();
if (tick - p.lastInlier > thresholdRetire) {
tracker.dropTrack(t);
num++;
}
}
return num;
}
/**
* Splits the set of active tracks into on plane and infinity sets. For each set also perform specific sanity
* checks to make sure basic constraints are still being meet. If not then the track will not be considered for
* motion estimation.
*/
private void sortTracksForEstimation() {
// reset data structures
planeSamples.reset();
farAngles.reset();
tracksOnPlane.clear();
tracksFar.clear();
// list of active tracks
List active = tracker.getActiveTracks(null);
for (PointTrack t : active) {
VoTrack p = t.getCookie();
// compute normalized image coordinate
pixelToNorm.compute(t.x, t.y, n);
// rotate pointing vector into plane reference frame
pointing.set(n.x, n.y, 1);
GeometryMath_F64.mult(cameraToPlane.getR(), pointing, pointing);
pointing.normalize();
if (p.onPlane) {
// see if it still intersects the plane
if (pointing.y > 0) {
// create data structure for robust motion estimation
PlanePtPixel ppp = planeSamples.grow();
ppp.normalizedCurr.set(n);
ppp.planeKey.set(p.ground);
tracksOnPlane.add(t);
}
} else {
// if the point is not on the plane visually and (optionally) if it passes a strict y-axis rotation
// test, consider using the point for estimating rotation.
boolean allGood = pointing.y < 0;
if (strictFar) {
allGood = isRotationFromAxisY(t, pointing);
}
// is it still above the ground plane and only has motion consistent with rotation on ground plane axis
if (allGood) {
computeAngleOfRotation(t, pointing);
tracksFar.add(t);
}
}
}
}
/**
* Checks for motion which can't be caused by rotations along the y-axis alone. This is done by adjusting the
* pointing vector in the plane reference frame such that it has the same y component as when the track was spawned
* and that the x-z components are normalized to one, to ensure consistent units. That pointing vector is then
* projected back into the image and a pixel difference computed.
*
* @param pointing Pointing vector of observation in plane reference frame
*/
protected boolean isRotationFromAxisY(PointTrack t, Vector3D_F64 pointing) {
VoTrack p = t.getCookie();
// remove rotations not along x-z plane
double normXZ = Math.sqrt(pointing.x * pointing.x + pointing.z * pointing.z);
pointingAdj.set(pointing.x / normXZ, p.pointingY, pointing.z / normXZ);
// Put pointing vector back into camera frame
GeometryMath_F64.multTran(cameraToPlane.getR(), pointingAdj, pointingAdj);
// compute normalized image coordinates
n.x = pointingAdj.x / pointingAdj.z;
n.y = pointingAdj.y / pointingAdj.z;
// compute pixel of projected point
normToPixel.compute(n.x, n.y, pixel);
// compute error
double error = pixel.distance2(t);
return error < thresholdPixelError * thresholdPixelError;
}
/**
* Computes the angle of rotation between two pointing vectors on the ground plane and adds it to a list.
*
* @param pointingPlane Pointing vector of observation in plane reference frame
*/
private void computeAngleOfRotation(PointTrack t, Vector3D_F64 pointingPlane) {
VoTrack p = t.getCookie();
// Compute ground pointing vector
groundCurr.x = pointingPlane.z;
groundCurr.y = -pointingPlane.x;
double norm = groundCurr.norm();
groundCurr.x /= norm;
groundCurr.y /= norm;
// dot product. vectors are normalized to 1 already
double dot = groundCurr.x * p.ground.x + groundCurr.y * p.ground.y;
// floating point round off error some times knocks it above 1.0
if (dot > 1.0)
dot = 1.0;
double angle = Math.acos(dot);
// cross product to figure out direction
if (groundCurr.x * p.ground.y - groundCurr.y * p.ground.x > 0)
angle = -angle;
farAngles.add(angle);
}
/**
* Estimates only rotation using points at infinity. A robust estimation algorithm is used which finds an angle
* which maximizes the inlier set, like RANSAC does. Unlike RANSAC this will produce an optimal result.
*/
private void estimateFar() {
// do nothing if there are objects at infinity
farInlierCount = 0;
if (farAngles.size == 0)
return;
farAnglesCopy.reset();
farAnglesCopy.addAll(farAngles);
// find angle which maximizes inlier set
farAngle = maximizeCountInSpread(farAnglesCopy.data, farAngles.size, 2 * thresholdFarAngleError);
// mark and count inliers
for (int i = 0; i < tracksFar.size(); i++) {
PointTrack t = tracksFar.get(i);
VoTrack p = t.getCookie();
if (UtilAngle.dist(farAngles.get(i), farAngle) <= thresholdFarAngleError) {
p.lastInlier = tick;
farInlierCount++;
}
}
}
/**
* Estimates the full (x,y,yaw) 2D motion estimate from ground points.
*
* @return true if successful or false if not
*/
private boolean estimateClose() {
// estimate 2D motion
if (!planeMotion.process(planeSamples.toList()))
return false;
// save solutions
closeMotionKeyToCurr = planeMotion.getModelParameters();
closeInlierCount = planeMotion.getMatchSet().size();
// mark inliers as used
for (int i = 0; i < closeInlierCount; i++) {
int index = planeMotion.getInputIndex(i);
VoTrack p = tracksOnPlane.get(index).getCookie();
p.lastInlier = tick;
}
return true;
}
/**
* Fuse the estimates for yaw from both sets of points using a weighted vector average and save the results
* into currToKey
*/
private void fuseEstimates() {
// weighted average for angle
double x = closeMotionKeyToCurr.c * closeInlierCount + Math.cos(farAngle) * farInlierCount;
double y = closeMotionKeyToCurr.s * closeInlierCount + Math.sin(farAngle) * farInlierCount;
// update the motion estimate
closeMotionKeyToCurr.setYaw(Math.atan2(y, x));
// save the results
closeMotionKeyToCurr.invert(currToKey);
}
/**
* Updates the relative position of all points so that the current frame is the reference frame. Mathematically
* this is not needed, but should help keep numbers from getting too large.
*/
private void changeCurrToReference() {
Se2_F64 keyToCurr = currToKey.invert(null);
List all = tracker.getAllTracks(null);
for (PointTrack t : all) {
VoTrack p = t.getCookie();
if (p.onPlane) {
SePointOps_F64.transform(keyToCurr, p.ground, p.ground);
} else {
GeometryMath_F64.rotate(keyToCurr.c, keyToCurr.s, p.ground, p.ground);
}
}
concatMotion();
}
/**
* Number of frames processed
* @return Number of frames processed
*/
public int getTick() {
return tick;
}
/**
* Add motion estimate from key frame into the estimate from world
*/
private void concatMotion() {
currToKey.concat(keyToWorld, temp);
keyToWorld.set(temp);
currToKey.reset();
}
/**
* Returns the 2D motion estimate
* @return Motion estimate from current frame into world frame in 2D
*/
public Se2_F64 getCurrToWorld2D() {
currToKey.concat(keyToWorld, currToWorld);
return currToWorld;
}
/**
* Converts 2D motion estimate into a 3D motion estimate
*
* @return from world to current frame.
*/
public Se3_F64 getWorldToCurr3D() {
// compute transform in 2D space
Se2_F64 currToWorld = getCurrToWorld2D();
// 2D to 3D coordinates
currPlaneToWorld3D.getT().set(-currToWorld.T.y, 0, currToWorld.T.x);
DMatrixRMaj R = currPlaneToWorld3D.getR();
// set rotation around Y axis.
// Transpose the 2D transform since the rotation are pointing in opposite directions
R.unsafe_set(0, 0, currToWorld.c);
R.unsafe_set(0, 2, -currToWorld.s);
R.unsafe_set(1, 1, 1);
R.unsafe_set(2, 0, currToWorld.s);
R.unsafe_set(2, 2, currToWorld.c);
currPlaneToWorld3D.invert(worldToCurrPlane3D);
worldToCurrPlane3D.concat(planeToCamera, worldToCurrCam3D);
return worldToCurrCam3D;
}
/**
* Finds the value which has the largest number of points above and below it within the specified spread
*
* @param data Input data. Is modified by sort
* @param size number of elements in data
* @param maxSpread the spread it's going after
* @return best value
*/
public static double maximizeCountInSpread(double[] data, int size, double maxSpread) {
if (size <= 0)
return 0;
Arrays.sort(data, 0, size);
int length = 0;
for (; length < size; length++) {
double s = UtilAngle.dist(data[0], data[length]);
if (s > maxSpread) {
break;
}
}
int bestStart = 0;
int bestLength = length;
int start;
for (start = 1; start < size && length < size; start++) {
length--;
while (length < size) {
double s = UtilAngle.dist(data[start], data[(start + length) % size]);
if (s > maxSpread) {
break;
} else {
length++;
}
}
if (length > bestLength) {
bestLength = length;
bestStart = start;
}
}
return data[(bestStart + bestLength / 2) % size];
}
public PointTracker getTracker() {
return tracker;
}
public boolean isStrictFar() {
return strictFar;
}
public void setStrictFar(boolean strictFar) {
this.strictFar = strictFar;
}
/**
* Additional track information for use in motion estimation
*/
public static class VoTrack {
// Y-axis of pointing vector in key-frame of plane reference frame. x-z components are normalized to one
// to ensure a constant scale is used
double pointingY;
// ----- Observations in key-frame
// 2D location or angle vector on ground plane for point on ground and at infinity, respectively
public Point2D_F64 ground = new Point2D_F64();
// the tick in which it was last an inlier
public long lastInlier;
// true for point on plane and false for infinity
public boolean onPlane;
}
}