cameraIntrinsicsPlumbBob.h

Go to the documentation of this file.

#ifndef DLR_COMPUTERVISION_CAMERAINTRINSICSPLUMBBOB_H
#define DLR_COMPUTERVISION_CAMERAINTRINSICSPLUMBBOB_H

#include <iostream>
#include <dlrComputerVision/cameraIntrinsics.h>
#include <dlrNumeric/array1D.h>
#include <dlrNumeric/utilities.h>

namespace dlr {

  namespace computerVision {

    namespace privateCode {

      // Forward declaration of class to help with reverse projection.
      class PlumbBobObjective;

    } // namespace privateCode;

    
    class CameraIntrinsicsPlumbBob : public CameraIntrinsics {
      // This class is defined externally so that it can be more
      // easily unit tested.
      friend class privateCode::PlumbBobObjective;
      
    public:

      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);


      virtual
      ~CameraIntrinsicsPlumbBob() {}


      double
      getCenterU() const {return m_centerU;}


      double
      getCenterV() const {return m_centerV;}


      double
      getFocalLengthX() const {return m_kX;}


      double
      getFocalLengthY() const {return m_kY;}


      virtual size_t
      getNumPixelsX() const {return m_numPixelsX;}


      virtual size_t
      getNumPixelsY() const {return m_numPixelsY;}

      
      double
      getRadialCoefficient0() const {return m_radialCoefficient0;}


      double
      getRadialCoefficient1() const {return m_radialCoefficient1;}


      double
      getRadialCoefficient2() const {return m_radialCoefficient2;}


      double
      getSkewCoefficient() const {return m_skewCoefficient;}


      double
      getTangentialCoefficient0() const {return m_tangentialCoefficient0;}


      double
      getTangentialCoefficient1() const {return m_tangentialCoefficient1;}

      
      virtual dlr::numeric::Vector2D
      project(const dlr::numeric::Vector3D& point) const;


      std::istream&
      readFromStream(std::istream& inputStream);


      virtual dlr::geometry::Ray3D
      reverseProject(const dlr::numeric::Vector2D& pixelPosition,
                     bool normalize = true) const;


      std::ostream&
      writeToStream(std::ostream& outputStream) const;

      
    protected:

      // Protected member function used during iterative approximation
      // in CameraIntrinsicsPlumbBob::reverseProject().
      void
      projectWithPartialDerivatives(double xNorm,
                                    double yNorm,
                                    double& uValue,
                                    double& vValue,
                                    double& dUdX,
                                    double& dUdY,
                                    double& dVdX,
                                    double& dVdY) const;
      
      double m_centerU;
      double m_centerV;
      double m_kX;
      double m_kY;
      size_t m_numPixelsX;
      size_t m_numPixelsY;
      double m_radialCoefficient0;
      double m_radialCoefficient1;
      double m_radialCoefficient2;
      double m_skewCoefficient;
      double m_tangentialCoefficient0;
      double m_tangentialCoefficient1;
    };


    inline std::ostream&
    operator<<(std::ostream& stream,
               const CameraIntrinsicsPlumbBob& intrinsics)
    {
      return intrinsics.writeToStream(stream);
    }


    inline std::istream&
    operator>>(std::istream& stream,
               CameraIntrinsicsPlumbBob& intrinsics)
    {
      return intrinsics.readFromStream(stream);
    }


    namespace privateCode {

      class PlumbBobObjective
        : public std::unary_function<dlr::numeric::Array1D<double>, double>
      {
      public:
        PlumbBobObjective(const CameraIntrinsicsPlumbBob& intrinsics,
                          const dlr::numeric::Vector2D& uvTarget);
        
        double
        operator()(const dlr::numeric::Array1D<double>& theta);

        double
        getOffset();
        
        dlr::numeric::Array1D<double>
        gradient(const dlr::numeric::Array1D<double>& theta);

      private:

        double
        computeBoundsPenalty(const Vector2D& uvPosition);

        void
        computeBoundsPenaltyGradient(double uValue, double vValue,
                                     double dUdX, double dUdY,
                                     double dVdX, double dVdY,
                                     double& dPdX, double& dPdY);

        CameraIntrinsicsPlumbBob m_intrinsics;
        double m_offset;
        dlr::numeric::Vector2D m_uvTarget;
      };
      

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


/* ============ Definitions of inline & template functions ============ */


namespace dlr {

  
} // namespace dlr

#endif /* #ifndef DLR_COMPUTERVISION_CAMERAINTRINSICSPLUMBBOB_H */