Difference between revisions of "Example Stereo Visual Odometry"
From BoofCV
Jump to navigationJump to searchm |
m |
||
(9 intermediate revisions by the same user not shown) | |||
Line 1: | Line 1: | ||
<center> | <center> | ||
<gallery widths=758px heights=384px > | <gallery widths=758px heights=384px > | ||
File:Stereo_visual_odometry_screenshot.jpg|Screenshot from visual odometry | File:Stereo_visual_odometry_screenshot.jpg|Screenshot from visual odometry demonstration. Example just outputs estimated location. | ||
</gallery> | </gallery> | ||
</center> | </center> | ||
Stereo visual odometry estimates the camera's egomotion using a pair of calibrated cameras. Stereo camera systems are | 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. | Example File: [https://github.com/lessthanoptimal/BoofCV/blob/v0.32/examples/src/boofcv/examples/sfm/ExampleVisualOdometryStereo.java ExampleVisualOdometryStereo.java] | ||
Concepts: | Concepts: | ||
Line 14: | Line 14: | ||
* Feature Tracking | * Feature Tracking | ||
Relevant | Relevant Videos: | ||
* [http://www.youtube.com/watch?v=D4I4NHSGaOc Youtube] | |||
Related Tutorials/Example Code: | Related Tutorials/Example Code: | ||
* [[Tutorial_Geometric_Vision| Tutorial Geometric Vision]] | * [[Tutorial_Geometric_Vision| Tutorial Geometric Vision]] | ||
* [[Example Visual Odometry Monocular Plane| Visual Odometry Monocular Plane]] | |||
* [[Example Visual Odometry Depth| Visual Odometry Depth]] | |||
= Example Code = | |||
<syntaxhighlight lang="java"> | <syntaxhighlight lang="java"> | ||
Line 29: | Line 31: | ||
* @author Peter Abeles | * @author Peter Abeles | ||
*/ | */ | ||
public class | public class ExampleVisualOdometryStereo { | ||
public static void main( String args[] ) { | public static void main( String args[] ) { | ||
Line 35: | Line 37: | ||
MediaManager media = DefaultMediaManager.INSTANCE; | MediaManager media = DefaultMediaManager.INSTANCE; | ||
String directory = " | String directory = UtilIO.pathExample("vo/backyard/"); | ||
// load camera description and the video sequence | // load camera description and the video sequence | ||
StereoParameters stereoParam = | StereoParameters stereoParam = CalibrationIO.load(media.openFile(new File(directory , "stereo.yaml").getPath())); | ||
SimpleImageSequence< | SimpleImageSequence<GrayU8> video1 = media.openVideo( | ||
SimpleImageSequence< | 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 how the image features are going to be tracked | // specify how the image features are going to be tracked | ||
PkltConfig | PkltConfig configKlt = new PkltConfig(); | ||
configKlt.pyramidScaling = new int[]{1, 2, 4, 8}; | configKlt.pyramidScaling = new int[]{1, 2, 4, 8}; | ||
configKlt. | configKlt.templateRadius = 3; | ||
PointTrackerTwoPass< | PointTrackerTwoPass<GrayU8> tracker = | ||
FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1)); | FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1), | ||
GrayU8.class, GrayS16.class); | |||
// computes the depth of each point | // computes the depth of each point | ||
StereoDisparitySparse< | StereoDisparitySparse<GrayU8> disparity = | ||
FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, | FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, GrayU8.class); | ||
// declares the algorithm | // declares the algorithm | ||
StereoVisualOdometry< | StereoVisualOdometry<GrayU8> visualOdometry = FactoryVisualOdometry.stereoDepth(1.5,120, 2,200,50,true, | ||
disparity, tracker, | 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 63: | 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() ) { | ||
GrayU8 left = video1.next(); | |||
GrayU8 right = video2.next(); | |||
if( !visualOdometry.process(left,right) ) { | if( !visualOdometry.process(left,right) ) { | ||
Line 70: | Line 75: | ||
} | } | ||
Se3_F64 leftToWorld = visualOdometry. | Se3_F64 leftToWorld = visualOdometry.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, | System.out.printf("Location %8.2f %8.2f %8.2f inliers %s\n", T.x, T.y, T.z, inlierPercent(visualOdometry)); | ||
} | } | ||
} | } | ||
Line 81: | Line 86: | ||
* and return a string. | * and return a string. | ||
*/ | */ | ||
public static String | public static String inlierPercent(StereoVisualOdometry alg) { | ||
if( !(alg instanceof AccessPointTracks3D)) | if( !(alg instanceof AccessPointTracks3D)) | ||
return ""; | return ""; | ||
Line 94: | Line 99: | ||
} | } | ||
return | return String.format("%%%5.3f", 100.0 * count / N); | ||
} | } | ||
} | } | ||
</syntaxhighlight> | </syntaxhighlight> |
Revision as of 21:04, 26 December 2018
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 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);
}
}