CCamModel.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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 |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers
#include <mrpt/vision/CCamModel.h>
#include <mrpt/vision/pinhole.h>
#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/utils/types_math.h>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::utils;

/**     Constructor */
CCamModel::CCamModel() : cam() {}
/**********************************************************************************************************************/
void CCamModel::jacob_undistor_fm(
        const mrpt::utils::TPixelCoordf& p, math::CMatrixDouble& J_undist)
{
        // JL: CHECK!!!!
        const double Cx = cam.cx();
        const double Cy = cam.cy();
        const double k1 = cam.k1();
        const double k2 = cam.k2();
        const double dx =
                1.0 / cam.fx();  // JL: Check if formulas hold with this change!!!
        const double dy = 1.0 / cam.fy();

        double xd = (p.x - Cx) * dx;
        double yd = (p.y - Cy) * dy;
        double rd2 = xd * xd + yd * yd;
        double rd4 = rd2 * rd2;

        J_undist.setSize(2, 2);
        J_undist(0, 0) =
                (1 + k1 * rd2 + k2 * rd4) +
                (p.x - Cx) * (k1 + 2 * k2 * rd2) * (2 * (p.x - Cx) * dx * dx);
        J_undist(1, 1) =
                (1 + k1 * rd2 + k2 * rd4) +
                (p.y - Cy) * (k1 + 2 * k2 * rd2) * (2 * (p.y - Cy) * dy * dy);
        J_undist(0, 1) =
                (p.x - Cx) * (k1 + 2 * k2 * rd2) * (2 * (p.y - Cy) * dy * dy);
        J_undist(1, 0) =
                (p.y - Cy) * (k1 + 2 * k2 * rd2) * (2 * (p.x - Cx) * dx * dx);
}
/******************************************************************************************************************************/

void CCamModel::jacob_undistor(
        const mrpt::utils::TPixelCoordf& p, mrpt::math::CMatrixDouble& J_undistor)
{
        // J_undistor.setSize(2,2);
        const double dx = p.x - cam.cx();
        const double dy = p.y - cam.cy();
        const double f = 1 - 2 * cam.k1() * square(dx) * square(dy);
        const double inv_f_15 = 1.0 / pow(f, 1.5);

        //// dy/du
        // CMatrixDouble dy_du(2,2);  // Default is all zeroes
        // dy_du(0,0) = 1.0/cam.fx;
        // dy_du(1,1) = 1.0/cam.fy;

        // du/dh
        // CMatrixDouble du_dh(2,2);

        J_undistor(0, 0) = (1 - 2 * cam.k1() * square(dy)) * inv_f_15;
        J_undistor(0, 1) = J_undistor(1, 0) = (2 * cam.k1() * dx * dy) * inv_f_15;
        J_undistor(1, 1) = (1 - 2 * cam.k1() * square(dx)) * inv_f_15;

        //// JL: TODO: CHECK!
        // const double Cx = cam.cx;
        // const double Cy = cam.cy;
        // const double k1 = cam.k1();
        // const double k2 = cam.k2();
        // const double dx = 1.0 / cam.fx;   // JL: Check if formulas hold with this
        // change!!!
        // const double dy = 1.0 / cam.fy;

        // double xd = (p.x-Cx)*dx;
        // double yd = (p.y-Cy)*dy;

        // double rd2 = xd*xd + yd*yd;
        // double rd4 = rd2 * rd2;

        // J_undistor.setSize(2,2);

        // J_undistor(0,0) = (1+k1*rd2+k2*rd4) + (p.x-Cx) * (k1+2*k2*rd2) *
        // (2*(p.x-Cx)*dx*dx);
        // J_undistor(1,1) = (1+k1*rd2+k2*rd4) + (p.y-Cy) * (k1+2*k2*rd2) *
        // (2*(p.y-Cy)*dy*dy);
        // J_undistor(0,1) = (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
        // J_undistor(1,0) = (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
}
/**********************************************************************************************************************/

void CCamModel::distort_a_point(
        const mrpt::utils::TPixelCoordf& p, mrpt::utils::TPixelCoordf& distorted_p)
{
        // JLBC: Added from Davison's SceneLib:
        //
        // 1 distortion coefficient model
        //
        const double dx = (p.x - cam.cx());  // / cam.fx;   //JL: commented out
        // cam.fxy... (dx,dy) units must be
        // pixels
        const double dy = (p.y - cam.cy());  // / cam.fy;

        const double r2 = square(dx) + square(dy);

        const double fact = 1.0 / sqrt(1 + 2 * cam.k1() * r2);

        distorted_p.x = cam.cx() + dx * fact;
        distorted_p.y = cam.cy() + dy * fact;
        return;
}
/*************************************************************************************************************************/
// Removes distortion of a pair of pixel coordinates x,y.
void CCamModel::undistort_point(
        const mrpt::utils::TPixelCoordf& p,
        mrpt::utils::TPixelCoordf& undistorted_p)
{
        std::vector<TPixelCoordf> in_p(1), out_p;
        in_p[0] = p;

        mrpt::vision::pinhole::undistort_points(
                in_p, out_p, cam.intrinsicParams, cam.getDistortionParamsAsVector());

        ASSERT_(out_p.size() == 1);
        undistorted_p = out_p[0];

        // It's explained fine in page 3, "A visual compass based on SLAM"
}

/*************************************************************************************************************************/
/*************************************************************************************************************************/
/**************************************************Davison
 * Style**********************************************************/
/*************************************************************************************************************************/

/**     Return the (distorted) pixel position of a 3D point given in coordinates
 * relative to the camera (+Z pointing forward, +X to the right)
 */
void CCamModel::project_3D_point(
        const mrpt::math::TPoint3D& p3D,
        mrpt::utils::TPixelCoordf& distorted_p) const
{
        // JLBC: From Davison's SceneLib:
        //
        // 1 distortion coefficient model (+ projection)
        //

        // Offsets from the image center for the undistorted projection, in units of
        // pixels:

        ASSERT_(p3D.z != 0)
        const double dx = (p3D.x / p3D.z) * cam.fx();
        const double dy = (p3D.y / p3D.z) * cam.fy();

        // 1 distortion coeff. model:
        const double r2 = square(dx) + square(dy);

        const double fact =
                1.0 / sqrt(1 + 2 * cam.k1() * r2);  // Note the "+2" sign

        distorted_p.x = cam.cx() + dx * fact;
        distorted_p.y = cam.cy() + dy * fact;
}

/**     Return the 3D location of a point (at a fixed distance z=1), for the given
 * (distorted) pixel position
  */
void CCamModel::unproject_3D_point(
        const mrpt::utils::TPixelCoordf& distorted_p,
        mrpt::math::TPoint3D& p3D) const
{
        // JLBC: From Davison's SceneLib:
        //
        // 1 distortion coefficient model (+ projection)
        //
        const double dx = distorted_p.x - cam.cx();
        const double dy = distorted_p.y - cam.cy();
        const double r2 = square(dx) + square(dy);
        double factor = 1.0 / sqrt(1 - 2 * cam.k1() * r2);  // Note the "-2" sign

        p3D.x = dx * factor / cam.fx();
        p3D.y = dy * factor / cam.fy();
        p3D.z = 1.0;
}

// Jacobian of the projection of 3D points (with distortion), as done in
// project_3D_point \f$ \frac{\partial \vct{h}}{\partial \vct{y}} \f$
// JL: See .h file for all the formulas
void CCamModel::jacobian_project_with_distortion(
        const mrpt::math::TPoint3D& p3D, math::CMatrixDouble& dh_dy) const
{
        /*
        \frac{\partial \vct{u}}{\partial \vct{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)
        */
        ASSERT_(p3D.z != 0)

        CMatrixDouble du_dy(2, 3);  // Default all to zeroes.

        du_dy(0, 0) = cam.fx() / p3D.z;
        du_dy(0, 2) = -p3D.x * cam.fx() / square(p3D.z);
        du_dy(1, 1) = cam.fy() / p3D.z;
        du_dy(1, 2) = -p3D.y * cam.fy() / square(p3D.z);

        /*
         f = 1+ 2  k_1  (u_x^2+u_y^2),  then:

        \frac{\partial \vct{h}}{\partial \vct{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)
        */
        const double ux = (p3D.x / p3D.z) *
                                          cam.fx();  // coordinates with (0,0) at the image center
        const double uy = (p3D.y / p3D.z) * cam.fy();

        const double ux_sqr = square(ux);
        const double uy_sqr = square(uy);

        const double r2 = ux_sqr + uy_sqr;

        const double f = 1 + 2 * cam.k1() * r2;
        const double f1_2 = sqrt(f);
        const double f3_2 = f1_2 * f;

        // Now form the proper dh_by_du by manipulating the outer product matrix

        CMatrixDouble dh_du(2, 2);
        dh_du *= -2 * cam.k1() / f3_2;
        dh_du(0, 0) += (1 / f1_2);
        dh_du(1, 1) += (1 / f1_2);

        // dh_du(0,0) = 1+2*cam.k1()*uy_sqr;
        // dh_du(1,1) = 1+2*cam.k1()*ux_sqr;

        // dh_du(0,1) =
        // dh_du(1,0) = -2*ux*uy*cam.k1()/pow(f,1.5);

        // Jacobian dh_dy = dh_du * du_dy   (Result is 2x3)
        dh_dy.multiply(dh_du, du_dy);
}

/* Jacobian of the unprojection of a pixel (with distortion) back into a 3D
point, as done in unproject_3D_point \f$ \frac{\partial \vct{y}}{\partial
\vct{h}} \f$, evaluated at the pixel p
\note JLBC: Added in March, 2009. Should be equivalent to Davison's
WideCamera::ProjectionJacobian
\sa unproject_3D_point
*/
void CCamModel::jacobian_unproject_with_distortion(
        const mrpt::utils::TPixelCoordf& p, math::CMatrixDouble& dy_dh) const
{
        // dy/du
        CMatrixDouble dy_du(3, 2);  // Default is all zeroes
        dy_du(0, 0) = 1.0 / cam.fx();
        dy_du(1, 1) = 1.0 / cam.fy();

        // MAAA:
        //// du/dh
        const double dx = p.x - cam.cx();
        const double dy = p.y - cam.cy();
        const double radi2 = square(dx) + square(dy);

        double f = 1 - 2 * cam.k1() * radi2;
        double f1_2 = sqrt(f);
        double f3_2 = f1_2 * f;

        CMatrixDouble du_dh(2, 3);

        // const double f = 1 - 2*cam.k1()*radi2;
        // const double inv_f_15 = 1.0f/powf(f,1.5f);
        // du_dh(0,0) = ( 1 - 2*cam.k1()*square(dy) ) * inv_f_15;
        // du_dh(0,1) =
        // du_dh(1,0) = ( 2*cam.k1()*dx*dy ) * inv_f_15;
        // du_dh(1,1) = ( 1 - 2*cam.k1()*square(dx) ) * inv_f_15;

        du_dh *= 2 * cam.k1() / f3_2;
        du_dh(0, 0) += (1 / f1_2);
        du_dh(1, 1) += (1 / f1_2);

        // Jacobian dy_dh = dy_du * du_dh   (Result is 3x2)
        dy_dh.multiply(dy_du, du_dh);
}

void CCamModel::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& source, const std::string& section)
{
        MRPT_START

        // Read camera parameters: They are mandatory, we'll raise an exception if
        // not found:
        double cx = 0.0f, cy = 0.0f, fx = 0.0f, fy = 0.0f;

        // MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(nrows,int,cam.nrows,    source,
        // section)
        // MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(ncols,int,cam.ncols,    source,
        // section)

        MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(cx, double, cx, source, section)
        MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(cy, double, cy, source, section)
        MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(fx, double, fx, source, section)
        MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(fy, double, fy, source, section)

        cam.setIntrinsicParamsFromValues(fx, fy, cx, cy);

        CVectorDouble DD;
        source.read_vector(section, "dist_params", CVectorDouble(), DD, true);
        ASSERT_(DD.size() == 4 || DD.size() == 5)

        this->cam.setDistortionParamsVector(DD);

        MRPT_END
}

/** This method displays clearly all the contents of the structure in textual
 * form, sending it to a CStream. */
void CCamModel::dumpToTextStream(CStream& out) const { MRPT_UNUSED_PARAM(out); }