Difference between revisions of "Example Stereo Visual Odometry"

From BoofCV
Jump to navigationJump to search
m
m
Line 7: Line 7:
Stereo visual odometry estimates the camera's egomotion using a pair of calibrated cameras.  Stereo camera systems are inheritly 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 odometyr 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 inheritly 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 odometyr 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.20/examples/src/boofcv/examples/sfm/ExampleVisualOdometryStereo.java ExampleVisualOdometryStereo.java]
Example File: [https://github.com/lessthanoptimal/BoofCV/blob/v0.23/examples/src/boofcv/examples/sfm/ExampleVisualOdometryStereo.java ExampleVisualOdometryStereo.java]


Concepts:
Concepts:
Line 43: Line 43:
// load camera description and the video sequence
// load camera description and the video sequence
StereoParameters stereoParam = UtilIO.loadXML(media.openFile(directory + "stereo.xml"));
StereoParameters stereoParam = UtilIO.loadXML(media.openFile(directory + "stereo.xml"));
SimpleImageSequence<ImageUInt8> video1 = media.openVideo(directory + "left.mjpeg", ImageType.single(ImageUInt8.class));
SimpleImageSequence<GrayU8> video1 = media.openVideo(directory + "left.mjpeg", ImageType.single(GrayU8.class));
SimpleImageSequence<ImageUInt8> video2 = media.openVideo(directory+"right.mjpeg", ImageType.single(ImageUInt8.class));
SimpleImageSequence<GrayU8> video2 = media.openVideo(directory+"right.mjpeg", ImageType.single(GrayU8.class));


// specify how the image features are going to be tracked
// specify how the image features are going to be tracked
Line 51: Line 51:
configKlt.templateRadius = 3;
configKlt.templateRadius = 3;


PointTrackerTwoPass<ImageUInt8> tracker =
PointTrackerTwoPass<GrayU8> tracker =
FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1),
FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1),
ImageUInt8.class, ImageSInt16.class);
GrayU8.class, GrayS16.class);


// computes the depth of each point
// computes the depth of each point
StereoDisparitySparse<ImageUInt8> disparity =
StereoDisparitySparse<GrayU8> disparity =
FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, ImageUInt8.class);
FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, GrayU8.class);


// declares the algorithm
// declares the algorithm
StereoVisualOdometry<ImageUInt8> visualOdometry = FactoryVisualOdometry.stereoDepth(1.5,120, 2,200,50,true,
StereoVisualOdometry<GrayU8> visualOdometry = FactoryVisualOdometry.stereoDepth(1.5,120, 2,200,50,true,
disparity, tracker, ImageUInt8.class);
disparity, tracker, GrayU8.class);


// Pass in intrinsic/extrinsic calibration.  This can be changed in the future.
// Pass in intrinsic/extrinsic calibration.  This can be changed in the future.
Line 68: Line 68:
// Process the video sequence and output the location plus number of inliers
// Process the video sequence and output the location plus number of inliers
while( video1.hasNext() ) {
while( video1.hasNext() ) {
ImageUInt8 left = video1.next();
GrayU8 left = video1.next();
ImageUInt8 right = video2.next();
GrayU8 right = video2.next();


if( !visualOdometry.process(left,right) ) {
if( !visualOdometry.process(left,right) ) {

Revision as of 05:39, 28 March 2016

Stereo visual odometry estimates the camera's egomotion using a pair of calibrated cameras. Stereo camera systems are inheritly 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 odometyr 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 Applets:

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 = UtilIO.loadXML(media.openFile(directory + "stereo.xml"));
		SimpleImageSequence<GrayU8> video1 = media.openVideo(directory + "left.mjpeg", ImageType.single(GrayU8.class));
		SimpleImageSequence<GrayU8> video2 = media.openVideo(directory+"right.mjpeg", ImageType.single(GrayU8.class));

		// specify how the image features are going to be tracked
		PkltConfig configKlt = new PkltConfig();
		configKlt.pyramidScaling = new int[]{1, 2, 4, 8};
		configKlt.templateRadius = 3;

		PointTrackerTwoPass<GrayU8> tracker =
				FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1),
						GrayU8.class, GrayS16.class);

		// computes the depth of each point
		StereoDisparitySparse<GrayU8> disparity =
				FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, GrayU8.class);

		// declares the algorithm
		StereoVisualOdometry<GrayU8> visualOdometry = FactoryVisualOdometry.stereoDepth(1.5,120, 2,200,50,true,
				disparity, tracker, GrayU8.class);

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

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

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

			Se3_F64 leftToWorld = visualOdometry.getCameraToWorld();
			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));
		}
	}

	/**
	 * If the algorithm implements AccessPointTracks3D, then count the number of inlier features
	 * and return a string.
	 */
	public static String inlierPercent(StereoVisualOdometry alg) {
		if( !(alg instanceof AccessPointTracks3D))
			return "";

		AccessPointTracks3D access = (AccessPointTracks3D)alg;

		int count = 0;
		int N = access.getAllTracks().size();
		for( int i = 0; i < N; i++ ) {
			if( access.isInlier(i) )
				count++;
		}

		return String.format("%%%5.3f", 100.0 * count / N);
	}
}