Difference between revisions of "Example Stereo Visual Odometry"

From BoofCV
m
m
(One intermediate revision by the same user not shown)
Line 7: Line 7:
 
Stereo visual odometry estimates the camera's egomotion using a pair of calibrated cameras.  Stereo camera systems are inherently more stable than monocular ones because the stereo pair provides good triangulation of image features and resolves the scale ambiguity.  The example below shows how to use a high level interface with visual odometry algorithms.  The basic high level interface hides internal data structures which are useful in many applications, which is why an optional interface is provided for accessing some of those structures.
 
Stereo visual odometry estimates the camera's egomotion using a pair of calibrated cameras.  Stereo camera systems are inherently more stable than monocular ones because the stereo pair provides good triangulation of image features and resolves the scale ambiguity.  The example below shows how to use a high level interface with visual odometry algorithms.  The basic high level interface hides internal data structures which are useful in many applications, which is why an optional interface is provided for accessing some of those structures.
  
Example File: [https://github.com/lessthanoptimal/BoofCV/blob/v0.32/examples/src/boofcv/examples/sfm/ExampleVisualOdometryStereo.java ExampleVisualOdometryStereo.java]
+
Example File: [https://github.com/lessthanoptimal/BoofCV/blob/v0.36/examples/src/main/java/boofcv/examples/sfm/ExampleVisualOdometryStereo.java ExampleVisualOdometryStereo.java]
  
 
Concepts:
 
Concepts:
Line 46: Line 46:
 
new File(directory,"right.mjpeg").getPath(), ImageType.single(GrayU8.class));
 
new File(directory,"right.mjpeg").getPath(), ImageType.single(GrayU8.class));
  
// specify how the image features are going to be tracked
+
// Specify which tracker and how it will behave
PkltConfig configKlt = new PkltConfig();
+
var configKlt = new ConfigPKlt();
configKlt.pyramidScaling = new int[]{1, 2, 4, 8};
+
configKlt.pyramidLevels = ConfigDiscreteLevels.levels(4);
configKlt.templateRadius = 3;
+
configKlt.templateRadius = 4;
 +
configKlt.toleranceFB = 3;
 +
configKlt.pruneClose = true;
  
PointTrackerTwoPass<GrayU8> tracker =
+
var configDet = new ConfigPointDetector();
FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1),
+
configDet.type = PointDetectorTypes.SHI_TOMASI;
GrayU8.class, GrayS16.class);
+
configDet.shiTomasi.radius = 4;
 +
configDet.general.maxFeatures = 300;
 +
configDet.general.radius = 5;
  
// computes the depth of each point
+
// We will estimate the location of features using block matching stereo
StereoDisparitySparse<GrayU8> disparity =
+
var configBM = new ConfigDisparityBM();
FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, GrayU8.class);
+
configBM.errorType = DisparityError.CENSUS;
 +
configBM.disparityMin = 0;
 +
configBM.disparityRange = 50;
 +
configBM.regionRadiusX = 3;
 +
configBM.regionRadiusY = 3;
 +
configBM.maxPerPixelError = 30;
 +
configBM.texture = 0.05;
 +
configBM.validateRtoL = 1;
 +
configBM.subpixel = true;
  
// declares the algorithm
+
// Configurations related to how the structure is chained together frame to frame
StereoVisualOdometry<GrayU8> visualOdometry = FactoryVisualOdometry.stereoDepth(1.5,120, 2,200,50,true,
+
var configVisOdom = new ConfigVisOdomTrackPnP();
disparity, tracker, GrayU8.class);
+
configVisOdom.keyframes.geoMinCoverage = 0.4;
 +
configVisOdom.ransac.iterations = 200;
 +
configVisOdom.ransac.inlierThreshold = 1.0;
 +
 
 +
// Declare each component then visual odometry
 +
PointTracker<GrayU8> tracker = FactoryPointTracker.klt(configKlt, configDet,GrayU8.class, GrayS16.class);
 +
StereoDisparitySparse<GrayU8> disparity = FactoryStereoDisparity.sparseRectifiedBM(configBM, GrayU8.class);
 +
StereoVisualOdometry<GrayU8> visodom = FactoryVisualOdometry.stereoMonoPnP(configVisOdom,disparity,tracker, GrayU8.class);
 +
 
 +
// Optionally dump verbose debugging information to stdout
 +
// Set<String> configuration = new HashSet<>();
 +
// configuration.add(VisualOdometry.VERBOSE_RUNTIME);
 +
// configuration.add(VisualOdometry.VERBOSE_TRACKING);
 +
// visodom.setVerbose(System.out,configuration);
  
 
// Pass in intrinsic/extrinsic calibration.  This can be changed in the future.
 
// Pass in intrinsic/extrinsic calibration.  This can be changed in the future.
visualOdometry.setCalibration(stereoParam);
+
visodom.setCalibration(stereoParam);
  
 
// Process the video sequence and output the location plus number of inliers
 
// Process the video sequence and output the location plus number of inliers
 +
long startTime = System.nanoTime();
 
while( video1.hasNext() ) {
 
while( video1.hasNext() ) {
 
GrayU8 left = video1.next();
 
GrayU8 left = video1.next();
 
GrayU8 right = video2.next();
 
GrayU8 right = video2.next();
  
if( !visualOdometry.process(left,right) ) {
+
if( !visodom.process(left,right) ) {
 
throw new RuntimeException("VO Failed!");
 
throw new RuntimeException("VO Failed!");
 
}
 
}
  
Se3_F64 leftToWorld = visualOdometry.getCameraToWorld();
+
Se3_F64 leftToWorld = visodom.getCameraToWorld();
 
Vector3D_F64 T = leftToWorld.getT();
 
Vector3D_F64 T = leftToWorld.getT();
  
System.out.printf("Location %8.2f %8.2f %8.2f     inliers %s\n", T.x, T.y, T.z, inlierPercent(visualOdometry));
+
System.out.printf("Location %8.2f %8.2f %8.2f   %s\n", T.x, T.y, T.z, trackStats(visodom));
 
}
 
}
 +
System.out.printf("FPS %4.2f\n", video1.getFrameNumber()/((System.nanoTime()-startTime)*1e-9));
 
}
 
}
  
 
/**
 
/**
* If the algorithm implements AccessPointTracks3D, then count the number of inlier features
+
* If the algorithm implements AccessPointTracks3D create a string which summarizing different tracking information
* and return a string.
 
 
*/
 
*/
public static String inlierPercent(StereoVisualOdometry alg) {
+
public static String trackStats(StereoVisualOdometry alg) {
 
if( !(alg instanceof AccessPointTracks3D))
 
if( !(alg instanceof AccessPointTracks3D))
 
return "";
 
return "";
Line 93: Line 119:
  
 
int count = 0;
 
int count = 0;
int N = access.getAllTracks().size();
+
int N = access.getTotalTracks();
 +
int totalNew = 0;
 
for( int i = 0; i < N; i++ ) {
 
for( int i = 0; i < N; i++ ) {
if( access.isInlier(i) )
+
if( access.isTrackInlier(i) )
 
count++;
 
count++;
 +
else if( access.isTrackNew(i) ) {
 +
totalNew++;
 +
}
 
}
 
}
  
return String.format("%%%5.3f", 100.0 * count / N);
+
return String.format("inlier %5.1f%% new %4d total %d", 100.0 * count / (N-totalNew),totalNew,N);
 
}
 
}
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>

Revision as of 22:30, 17 May 2020

Stereo visual odometry estimates the camera's egomotion using a pair of calibrated cameras. Stereo camera systems are inherently more stable than monocular ones because the stereo pair provides good triangulation of image features and resolves the scale ambiguity. The example below shows how to use a high level interface with visual odometry algorithms. The basic high level interface hides internal data structures which are useful in many applications, which is why an optional interface is provided for accessing some of those structures.

Example File: ExampleVisualOdometryStereo.java

Concepts:

  • Structure from Motion
  • Geometric Vision
  • Feature Tracking

Relevant Videos:

Related Tutorials/Example Code:

Example Code

/**
 * Bare bones example showing how to estimate the camera's ego-motion using a stereo camera system. Additional
 * information on the scene can be optionally extracted from the algorithm if it implements AccessPointTracks3D.
 *
 * @author Peter Abeles
 */
public class ExampleVisualOdometryStereo {

	public static void main( String args[] ) {

		MediaManager media = DefaultMediaManager.INSTANCE;

		String directory = UtilIO.pathExample("vo/backyard/");

		// load camera description and the video sequence
		StereoParameters stereoParam = CalibrationIO.load(media.openFile(new File(directory , "stereo.yaml").getPath()));
		SimpleImageSequence<GrayU8> video1 = media.openVideo(
				new File(directory , "left.mjpeg").getPath(), ImageType.single(GrayU8.class));
		SimpleImageSequence<GrayU8> video2 = media.openVideo(
				new File(directory,"right.mjpeg").getPath(), ImageType.single(GrayU8.class));

		// Specify which tracker and how it will behave
		var configKlt = new ConfigPKlt();
		configKlt.pyramidLevels = ConfigDiscreteLevels.levels(4);
		configKlt.templateRadius = 4;
		configKlt.toleranceFB = 3;
		configKlt.pruneClose = true;

		var configDet = new ConfigPointDetector();
		configDet.type = PointDetectorTypes.SHI_TOMASI;
		configDet.shiTomasi.radius = 4;
		configDet.general.maxFeatures = 300;
		configDet.general.radius = 5;

		// We will estimate the location of features using block matching stereo
		var configBM = new ConfigDisparityBM();
		configBM.errorType = DisparityError.CENSUS;
		configBM.disparityMin = 0;
		configBM.disparityRange = 50;
		configBM.regionRadiusX = 3;
		configBM.regionRadiusY = 3;
		configBM.maxPerPixelError = 30;
		configBM.texture = 0.05;
		configBM.validateRtoL = 1;
		configBM.subpixel = true;

		// Configurations related to how the structure is chained together frame to frame
		var configVisOdom = new ConfigVisOdomTrackPnP();
		configVisOdom.keyframes.geoMinCoverage = 0.4;
		configVisOdom.ransac.iterations = 200;
		configVisOdom.ransac.inlierThreshold = 1.0;

		// Declare each component then visual odometry
		PointTracker<GrayU8> tracker = FactoryPointTracker.klt(configKlt, configDet,GrayU8.class, GrayS16.class);
		StereoDisparitySparse<GrayU8> disparity = FactoryStereoDisparity.sparseRectifiedBM(configBM, GrayU8.class);
		StereoVisualOdometry<GrayU8> visodom = FactoryVisualOdometry.stereoMonoPnP(configVisOdom,disparity,tracker, GrayU8.class);

		// Optionally dump verbose debugging information to stdout
//		Set<String> configuration = new HashSet<>();
//		configuration.add(VisualOdometry.VERBOSE_RUNTIME);
//		configuration.add(VisualOdometry.VERBOSE_TRACKING);
//		visodom.setVerbose(System.out,configuration);

		// Pass in intrinsic/extrinsic calibration.  This can be changed in the future.
		visodom.setCalibration(stereoParam);

		// Process the video sequence and output the location plus number of inliers
		long startTime = System.nanoTime();
		while( video1.hasNext() ) {
			GrayU8 left = video1.next();
			GrayU8 right = video2.next();

			if( !visodom.process(left,right) ) {
				throw new RuntimeException("VO Failed!");
			}

			Se3_F64 leftToWorld = visodom.getCameraToWorld();
			Vector3D_F64 T = leftToWorld.getT();

			System.out.printf("Location %8.2f %8.2f %8.2f    %s\n", T.x, T.y, T.z, trackStats(visodom));
		}
		System.out.printf("FPS %4.2f\n", video1.getFrameNumber()/((System.nanoTime()-startTime)*1e-9));
	}

	/**
	 * If the algorithm implements AccessPointTracks3D create a string which summarizing different tracking information
	 */
	public static String trackStats(StereoVisualOdometry alg) {
		if( !(alg instanceof AccessPointTracks3D))
			return "";

		AccessPointTracks3D access = (AccessPointTracks3D)alg;

		int count = 0;
		int N = access.getTotalTracks();
		int totalNew = 0;
		for( int i = 0; i < N; i++ ) {
			if( access.isTrackInlier(i) )
				count++;
			else if( access.isTrackNew(i) ) {
				totalNew++;
			}
		}

		return String.format("inlier %5.1f%% new %4d total %d", 100.0 * count / (N-totalNew),totalNew,N);
	}
}