Example Visual Odometry Depth
From BoofCV
Jump to navigationJump to searchThe printable version is no longer supported and may have rendering errors. Please update your browser bookmarks and please use the default browser print function instead.
This example demonstrates how to estimate the camera's ego motion using an RGB-D sensor, such as the Kinect, which contains visual and depth information.
Example Code:
Concepts:
- RGB-D
Relevant Videos:
Related Examples:
Example Code
/**
* Bare bones example showing how to estimate the camera's ego-motion using a depth camera system, e.g. Kinect.
* Additional information on the scene can be optionally extracted from the algorithm if it implements AccessPointTracks3D.
*
* @author Peter Abeles
*/
public class ExampleVisualOdometryDepth {
public static void main( String[] args ) {
MediaManager media = DefaultMediaManager.INSTANCE;
String directory = UtilIO.pathExample("kinect/straight");
// load camera description and the video sequence
VisualDepthParameters param = CalibrationIO.load(
media.openFile(new File(directory, "visualdepth.yaml").getPath()));
// specify how the image features are going to be tracked
ConfigRgbDepthTrackPnP config = new ConfigRgbDepthTrackPnP();
config.depthScale = 1e-3; // convert depth image distance units to meters
config.tracker.typeTracker = ConfigPointTracker.TrackerType.KLT;
config.tracker.detDesc.detectPoint.type = PointDetectorTypes.SHI_TOMASI;
config.tracker.detDesc.detectPoint.shiTomasi.radius = 3;
config.tracker.detDesc.detectPoint.general.maxFeatures = 600;
config.tracker.detDesc.detectPoint.general.radius = 3;
config.tracker.detDesc.detectPoint.general.threshold = 1;
// declares the algorithm
DepthVisualOdometry<GrayU8, GrayU16> visualOdometry =
FactoryVisualOdometry.rgbDepthPnP(config, GrayU8.class, GrayU16.class);
// Pass in intrinsic/extrinsic calibration. This can be changed in the future.
visualOdometry.setCalibration(param.visualParam, new DoNothing2Transform2_F32());
// Process the video sequence and output the location plus number of inliers
SimpleImageSequence<GrayU8> videoVisual = media.openVideo(
new File(directory, "rgb.mjpeg").getPath(), ImageType.single(GrayU8.class));
SimpleImageSequence<GrayU16> videoDepth = media.openVideo(
new File(directory, "depth.mpng").getPath(), ImageType.single(GrayU16.class));
long startTime = System.nanoTime();
while (videoVisual.hasNext()) {
GrayU8 visual = videoVisual.next();
GrayU16 depth = videoDepth.next();
if (!visualOdometry.process(visual, depth)) {
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, %s\n", T.x, T.y, T.z, trackStats(visualOdometry));
}
System.out.printf("FPS %4.2f\n", videoVisual.getFrameNumber()/((System.nanoTime() - startTime)*1e-9));
}
}