cameraIntrinsicsPlumbBob.cpp

Go to the documentation of this file.

#include <iomanip>
#include <dlrCommon/inputStream.h>
#include <dlrComputerVision/cameraIntrinsicsPlumbBob.h>
#include <dlrOptimization/optimizerBFGS.h>
#include <dlrOptimization/optimizerNelderMead.h>

using namespace dlr::numeric;
using namespace dlr::geometry;

namespace {

  // We require reverse projection accuracy of better than 1/100th of
  // a pixel.
  const double l_maximumReverseProjectionResidual = 0.01 * 0.01;
  
}


namespace dlr {

  namespace computerVision {


    // The default constructor initializes the CameraIntrinsicsPlumbBob
    // instance to a consistent (but not terribly useful) state.
    CameraIntrinsicsPlumbBob::
    CameraIntrinsicsPlumbBob()
      : CameraIntrinsics(),
        m_centerU(50),
        m_centerV(50),
        m_kX(1.0),
        m_kY(1.0),
        m_numPixelsX(100),
        m_numPixelsY(100),
        m_radialCoefficient0(0.0),
        m_radialCoefficient1(0.0),
        m_radialCoefficient2(0.0),
        m_skewCoefficient(0.0),
        m_tangentialCoefficient0(0.0),
        m_tangentialCoefficient1(0.0)
    {
      // Empty.
    }
      

    // This constructor allows the caller to explicitly set the
    // camera intrinsic parameters.
    CameraIntrinsicsPlumbBob::
    CameraIntrinsicsPlumbBob(size_t numPixelsX,
                             size_t numPixelsY,
                             double focalLengthX,
                             double focalLengthY,
                             double centerU,
                             double centerV,
                             double skewCoefficient,
                             double radialCoefficient0,
                             double radialCoefficient1,
                             double radialCoefficient2,
                             double tangentialCoefficient0,
                             double tangentialCoefficient1)
      : CameraIntrinsics(),
        m_centerU(centerU),
        m_centerV(centerV),
        m_kX(focalLengthX),
        m_kY(focalLengthY),
        m_numPixelsX(numPixelsX),
        m_numPixelsY(numPixelsY),
        m_radialCoefficient0(radialCoefficient0),
        m_radialCoefficient1(radialCoefficient1),
        m_radialCoefficient2(radialCoefficient2),
        m_skewCoefficient(skewCoefficient),
        m_tangentialCoefficient0(tangentialCoefficient0),
        m_tangentialCoefficient1(tangentialCoefficient1)
    {
      // Empty.
    }
    


    // This member function takes a point in 3D camera coordinates
    // and projects it into pixel coordinates.
    Vector2D
    CameraIntrinsicsPlumbBob::
    project(const Vector3D& point) const
    {
      // The Vector2D constructor will divide by point.z().
      Vector2D normalizedPoint(point.x(), point.y(), point.z());

      double xSquared = normalizedPoint.x() * normalizedPoint.x();
      double ySquared = normalizedPoint.y() * normalizedPoint.y();
      double rSquared = xSquared + ySquared;
      double rFourth = rSquared * rSquared;
      double rSixth = rSquared * rFourth;

      // Compute distortion terms according to plumb bob model.
      double radialDistortion = (1.0 + m_radialCoefficient0 * rSquared
                                 + m_radialCoefficient1 * rFourth
                                 + m_radialCoefficient2 * rSixth);

      double crossTerm = normalizedPoint.x() * normalizedPoint.y();
      Vector2D tangentialDistortion(
        (2.0 * m_tangentialCoefficient0 * crossTerm
         + m_tangentialCoefficient1 * (rSquared + 2.0 * xSquared)),
        (m_tangentialCoefficient0 * (rSquared + 2.0 * ySquared)
         + 2.0 * m_tangentialCoefficient1 * crossTerm));

      // Apply distortion and skew, then project into pixel coordinates.
      Vector2D distortedPoint(
        radialDistortion * normalizedPoint + tangentialDistortion);
      Vector2D skewedPoint(
        distortedPoint.x() + m_skewCoefficient * distortedPoint.y(),
        distortedPoint.y());
      
      return Vector2D(m_kX * skewedPoint.x() + m_centerU,
                      m_kY * skewedPoint.y() + m_centerV);
    }
    

    // This member function sets the calibration from an input
    // stream.
    std::istream&
    CameraIntrinsicsPlumbBob::
    readFromStream(std::istream& stream)
    {
      // If stream is in a bad state, we can't read from it.
      if (!stream){
        return stream;
      }
    
      // Construct an InputStream instance so we can use our
      // convenience functions.
      InputStream inputStream(stream, InputStream::SKIP_WHITESPACE);

      double centerU;
      double centerV;
      double kX;
      double kY;
      size_t numpixelsX;
      size_t numpixelsY;
      double radialCoefficient0;
      double radialCoefficient1;
      double radialCoefficient2;
      double skewCoefficient;
      double tangentialCoefficient0;
      double tangentialCoefficient1;
      
      inputStream.expect("CameraIntrinsicsPlumbBob");
      inputStream.expect("{");
      stream >> numpixelsX;
      inputStream.expect(",");
      stream >> numpixelsY;
      inputStream.expect(",");
      stream >> kX;
      inputStream.expect(",");
      stream >> kY;
      inputStream.expect(",");
      stream >> centerU;
      inputStream.expect(",");
      stream >> centerV;
      inputStream.expect(",");
      stream >> skewCoefficient;
      inputStream.expect(",");
      stream >> radialCoefficient0;
      inputStream.expect(",");
      stream >> radialCoefficient1;
      inputStream.expect(",");
      stream >> radialCoefficient2;
      inputStream.expect(",");
      stream >> tangentialCoefficient0;
      inputStream.expect(",");
      stream >> tangentialCoefficient1;
      inputStream.expect("}");

      if(stream) {
        m_centerU = centerU;
        m_centerV = centerV;
        m_kX = kX;
        m_kY = kY;
        m_numPixelsX = numpixelsX;
        m_numPixelsY = numpixelsY;
        m_radialCoefficient0 = radialCoefficient0;
        m_radialCoefficient1 = radialCoefficient1;
        m_radialCoefficient2 = radialCoefficient2;
        m_skewCoefficient = skewCoefficient;
        m_tangentialCoefficient0 = tangentialCoefficient0;
        m_tangentialCoefficient1 = tangentialCoefficient1;
      }
      return stream;
    }


    // This member function takes a point in 2D pixel coordinates
    // and returns a ray in 3D camera coordinates passing through
    // all of the 3D points that project to the specified 2D
    // position.
    geometry::Ray3D
    CameraIntrinsicsPlumbBob::
    reverseProject(const Vector2D& pixelPosition,
                   bool normalize) const
    {
      // TODO(xxx): Collapse this optimization to 1D when tangential
      // distortion is zero.

      // Ignoring distortion, we calculate an initial guess using the
      // pinhole camera model.  See
      // CameraIntrinsicsPinhole::reverseProject() for more detailed
      // comments.
      //
      // Array1D<double> startPoint(2);
      // startPoint[0] = (pixelPosition.x() - m_centerU) / m_kX;
      // startPoint[1] = (pixelPosition.y() - m_centerV) / m_kY;

      // Actually, for extreme distortions, the estimate above can be
      // a point that projects quite far outside the image.  This can
      // cause numerical problems because we have an eighth power
      // out-of-bounds error term that blows up quickly.  Instead of
      // using the pinhole projection above, we choose the safest
      // start point we can think of... the center of projection.
      Array1D<double> startPoint(2);
      startPoint = 0.0;
      
      // Now optimize to find a better answer.
      privateCode::PlumbBobObjective objective(*this, pixelPosition);
      OptimizerBFGS<privateCode::PlumbBobObjective> optimizer(objective);
      optimizer.setStartPoint(startPoint);
      Array1D<double> endPoint = optimizer.optimum();
      double residual = optimizer.optimalValue();

      // This would be the place to check convergence and try a
      // different start point, if we were so inclined.
      
      if(residual
         > l_maximumReverseProjectionResidual + objective.getOffset()) {
        DLR_THROW(ValueException, "CameraIntrinsicsPlumbBob::reverseProject()",
                  "Reverse projection failed to converge.");
      }
      
      return Ray3D(Vector3D(0.0, 0.0, 0.0),
                   Vector3D(endPoint[0], endPoint[1], 1.0, normalize));
    }


    // This member function writes the calibration to an
    // outputstream in a format which is compatible with member
    // function readFromStream().
    std::ostream&
    CameraIntrinsicsPlumbBob::
    writeToStream(std::ostream& stream) const
    {
      std::ios::fmtflags flags = stream.flags();
      std::streamsize precision = stream.precision();
      stream.precision(15);
      stream << "CameraIntrinsicsPlumbBob {"
             << std::fixed << std::setw(15)
             << m_numPixelsX << ", "
             << m_numPixelsY << ", "
             << m_kX << ", "
             << m_kY << ", "
             << m_centerU << ", "
             << m_centerV << ", "
             << m_skewCoefficient << ", "
             << m_radialCoefficient0 << ", "
             << m_radialCoefficient1 << ", "
             << m_radialCoefficient2 << ", "
             << m_tangentialCoefficient0 << ", "
             << m_tangentialCoefficient1 << "}";
      stream.precision(precision);
      stream.flags(flags);
      return stream;
    }


    void
    CameraIntrinsicsPlumbBob::
    projectWithPartialDerivatives(double xNorm,
                                  double yNorm,
                                  double& uValue,
                                  double& vValue,
                                  double& dUdX,
                                  double& dUdY,
                                  double& dVdX,
                                  double& dVdY) const
    {
      double xSquared = xNorm * xNorm;
      double ySquared = yNorm * yNorm;
      double rSquared = xSquared + ySquared;
      double rFourth = rSquared * rSquared;
      double rSixth = rSquared * rFourth;

      double dRSquaredDX = 2.0 * xNorm;
      double dRSquaredDY = 2.0 * yNorm;
      double dRFourthDX = 2.0 * rSquared * dRSquaredDX;
      double dRFourthDY = 2.0 * rSquared * dRSquaredDY;
      double dRSixthDX = 3.0 * rFourth * dRSquaredDX;
      double dRSixthDY = 3.0 * rFourth * dRSquaredDY;

      // Compute distortion terms according to plumb bob model.
      double radialDistortion = (1.0 + m_radialCoefficient0 * rSquared
                                 + m_radialCoefficient1 * rFourth
                                 + m_radialCoefficient2 * rSixth);
      double dRadialDistortionDX =
        (m_radialCoefficient0 * dRSquaredDX
         + m_radialCoefficient1 * dRFourthDX
         + m_radialCoefficient2 * dRSixthDX);
      double dRadialDistortionDY =
        (m_radialCoefficient0 * dRSquaredDY
         + m_radialCoefficient1 * dRFourthDY
         + m_radialCoefficient2 * dRSixthDY);

      double crossTerm = xNorm * yNorm;
      double dCrossTermDX = yNorm;
      double dCrossTermDY = xNorm;
      
      Vector2D tangentialDistortion(
        (2.0 * m_tangentialCoefficient0 * crossTerm
         + m_tangentialCoefficient1 * (rSquared + 2.0 * xSquared)),
        (m_tangentialCoefficient0 * (rSquared + 2.0 * ySquared)
         + 2.0 * m_tangentialCoefficient1 * crossTerm));
      double dTangXDX =
        (2.0 * m_tangentialCoefficient0 * dCrossTermDX
         + m_tangentialCoefficient1 * (dRSquaredDX + 4.0 * xNorm));
      double dTangXDY =
        (2.0 * m_tangentialCoefficient0 * dCrossTermDY
         + m_tangentialCoefficient1 * dRSquaredDY);
      double dTangYDX =
        (m_tangentialCoefficient0 * dRSquaredDX
         + 2.0 * m_tangentialCoefficient1 * dCrossTermDX);
      double dTangYDY =
        (m_tangentialCoefficient0 * (dRSquaredDY + 4.0 * yNorm)
         + 2.0 * m_tangentialCoefficient1 * dCrossTermDY);

      // Apply distortion and skew, then project into pixel coordinates.
      Vector2D distortedPoint(
        radialDistortion * xNorm + tangentialDistortion.x(),
        radialDistortion * yNorm + tangentialDistortion.y());

      double dDistortedXDX =
        dRadialDistortionDX * xNorm + radialDistortion + dTangXDX;
      double dDistortedXDY = dRadialDistortionDY * xNorm + dTangXDY;
      double dDistortedYDX = dRadialDistortionDX * yNorm + dTangYDX;
      double dDistortedYDY =
        dRadialDistortionDY * yNorm + radialDistortion + dTangYDY;
      
      Vector2D skewedPoint(
        distortedPoint.x() + m_skewCoefficient * distortedPoint.y(),
        distortedPoint.y());
      double dSkewedXDX = dDistortedXDX + m_skewCoefficient * dDistortedYDX;
      double dSkewedXDY = dDistortedXDY + m_skewCoefficient * dDistortedYDY;
      double dSkewedYDX = dDistortedYDX;
      double dSkewedYDY = dDistortedYDY;
      
      uValue = m_kX * skewedPoint.x() + m_centerU;
      vValue = m_kY * skewedPoint.y() + m_centerV;
      dUdX = m_kX * dSkewedXDX;
      dUdY = m_kX * dSkewedXDY;
      dVdX = m_kY * dSkewedYDX;
      dVdY = m_kY * dSkewedYDY;
    }
    


    namespace privateCode {

      PlumbBobObjective::
      PlumbBobObjective(const CameraIntrinsicsPlumbBob& intrinsics,
                        const Vector2D& uvTarget)
        : m_intrinsics(intrinsics),
          m_offset(0.0001),
          m_uvTarget(uvTarget)
      {
        // Empty.
      }

        
      double
      PlumbBobObjective::
      operator()(const Array1D<double>& theta)
      {
        Vector3D candidate(theta[0], theta[1], 1.0);
        Vector2D projection = m_intrinsics.project(candidate);

        // The high order terms of the distortion model can lead to
        // false minima outside the image boundaries.  We add an even
        // higher order penalty for projecting outside the image,
        // hopefully reducing this problem.
        double boundsPenalty = this->computeBoundsPenalty(projection);

        return (magnitudeSquared(projection - m_uvTarget) + boundsPenalty
                + m_offset);
      }


      double
      PlumbBobObjective::
      getOffset()
      {
        return m_offset;
      }

      
      Array1D<double>
      PlumbBobObjective::
      gradient(const Array1D<double>& theta)
      {
        double uValue;
        double vValue;
        double dUdX;
        double dUdY;
        double dVdX;
        double dVdY;
        m_intrinsics.projectWithPartialDerivatives(
          theta[0], theta[1], uValue, vValue, dUdX, dUdY, dVdX, dVdY);

        double twoTimesDeltaU = 2.0 * (uValue - m_uvTarget.x());
        double twoTimesDeltaV = 2.0 * (vValue - m_uvTarget.y());

        Array1D<double> gradient(2);
        gradient[0] = twoTimesDeltaU * dUdX + twoTimesDeltaV * dVdX;
        gradient[1] = twoTimesDeltaU * dUdY + twoTimesDeltaV * dVdY;

        double dPdX;
        double dPdY;
        this->computeBoundsPenaltyGradient(
          uValue, vValue, dUdX, dUdY, dVdX, dVdY, dPdX, dPdY);
        gradient[0] += dPdX;
        gradient[1] += dPdY;
        
        return gradient;
      }


      double
      PlumbBobObjective::
      computeBoundsPenalty(const Vector2D& uvPosition)
      {
        double uViolation = 0.0;
        double scaleU = m_intrinsics.getNumPixelsX() / 10.0;
        if(uvPosition.x() < 0) {
          uViolation = -uvPosition.x() / scaleU;
        } else if(uvPosition.x() >= m_intrinsics.getNumPixelsX()) {
          uViolation = ((uvPosition.x() - m_intrinsics.getNumPixelsX())
                        / scaleU);
        }
        double vViolation = 0.0;
        double scaleV = m_intrinsics.getNumPixelsY() / 10.0;
        if(uvPosition.y() < 0) {
          vViolation = -uvPosition.y() / scaleV;
        } else if(uvPosition.y() >= m_intrinsics.getNumPixelsY()) {
          vViolation = ((uvPosition.y() - m_intrinsics.getNumPixelsY())
                        / scaleV);
        }

        // Bounds penaly is violation**8 so as to dominate the r**6
        // term in the distortion model.
        uViolation *= uViolation;
        uViolation *= uViolation;
        uViolation *= uViolation;
        vViolation *= vViolation;
        vViolation *= vViolation;
        vViolation *= vViolation;
        return uViolation + vViolation;
      }
        

      void
      PlumbBobObjective::
      computeBoundsPenaltyGradient(double uValue, double vValue,
                                   double dUdX, double dUdY,
                                   double dVdX, double dVdY,
                                   double& dPdX, double& dPdY)
      {
        double uViolation = 0.0;
        double scaleU = m_intrinsics.getNumPixelsX() / 10.0;
        double dUVdX = 0.0;
        double dUVdY = 0.0;
        if(uValue < 0) {
          uViolation = -uValue / scaleU;
          dUVdX = -dUdX / scaleU;
          dUVdY = -dUdY / scaleU;
        } else if(uValue >= m_intrinsics.getNumPixelsX()) {
          uViolation = (uValue - m_intrinsics.getNumPixelsX()) / scaleU;
          dUVdX = dUdX / scaleU;
          dUVdY = dUdY / scaleU;
        }
        double vViolation = 0.0;
        double scaleV = m_intrinsics.getNumPixelsY() / 10.0;
        double dVVdX = 0.0;
        double dVVdY = 0.0;
        if(vValue < 0) {
          vViolation = -vValue / scaleV;
          dVVdX = -dVdX / scaleV;
          dVVdY = -dVdY / scaleV;
        } else if(vValue >= m_intrinsics.getNumPixelsY()) {
          vViolation = (vValue - m_intrinsics.getNumPixelsY()) / scaleV;
          dVVdX = dVdX / scaleV;
          dVVdY = dVdY / scaleV;
        }
        // Bounds penaly is violation**8 so as to dominate the r**6
        // term in the distortion model.
        double uViolationTo2 = uViolation * uViolation;
        double uViolationTo4 = uViolationTo2 * uViolationTo2;
        double uViolationTo7 = uViolationTo4 * uViolationTo2 * uViolation;
        double vViolationTo2 = vViolation * vViolation;
        double vViolationTo4 = vViolationTo2 * vViolationTo2;
        double vViolationTo7 = vViolationTo4 * vViolationTo2 * vViolation;

        dPdX = 8.0 * uViolationTo7 * dUVdX + 8.0 * vViolationTo7 * dVVdX;
        dPdY = 8.0 * uViolationTo7 * dUVdY + 8.0 * vViolationTo7 * dVVdY;
      }
        

    } // namespace privateCode;
    
  } // namespace computerVision
  
} // namespace dlr