stereoRectify.cpp

Go to the documentation of this file.

#include <dlrComputerVision/stereoRectify.h>
#include <dlrLinearAlgebra/linearAlgebra.h>
#include <dlrNumeric/utilities.h>

namespace linalg = dlr::linearAlgebra;
namespace num = dlr::numeric;

// Anonymous namespace for local functions.
namespace {

  namespace cv = dlr::computerVision;
  
  num::Array2D<double>
  extractUpperLeftBlock(cv::CameraIntrinsicsPinhole const& intrinsics,
                        num::Transform3D const& cameraTworld)
  {
    num::Array2D<double> result(3, 3);
#if 0
    result(0, 0) = (intrinsics.getKx() * cameraTworld(0, 0)
                    + intrinsics.getCenterU() * cameraTworld(2, 0));
    result(0, 1) = (intrinsics.getKx() * cameraTworld(0, 1)
                    + intrinsics.getCenterU() * cameraTworld(2, 1));
    result(0, 2) = (intrinsics.getKx() * cameraTworld(0, 2)
                    + intrinsics.getCenterU() * cameraTworld(2, 2));
    result(1, 0) = (intrinsics.getKy() * cameraTworld(1, 0)
                    + intrinsics.getCenterV() * cameraTworld(2, 0));
    result(1, 1) = (intrinsics.getKy() * cameraTworld(1, 1)
                    + intrinsics.getCenterV() * cameraTworld(2, 1));
    result(1, 2) = (intrinsics.getKy() * cameraTworld(1, 2)
                    + intrinsics.getCenterV() * cameraTworld(2, 2));
    result(2, 0) = cameraTworld(2, 0);
    result(2, 1) = cameraTworld(2, 1);
    result(2, 2) = cameraTworld(2, 2);
#else
    result(0, 0) = (intrinsics.getKx() * cameraTworld(0, 0)
                    + intrinsics.getCenterU() * cameraTworld(2, 0));
    result(0, 1) = (intrinsics.getKx() * cameraTworld(0, 1)
                    + intrinsics.getCenterU() * cameraTworld(2, 1));
    result(0, 2) = (intrinsics.getKx() * cameraTworld(0, 2)
                    + intrinsics.getCenterU() * cameraTworld(2, 2));

    double ky = intrinsics.getKy();
    double c10 =  cameraTworld(1, 0);
    double dummy = ky * c10;
    dummy += intrinsics.getCenterV() * cameraTworld(2, 0);
    result(1, 0) = dummy;
    result(1, 1) = (intrinsics.getKy() * cameraTworld(1, 1)
                    + intrinsics.getCenterV() * cameraTworld(2, 1));
    result(1, 2) = (intrinsics.getKy() * cameraTworld(1, 2)
                    + intrinsics.getCenterV() * cameraTworld(2, 2));
    result(2, 0) = cameraTworld(2, 0);
    result(2, 1) = cameraTworld(2, 1);
    result(2, 2) = cameraTworld(2, 2);
#endif
    return result;
  }

  
}; // namespace


namespace dlr {

  namespace computerVision {
    
    void
    stereoRectify(CameraIntrinsicsPinhole const& intrinsics0,
                  CameraIntrinsicsPinhole const& intrinsics1,
                  numeric::Transform3D const& camera0Tworld,
                  numeric::Transform3D const& camera1Tworld,
                  CameraIntrinsicsPinhole& rectifiedIntrinsics0,
                  CameraIntrinsicsPinhole& rectifiedIntrinsics1,
                  numeric::Transform3D& rcamera0Tworld,
                  numeric::Transform3D& rcamera1Tworld,
                  numeric::Transform2D& image0Trimage0,
                  numeric::Transform2D& image1Trimage1)
    {
      // Hack(xxx): find a principled way to set this threshold.
      double localEpsilon = 1.0E-12;
      
      // Recover optical centers (focal points) of the two cameras in
      // world coords.  These won't change with rectification.
      num::Transform3D worldTcamera0 = camera0Tworld.invert();
      num::Transform3D worldTcamera1 = camera1Tworld.invert();
      num::Vector3D focalPoint0(
        worldTcamera0(0, 3), worldTcamera0(1, 3), worldTcamera0(2, 3));
      num::Vector3D focalPoint1(
        worldTcamera1(0, 3), worldTcamera1(1, 3), worldTcamera1(2, 3));

      // Recover new X axis (expressed in world coordinates) for both
      // cameras.  This is parallel to the stereo baseline.
      num::Vector3D xAxis = focalPoint1 - focalPoint0;
      double xMagnitude = num::magnitude(xAxis);
      if(xMagnitude < localEpsilon) {
        DLR_THROW(ValueException, "rectify()",
                  "Optical centers of camera1 and camera0 are too close "
                  "together.");
      }
      xAxis /= xMagnitude;

      // New Y axis (expressed in world coordinates), again for both
      // cameras, is orthogonal to X, and orthogonal to Z axis of
      // unrectified camera0.  Note that this fails if camera1 lies on
      // the optical axis of camera0.
      num::Vector3D oldZ(
        camera0Tworld(2, 0), camera0Tworld(2, 1), camera0Tworld(2, 2));
      num::Vector3D yAxis = num::cross(oldZ, xAxis);
      double yMagnitude = num::magnitude(yAxis);
      if(yMagnitude < localEpsilon) {
        DLR_THROW(ValueException, "rectify()",
                  "Camera1 lies too close to the optical axis of camera0.");
      }
      yAxis /= yMagnitude;

      // New Z axis (expressed in world coordinates) follows directly
      // from X and Y.
      num::Vector3D zAxis = num::cross(xAxis, yAxis);
      double zMagnitude = num::magnitude(zAxis);
      if(zMagnitude < localEpsilon) {
        DLR_THROW(LogicException, "rectify()",
                  "Error in computation of z axis.");
      }
      zAxis /= zMagnitude;
      
      // The rectified cameras share the same orientation.  The
      // locations of the optical centers (i.e., the translation
      // component of the cameraTworld transforms) remain unchanged.
      num::Transform3D worldTrcamera0(
        xAxis.x(), yAxis.x(), zAxis.x(), worldTcamera0(0, 3),
        xAxis.y(), yAxis.y(), zAxis.y(), worldTcamera0(1, 3),
        xAxis.z(), yAxis.z(), zAxis.z(), worldTcamera0(2, 3),
        0.0, 0.0, 0.0, 1.0);
      num::Transform3D worldTrcamera1(
        xAxis.x(), yAxis.x(), zAxis.x(), worldTcamera1(0, 3),
        xAxis.y(), yAxis.y(), zAxis.y(), worldTcamera1(1, 3),
        xAxis.z(), yAxis.z(), zAxis.z(), worldTcamera1(2, 3),
        0.0, 0.0, 0.0, 1.0);

      rcamera0Tworld = worldTrcamera0.invert();
      rcamera1Tworld = worldTrcamera1.invert();
      
      // The rectified cameras will share the same intrinsic
      // parameters.  Note that these parameters are not uniquely
      // determined.  Choosing them poorly just means that the input
      // images will project into the rectified images suboptimally.
      //
      // Note(xxx): eventually we'll want to do something smart here,
      // but for now we just take the easy path of averaging values
      // from the two input cameras, and taking image size from
      // camera0.
      size_t numPixelsX = intrinsics0.getNumPixelsX();
      size_t numPixelsY = intrinsics0.getNumPixelsY();
      double focalLength =
        (intrinsics0.getFocalLength() + intrinsics1.getFocalLength()) / 2.0;
      double pixelSizeX = 
        (intrinsics0.getPixelSizeX() + intrinsics1.getPixelSizeX()) / 2.0;
      double pixelSizeY = 
        (intrinsics0.getPixelSizeY() + intrinsics1.getPixelSizeY()) / 2.0;
      double centerU = 
        (intrinsics0.getCenterU() + intrinsics1.getCenterU()) / 2.0;
      double centerV = 
        (intrinsics0.getCenterV() + intrinsics1.getCenterV()) / 2.0;
      rectifiedIntrinsics0 = CameraIntrinsicsPinhole(
        numPixelsX, numPixelsY, focalLength, pixelSizeX, pixelSizeY,
        centerU, centerV);
      rectifiedIntrinsics1 = rectifiedIntrinsics0;

      // Finally, 2D transformations to take coordinates in the
      // recified images and return matching coordinates in the
      // unrectified image.
      num::Array2D<double> oldQ0 = extractUpperLeftBlock(
        intrinsics0, camera0Tworld);
      num::Array2D<double> oldQ1 = extractUpperLeftBlock(
        intrinsics1, camera1Tworld);
      num::Array2D<double> newQ0 = extractUpperLeftBlock(
        rectifiedIntrinsics0, rcamera0Tworld);
      num::Array2D<double> newQ1 = extractUpperLeftBlock(
        rectifiedIntrinsics1, rcamera1Tworld);

      num::Array2D<double> rectArray0 =
        num::matrixMultiply(oldQ0, linalg::inverse(newQ0));
      num::Array2D<double> rectArray1 =
        num::matrixMultiply(oldQ1, linalg::inverse(newQ1));
      image0Trimage0.setTransform(
        rectArray0(0, 0), rectArray0(0, 1), rectArray0(0, 2),
        rectArray0(1, 0), rectArray0(1, 1), rectArray0(1, 2),
        rectArray0(2, 0), rectArray0(2, 1), rectArray0(2, 2));
      image1Trimage1.setTransform(
        rectArray1(0, 0), rectArray1(0, 1), rectArray1(0, 2),
        rectArray1(1, 0), rectArray1(1, 1), rectArray1(1, 2),
        rectArray1(2, 0), rectArray1(2, 1), rectArray1(2, 2));
    }


  } // namespace computerVision

} // namespace dlr