Difference between revisions of "Example Stereo Single Camera"

In this scenario we wish to compute a dense point cloud from two views taken with the same calibrated camera. Because the intrinsic parameters are known, it is easier to converge towards a valid solution than the entirely uncalibrated scenario.

Because the extrinsic parameters (translation and rotation) between the two views is unknown the scene's structure can only be recovered up to a scale factor. In this example natural features are used to determine the geometric relationship between the two views. The algorithm can be summarized as follows:

• Load camera calibration and two images
• Detect, describe, and associate image features
• Compute camera motion (Essential matrix)
• Rectify image pair
• Compute dense stereo disparity
• Convert into 3D point cloud

Example File:

Concepts:

• Point feature association
• Epipolar geometry
• Rectification
• Dense stereo processing

Related Tutorials/Example Code:

Example Code

```/**
* Example demonstrating how to use to images taken from a single calibrated camera to create a stereo disparity image,
* from which a dense 3D point cloud of the scene can be computed.  For this technique to work the camera's motion
* needs to be approximately tangential to the direction the camera is pointing.  The code below assumes that the first
* image is to the left of the second image.
*
* @author Peter Abeles
*/
public class ExampleStereoTwoViewsOneCamera {

// Disparity calculation parameters
private static final int minDisparity = 15;
private static final int rangeDisparity = 85;

public static void main(String args[]) {
// specify location of images and calibration
String calibDir = UtilIO.pathExample("calibration/mono/Sony_DSC-HX5V_Chess/");
String imageDir = UtilIO.pathExample("stereo/");

// Camera parameters
CameraPinholeBrown intrinsic = CalibrationIO.load(new File(calibDir , "intrinsic.yaml"));

// Input images from the camera moving left to right
BufferedImage origLeft = UtilImageIO.loadImage(imageDir , "mono_wall_01.jpg");
BufferedImage origRight = UtilImageIO.loadImage(imageDir, "mono_wall_02.jpg");

// Input images with lens distortion
GrayU8 distortedLeft = ConvertBufferedImage.convertFrom(origLeft, (GrayU8) null);
GrayU8 distortedRight = ConvertBufferedImage.convertFrom(origRight, (GrayU8) null);

// matched features between the two images
List<AssociatedPair> matchedFeatures = ExampleFundamentalMatrix.computeMatches(origLeft, origRight);

// convert from pixel coordinates into normalized image coordinates
List<AssociatedPair> matchedCalibrated = convertToNormalizedCoordinates(matchedFeatures, intrinsic);

// Robustly estimate camera motion
List<AssociatedPair> inliers = new ArrayList<>();
Se3_F64 leftToRight = estimateCameraMotion(intrinsic, matchedCalibrated, inliers);

drawInliers(origLeft, origRight, intrinsic, inliers);

// Rectify and remove lens distortion for stereo processing
DMatrixRMaj rectifiedK = new DMatrixRMaj(3, 3);
DMatrixRMaj rectifiedR = new DMatrixRMaj(3, 3);
GrayU8 rectifiedLeft = distortedLeft.createSameShape();
GrayU8 rectifiedRight = distortedRight.createSameShape();
GrayU8 rectifiedMask = distortedLeft.createSameShape();

rectifyImages(distortedLeft, distortedRight, leftToRight, intrinsic,intrinsic,

// compute disparity
ConfigDisparityBMBest5 config = new ConfigDisparityBMBest5();
config.disparityMin = minDisparity;
config.disparityRange = rangeDisparity;
config.subpixel = true;
config.maxPerPixelError = 20;
config.validateRtoL = 1;
config.texture = 0.1;
StereoDisparity<GrayS16, GrayF32> disparityAlg =
FactoryStereoDisparity.blockMatchBest5(config, GrayS16.class, GrayF32.class);

// Apply the Laplacian across the image to add extra resistance to changes in lighting or camera gain
GrayS16 derivLeft = new GrayS16(rectifiedLeft.width,rectifiedLeft.height);
GrayS16 derivRight = new GrayS16(rectifiedLeft.width,rectifiedLeft.height);
GImageDerivativeOps.laplace(rectifiedLeft, derivLeft,BorderType.EXTENDED);
GImageDerivativeOps.laplace(rectifiedRight,derivRight,BorderType.EXTENDED);

// process and return the results
disparityAlg.process(derivLeft, derivRight);
GrayF32 disparity = disparityAlg.getDisparity();

// show results
BufferedImage visualized = VisualizeImageData.disparity(disparity, null, rangeDisparity, 0);

BufferedImage outLeft = ConvertBufferedImage.convertTo(rectifiedLeft, null);
BufferedImage outRight = ConvertBufferedImage.convertTo(rectifiedRight, null);

ShowImages.showWindow(new RectifiedPairPanel(true, outLeft, outRight), "Rectification",true);
ShowImages.showWindow(visualized, "Disparity",true);

showPointCloud(disparity, outLeft, leftToRight, rectifiedK,rectifiedR, minDisparity, rangeDisparity);

System.out.println("Total found " + matchedCalibrated.size());
System.out.println("Total Inliers " + inliers.size());
}

/**
* Estimates the camera motion robustly using RANSAC and a set of associated points.
*
* @param intrinsic   Intrinsic camera parameters
* @param matchedNorm set of matched point features in normalized image coordinates
* @param inliers     OUTPUT: Set of inlier features from RANSAC
* @return Found camera motion.  Note translation has an arbitrary scale
*/
public static Se3_F64 estimateCameraMotion(CameraPinholeBrown intrinsic,
List<AssociatedPair> matchedNorm, List<AssociatedPair> inliers)
{
ModelMatcherMultiview<Se3_F64, AssociatedPair> epipolarMotion =
FactoryMultiViewRobust.baselineRansac(new ConfigEssential(),new ConfigRansac(200,0.5));
epipolarMotion.setIntrinsic(0,intrinsic);
epipolarMotion.setIntrinsic(1,intrinsic);

if (!epipolarMotion.process(matchedNorm))
throw new RuntimeException("Motion estimation failed");

// save inlier set for debugging purposes

return epipolarMotion.getModelParameters();
}

/**
* Convert a set of associated point features from pixel coordinates into normalized image coordinates.
*/
public static List<AssociatedPair> convertToNormalizedCoordinates(List<AssociatedPair> matchedFeatures, CameraPinholeBrown intrinsic) {

Point2Transform2_F64 p_to_n = LensDistortionFactory.narrow(intrinsic).undistort_F64(true, false);

List<AssociatedPair> calibratedFeatures = new ArrayList<>();

for (AssociatedPair p : matchedFeatures) {
AssociatedPair c = new AssociatedPair();

p_to_n.compute(p.p1.x, p.p1.y, c.p1);
p_to_n.compute(p.p2.x, p.p2.y, c.p2);

}

return calibratedFeatures;
}

/**
* Remove lens distortion and rectify stereo images
*
* @param distortedLeft  Input distorted image from left camera.
* @param distortedRight Input distorted image from right camera.
* @param leftToRight    Camera motion from left to right
* @param intrinsicLeft  Intrinsic camera parameters
* @param rectifiedLeft  Output rectified image for left camera.
* @param rectifiedRight Output rectified image for right camera.
* @param rectifiedMask  Mask that indicates invalid pixels in rectified image. 1 = valid, 0 = invalid
* @param rectifiedK     Output camera calibration matrix for rectified camera
*/
public static <T extends ImageBase<T>>
void rectifyImages(T distortedLeft,
T distortedRight,
Se3_F64 leftToRight,
CameraPinholeBrown intrinsicLeft,
CameraPinholeBrown intrinsicRight,
T rectifiedLeft,
T rectifiedRight,
DMatrixRMaj rectifiedK,
DMatrixRMaj rectifiedR) {
RectifyCalibrated rectifyAlg = RectifyImageOps.createCalibrated();

// original camera calibration matrices
DMatrixRMaj K1 = PerspectiveOps.pinholeToMatrix(intrinsicLeft, (DMatrixRMaj)null);
DMatrixRMaj K2 = PerspectiveOps.pinholeToMatrix(intrinsicRight, (DMatrixRMaj)null);

rectifyAlg.process(K1, new Se3_F64(), K2, leftToRight);

// rectification matrix for each image
DMatrixRMaj rectRot = rectifyAlg.getRectifiedRotation();
DMatrixRMaj rect1 = rectifyAlg.getRect1();
DMatrixRMaj rect2 = rectifyAlg.getRect2();
rectifiedR.set(rectifyAlg.getRectifiedRotation());

// New calibration matrix,
rectifiedK.set(rectifyAlg.getCalibrationMatrix());

// Adjust the rectification to make the view area more useful
ImageDimension rectShape = new ImageDimension();
RectifyImageOps.fullViewLeft(intrinsicLeft, rectRot, rect1, rect2, rectifiedK, rectShape);
//		RectifyImageOps.allInsideLeft(intrinsicLeft, rectR, rect1, rect2, rectifiedK, rectShape);
// Taking in account the relative rotation between the image axis and the baseline is important in
// this scenario since a person can easily hold the camera at an odd angle. If you don't adjust
// the rectified image size you might end up with a lot of wasted pixels and a low resolution model!
rectifiedLeft.reshape(rectShape.width,rectShape.height);
rectifiedRight.reshape(rectShape.width,rectShape.height);

// undistorted and rectify images
FMatrixRMaj rect1_F32 = new FMatrixRMaj(3,3);
FMatrixRMaj rect2_F32 = new FMatrixRMaj(3,3);
ConvertMatrixData.convert(rect1, rect1_F32);
ConvertMatrixData.convert(rect2, rect2_F32);

// Extending the image prevents a harsh edge reducing false matches at the image border
// SKIP is another option, possibly a tinny bit faster, but has a harsh edge which will need to be filtered
ImageDistort<T,T> distortLeft =
RectifyImageOps.rectifyImage(intrinsicLeft, rect1_F32, BorderType.EXTENDED, distortedLeft.getImageType());
ImageDistort<T,T> distortRight =
RectifyImageOps.rectifyImage(intrinsicRight, rect2_F32, BorderType.EXTENDED, distortedRight.getImageType());

distortRight.apply(distortedRight, rectifiedRight);
}

/**
* Draw inliers for debugging purposes.  Need to convert from normalized to pixel coordinates.
*/
public static void drawInliers(BufferedImage left, BufferedImage right, CameraPinholeBrown intrinsic,
List<AssociatedPair> normalized) {
Point2Transform2_F64 n_to_p = LensDistortionFactory.narrow(intrinsic).distort_F64(false,true);

List<AssociatedPair> pixels = new ArrayList<>();

for (AssociatedPair n : normalized) {
AssociatedPair p = new AssociatedPair();

n_to_p.compute(n.p1.x, n.p1.y, p.p1);
n_to_p.compute(n.p2.x, n.p2.y, p.p2);

}

// display the results
AssociationPanel panel = new AssociationPanel(20);
panel.setAssociation(pixels);
panel.setImages(left, right);

ShowImages.showWindow(panel, "Inlier Features", true);
}

/**
* Show results as a point cloud
*/
public static void showPointCloud(ImageGray disparity, BufferedImage left,
Se3_F64 motion, DMatrixRMaj rectifiedK , DMatrixRMaj rectifiedR,
int minDisparity, int rangeDisparity)
{
DisparityToColorPointCloud d2c = new DisparityToColorPointCloud();
double baseline = motion.getT().norm();
d2c.configure(baseline, rectifiedK, rectifiedR, new DoNothing2Transform2_F64(), minDisparity, minDisparity+rangeDisparity);
d2c.process(disparity,left);

CameraPinhole rectifiedPinhole = PerspectiveOps.matrixToPinhole(rectifiedK,disparity.width,disparity.height,null);

// skew the view to make the structure easier to see
Se3_F64 cameraToWorld = SpecialEuclideanOps_F64.eulerXyz(-baseline*5,0,0,0,0.2,0,null);

PointCloudViewer pcv = VisualizeData.createPointCloudViewer();
pcv.setCameraHFov(PerspectiveOps.computeHFov(rectifiedPinhole));
pcv.setCameraToWorld(cameraToWorld);
pcv.setTranslationStep(baseline/3);