CCamModel.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CCamModel_H
#define CCamModel_H

#include <mrpt/img/TCamera.h>
#include <mrpt/system/os.h>
#include <mrpt/vision/utils.h>
#include <mrpt/math/lightweight_geom_data.h>
#include <mrpt/math/CArrayNumeric.h>

namespace mrpt::vision
{
/** This class represent a pinhole camera model for Monocular SLAM and
 * implements some associated Jacobians
 *
 *  The camera parameters are accessible in the public member CCamModel::cam
 *
 * - Versions:
 *    - First version: By Antonio J. Ortiz de Galistea.
 *    - 2009-2010: Rewritten by various authors.
 *
 * \sa mrpt::img::TCamera, CMonoSlam, the application <a
 * href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a>
 * for calibrating a camera
 * \ingroup mrpt_vision_grp
 */
class CCamModel : public mrpt::config::CLoadableOptions
{
   public:
    /** The parameters of a camera */
    mrpt::img::TCamera cam;

    /** Default Constructor */
    CCamModel();

    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source,
        const std::string& section) override;  // See base docs
    void dumpToTextStream(std::ostream& out) const override;  // See base docs

    /** Constructor from a ini file
     */
    CCamModel(const mrpt::config::CConfigFileBase& cfgIni);

    /** Jacobian for undistortion the image coordinates */
    void jacob_undistor_fm(
        const mrpt::img::TPixelCoordf& uvd, math::CMatrixDouble& J_undist);

    /** Calculate the image coordinates undistorted
     */
    void jacob_undistor(
        const mrpt::img::TPixelCoordf& p, mrpt::math::CMatrixDouble& J_undist);

    /** Return the pixel position distorted by the camera
     */
    void distort_a_point(
        const mrpt::img::TPixelCoordf& p, mrpt::img::TPixelCoordf& distorted_p);

    /** Return the pixel position undistorted by the camera
     *  The input values 'col' and 'row' will be replace for the new values
     *(undistorted)
     */
    void undistort_point(
        const mrpt::img::TPixelCoordf& p,
        mrpt::img::TPixelCoordf& undistorted_p);

    /** Return the (distorted) pixel position of a 3D point given in
     * coordinates relative to the camera (+Z pointing forward, +X to the right)
     * \sa unproject_3D_point
     */
    void project_3D_point(
        const mrpt::math::TPoint3D& p3D,
        mrpt::img::TPixelCoordf& distorted_p) const;

    /** Return the 3D location of a point (at a fixed distance z=1), for the
     * given (distorted) pixel position
     * \sa project_3D_point
     * \note Of course, there is a depth ambiguity, so the returned 3D point
     * must be considered a direction from the camera focus, or a vector, rather
     * than a meaninful physical point.
     */
    void unproject_3D_point(
        const mrpt::img::TPixelCoordf& distorted_p,
        mrpt::math::TPoint3D& p3D) const;

    /** Jacobian of the projection of 3D points (with distortion), as done in
    project_3D_point \f$ \frac{\partial h}{\partial y} \f$, evaluated at the
    point p3D (read below the full explanation)

    We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin
    at the top-left corner),
    and \f$ y=( y_x ~ y_y ~ y_z ) \f$ as the 3D point in space, in coordinates
    relative to the camera (+Z pointing forwards).

    Then this method computes the 2x3 Jacobian:

    \f[
    \frac{\partial h}{\partial y} =  \frac{\partial h}{\partial u}
    \frac{\partial u}{\partial y}
    \f]

    With:

    \f[
    \frac{\partial u}{\partial y} =
    \left( \begin{array}{ccc}
     \frac{f_x}{y_z} &  0 & - y \frac{f_x}{y_z^2} \\
     0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\
    \end{array} \right)
    \f]

    where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and
    y, respectively.
    And, if we define:

    \f[
     f = 1+ 2  k_1  (u_x^2+u_y^2)
    \f]

    then:

    \f[
    \frac{\partial h}{\partial u} =
    \left( \begin{array}{cc}
     \frac{ 1+2 k_1 u_y^2 }{f^{3/2}}  &  -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
     -\frac{2 u_x u_y k_1 }{f^{3/2}}  & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
    \end{array} \right)
    \f]

    \note JLBC: Added in March, 2009. Should be equivalent to Davison's
    WideCamera::ProjectionJacobian
    \sa project_3D_point
    */
    void jacobian_project_with_distortion(
        const mrpt::math::TPoint3D& p3D, math::CMatrixDouble& dh_dy) const;

    /** Jacobian of the unprojection of a pixel (with distortion) back into a 3D
    point, as done in unproject_3D_point \f$ \frac{\partial y}{\partial h} \f$,
    evaluated at the pixel p
    \note JLBC: Added in March, 2009. Should be equivalent to Davison's
    WideCamera::UnprojectionJacobian
    \sa unproject_3D_point
    */
    void jacobian_unproject_with_distortion(
        const mrpt::img::TPixelCoordf& p, math::CMatrixDouble& dy_dh) const;

    template <typename T>
    struct CameraTempVariables
    {
        T x_, y_;
        T x_2, y_2;
        T R;
        T K;
        T x__, y__;
    };
    template <typename T, typename POINT>
    void getTemporaryVariablesForTransform(
        const POINT& p, CameraTempVariables<T>& v) const
    {
        v.x_ = p[1] / p[0];
        v.y_ = p[2] / p[0];
        v.x_2 = square(v.x_);
        v.y_2 = square(v.y_);
        v.R = v.x_2 + v.y_2;
        v.K = 1 + v.R * (cam.k1() + v.R * (cam.k2() + v.R * cam.k3()));
        T xy = v.x_ * v.y_, p1 = cam.p1(), p2 = cam.p2();
        v.x__ = v.x_ * v.K + 2 * p1 * xy + p2 * (3 * v.x_2 + v.y_2);
        v.y__ = v.y_ * v.K + p1 * (v.x_2 + 3 * v.y_2) + 2 * p2 * xy;
    }

    template <typename T, typename POINT, typename PIXEL>
    inline void getFullProjection(const POINT& pIn, PIXEL& pOut) const
    {
        CameraTempVariables<T> tmp;
        getTemporaryVariablesForTransform(pIn, tmp);
        getFullProjectionT(tmp, pOut);
    }

    template <typename T, typename PIXEL>
    inline void getFullProjectionT(
        const CameraTempVariables<T>& tmp, PIXEL& pOut) const
    {
        pOut[0] = cam.fx() * tmp.x__ + cam.cx();
        pOut[1] = cam.fy() * tmp.y__ + cam.cy();
    }

    template <typename T, typename POINT, typename MATRIX>
    inline void getFullJacobian(const POINT& pIn, MATRIX& mOut) const
    {
        CameraTempVariables<T> tmp;
        getTemporaryVariablesForTransform(pIn, tmp);
        getFullJacobianT(pIn, tmp, mOut);
    }

    template <typename T, typename POINT, typename MATRIX>
    void getFullJacobianT(
        const POINT& pIn, const CameraTempVariables<T>& tmp, MATRIX& mOut) const
    {
        T x_ = 1 / pIn[0];
        T x_2 = square(x_);
        // First two jacobians...
        mrpt::math::CMatrixFixedNumeric<T, 3, 3> J21;
        T tmpK = 2 * (cam.k1() + tmp.R * (2 * cam.k2() + 3 * tmp.R * cam.k3()));
        T tmpKx = tmpK * tmp.x_;
        T tmpKy = tmpK * tmp.y_;
        T yx2 = -pIn[1] * x_2;
        T zx2 = -pIn[2] * x_2;
        J21.set_unsafe(0, 0, yx2);
        J21.set_unsafe(0, 1, x_);
        J21.set_unsafe(0, 2, 0);
        J21.set_unsafe(1, 0, zx2);
        J21.set_unsafe(1, 1, 0);
        J21.set_unsafe(1, 2, x_);
        J21.set_unsafe(2, 0, tmpKx * yx2 + tmpKy * zx2);
        J21.set_unsafe(2, 1, tmpKx * x_);
        J21.set_unsafe(2, 2, tmpKy * x_);
        // Last two jacobians...
        T pxpy = 2 * (cam.p1() * tmp.x_ + cam.p2() * tmp.y_);
        T p1y = cam.p1() * tmp.y_;
        T p2x = cam.p2() * tmp.x_;
        mrpt::math::CMatrixFixedNumeric<T, 2, 3> J43;
        T fx = cam.fx(), fy = cam.fy();
        J43.set_unsafe(0, 0, fx * (tmp.K + 2 * p1y + 6 * p2x));
        J43.set_unsafe(0, 1, fx * pxpy);
        J43.set_unsafe(0, 2, fx * tmp.x_);
        J43.set_unsafe(1, 0, fy * pxpy);
        J43.set_unsafe(1, 1, fy * (tmp.K + 6 * p1y + 2 * p2x));
        J43.set_unsafe(1, 2, fy * tmp.y_);
        mOut.multiply(J43, J21);
        // cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;
    }

   private:
    // These functions are little tricks to avoid multiple initialization.
    // They are intended to initialize the common parts of the jacobians just
    // once,
    // and not in each iteration.
    // They are mostly useless outside the scope of this function.
    mrpt::math::CMatrixFixedNumeric<double, 2, 2> firstInverseJacobian() const
    {
        mrpt::math::CMatrixFixedNumeric<double, 2, 2> res;
        res.set_unsafe(0, 1, 0);
        res.set_unsafe(1, 0, 0);
        return res;
    }
    mrpt::math::CMatrixFixedNumeric<double, 4, 2> secondInverseJacobian() const
    {
        mrpt::math::CMatrixFixedNumeric<double, 4, 2> res;
        res.set_unsafe(0, 0, 1);
        res.set_unsafe(0, 1, 0);
        res.set_unsafe(1, 0, 0);
        res.set_unsafe(1, 1, 1);
        return res;
    }
    mrpt::math::CMatrixFixedNumeric<double, 3, 4> thirdInverseJacobian() const
    {
        mrpt::math::CMatrixFixedNumeric<double, 3, 4> res;
        res.set_unsafe(0, 1, 0);
        res.set_unsafe(0, 2, 0);
        res.set_unsafe(1, 0, 0);
        res.set_unsafe(1, 2, 0);
        res.set_unsafe(2, 0, 0);
        res.set_unsafe(2, 1, 0);
        res.set_unsafe(2, 2, 1);
        res.set_unsafe(2, 3, 0);
        return res;
    }

   public:
    template <typename POINTIN, typename POINTOUT, typename MAT22>
    void getFullInverseModelWithJacobian(
        const POINTIN& pIn, POINTOUT& pOut, MAT22& jOut) const
    {
        // Temporary variables (well, there are some more, but these are the
        // basics)
        // WARNING!: this shortcut to avoid repeated initialization makes the
        // method somewhat
        // faster, but makes it incapable of being used in more than one thread
        // simultaneously!
        using mrpt::square;
        static mrpt::math::CMatrixFixedNumeric<double, 2, 2> J1(
            firstInverseJacobian());
        static mrpt::math::CMatrixFixedNumeric<double, 4, 2> J2(
            secondInverseJacobian());
        static mrpt::math::CMatrixFixedNumeric<double, 3, 4> J3(
            thirdInverseJacobian());
        static mrpt::math::CMatrixFixedNumeric<double, 2, 3> J4;  // This is not
        // initialized
        // in a
        // special
        // way,
        // although
        // declaring
        // it
        mrpt::math::CArrayNumeric<double, 4> tmp1;
        mrpt::math::CArrayNumeric<double, 2> tmp2;  // This would be a
        // array<double,3>, but to
        // avoid copying, we let
        // "R2" lie in tmp1.
        // Camera Parameters
        double cx = cam.cx(), cy = cam.cy(), ifx = 1 / cam.fx(),
               ify = 1 / cam.fy();
        double K1 = cam.k1(), K2 = cam.k2(), p1 = cam.p1(), p2 = cam.p2(),
               K3 = cam.k3();
        // First step: intrinsic matrix.
        tmp1[0] = (pIn[0] - cx) * ifx;
        tmp1[1] = (pIn[1] - cy) * ify;
        J1.set_unsafe(0, 0, ifx);
        J1.set_unsafe(1, 1, ify);
        // Second step: adding temporary variables, related to the distortion.
        tmp1[2] = square(tmp1[0]) + square(tmp1[1]);
        double sK1 = square(K1);
        double K12 = sK1 - K2;
        double K123 = -K1 * sK1 + 2 * K1 * K2 - K3;  //-K1^3+2K1K2-K3
        // tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);
        tmp1[3] = 1 + tmp1[2] * (-K1 + tmp1[2] * (K12 + tmp1[2] * K123));
        J2.set_unsafe(2, 0, 2 * tmp1[0]);
        J2.set_unsafe(2, 1, 2 * tmp1[1]);
        double jTemp = -2 * K1 + 4 * tmp1[2] * K12 + 6 * square(tmp1[2]) * K123;
        J2.set_unsafe(3, 0, tmp1[0] * jTemp);
        J2.set_unsafe(3, 1, tmp1[1] * jTemp);
        // Third step: radial distortion. Really simple, since most work has
        // been done in the previous step.
        tmp2[0] = tmp1[0] * tmp1[3];
        tmp2[1] = tmp1[1] * tmp1[3];
        J3.set_unsafe(0, 0, tmp1[3]);
        J3.set_unsafe(0, 3, tmp1[0]);
        J3.set_unsafe(1, 1, tmp1[3]);
        J3.set_unsafe(1, 3, tmp1[1]);
        // Fourth step: tangential distorion. A little more complicated, but not
        // much more.
        double prod = tmp2[0] * tmp2[1];
        // References to tmp1[2] are not errors! That element is "R2".
        pOut[0] = tmp2[0] - p1 * prod - p2 * (tmp1[2] + 2 * square(tmp2[0]));
        pOut[1] = tmp2[1] - p1 * (tmp1[2] + 2 * square(tmp2[1])) - p2 * prod;
        J4.set_unsafe(0, 0, 1 - p1 * tmp2[1] - 4 * p2 * tmp2[0]);
        J4.set_unsafe(0, 1, -p1 * tmp2[0]);
        J4.set_unsafe(0, 2, -p2);
        J4.set_unsafe(1, 0, -p2 * tmp2[1]);
        J4.set_unsafe(1, 1, 1 - 4 * p1 * tmp2[1] - p2 * tmp2[0]);
        J4.set_unsafe(1, 2, -p1);
        // As fast as possible, and without more temporaries, let the jacobian
        // be J4*J3*J2*J1;
        jOut.multiply_ABC(J4, J3, J2);  // Note that using the other order is
        // not possible due to matrix sizes
        // (jOut may, and most probably will, be
        // fixed).
        jOut.multiply(jOut, J1);
    }

};  // end class

}
#endif  //__CCamModel_H