pinhole.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers

#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/vision/pinhole.h>

// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::vision::pinhole;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace std;

/* -------------------------------------------------------
                projectPoints_no_distortion
   ------------------------------------------------------- */
void mrpt::vision::pinhole::projectPoints_no_distortion(
    const std::vector<mrpt::math::TPoint3D>& in_points_3D,
    const mrpt::poses::CPose3D& cameraPose,
    const mrpt::math::CMatrixDouble33& intrinsicParams,
    std::vector<TPixelCoordf>& projectedPoints, bool accept_points_behind)
{
    MRPT_START

    // Do NOT distort points:
    static const std::vector<double> distortion_dummy(4, 0);

    projectPoints_with_distortion(
        in_points_3D, cameraPose, intrinsicParams, distortion_dummy,
        projectedPoints, accept_points_behind);
    MRPT_END
}

/* -------------------------------------------------------
                projectPoints_with_distortion
   ------------------------------------------------------- */
void mrpt::vision::pinhole::projectPoints_with_distortion(
    const std::vector<mrpt::math::TPoint3D>& in_points_3D,
    const mrpt::poses::CPose3D& cameraPose,
    const mrpt::math::CMatrixDouble33& intrinsicParams,
    const std::vector<double>& distortionParams,
    std::vector<mrpt::img::TPixelCoordf>& projectedPoints,
    bool accept_points_behind)
{
    MRPT_START
#if MRPT_HAS_OPENCV

    ASSERT_(intrinsicParams.rows() == 3);
    ASSERT_(intrinsicParams.cols() == 3);
    ASSERT_(distortionParams.size() == 4 || distortionParams.size() == 5);

    const size_t N = in_points_3D.size();
    projectedPoints.resize(N);

    if (!N) return;  // Nothing to do

    vector<CvPoint3D64f> objPoints(N);

    // generate points relative to camera:
    for (size_t i = 0; i < N; i++)
        cameraPose.inverseComposePoint(
            in_points_3D[i].x, in_points_3D[i].y, in_points_3D[i].z,
            objPoints[i].x, objPoints[i].y, objPoints[i].z);

    // Points are already translated & rotated:
    static double rotation_matrix[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
    static double translation_vector[] = {0, 0, 0};

    // Projection matrix:
    //   0 1 2
    //   3 4 5
    //   6 7 8
    std::vector<double> proj_matrix(9);
    proj_matrix[0] = intrinsicParams(0, 0);
    proj_matrix[4] = intrinsicParams(1, 1);
    proj_matrix[2] = intrinsicParams(0, 2);
    proj_matrix[5] = intrinsicParams(1, 2);

    // Do the projection:
    cv::Mat object_points = cv::Mat(N, 1, CV_64FC3, &objPoints[0]);

    cv::Mat rotvec;
    cv::Rodrigues(cv::Mat(3, 3, CV_64FC1, rotation_matrix), rotvec);

    cv::Mat _translation_vector = cv::Mat(3, 1, CV_64FC1, translation_vector);
    cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, &proj_matrix[0]);
    cv::Mat dist_coeffs =
        cv::Mat(5, 1, CV_64FC1, const_cast<double*>(&distortionParams[0]));

    vector<cv::Point2d> image_points;

    cv::projectPoints(
        object_points, rotvec, _translation_vector, camera_matrix, dist_coeffs,
        image_points);

    for (size_t i = 0; i < N; i++)
    {
        if (accept_points_behind || objPoints[i].z > 0)
        {  // Valid point or we accept them:
            projectedPoints[i].x = image_points[i].x;
            projectedPoints[i].y = image_points[i].y;
        }
        else
        {  // Invalid point behind the camera:
            projectedPoints[i].x = -1;
            projectedPoints[i].y = -1;
        }
    }

#else
    THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV");
#endif
    MRPT_END
}

/* -------------------------------------------------------
                undistort_points
   ------------------------------------------------------- */
void mrpt::vision::pinhole::undistort_points(
    const std::vector<mrpt::img::TPixelCoordf>& in_dist_pixels,
    std::vector<mrpt::img::TPixelCoordf>& out_pixels,
    const mrpt::math::CMatrixDouble33& A, const std::vector<double>& Dk)
{  // Hub function:
    TCamera cam;
    cam.intrinsicParams = A;
    ASSERT_(Dk.size() <= cam.dist.size());
    for (size_t i = 0; i < cam.dist.size(); i++) cam.dist[i] = Dk[i];
    undistort_points(in_dist_pixels, out_pixels, cam);
}

void mrpt::vision::pinhole::undistort_points(
    const std::vector<mrpt::img::TPixelCoordf>& in_dist_pixels,
    std::vector<mrpt::img::TPixelCoordf>& out_pixels,
    const mrpt::img::TCamera& cameraModel)
{
    MRPT_START

    // based on code from OpenCV 1.1.0, function cvUndistortPoints, file
    // cvundistort.cpp
    // Jose Luis: Great code clean up wrt opencv's since we assume C++ and
    // availability of MRPT's matrices.
    const size_t n = in_dist_pixels.size();
    out_pixels.resize(n);

    const double fx = cameraModel.fx();
    const double fy = cameraModel.fy();
    const double ifx = 1. / fx;
    const double ify = 1. / fy;
    const double cx = cameraModel.cx();
    const double cy = cameraModel.cy();

    for (size_t i = 0; i < n; i++)
    {
        double x = in_dist_pixels[i].x;
        double y = in_dist_pixels[i].y;

        double x0 = x = (x - cx) * ifx;
        double y0 = y = (y - cy) * ify;

        // compensate distortion iteratively
        for (unsigned int j = 0; j < 5; j++)
        {
            double r2 = x * x + y * y;
            double icdist =
                1. /
                (1 + ((cameraModel.dist[4] * r2 + cameraModel.dist[1]) * r2 +
                      cameraModel.dist[0]) *
                         r2);
            double deltaX = 2 * cameraModel.dist[2] * x * y +
                            cameraModel.dist[3] * (r2 + 2 * x * x);
            double deltaY = cameraModel.dist[2] * (r2 + 2 * y * y) +
                            2 * cameraModel.dist[3] * x * y;
            x = (x0 - deltaX) * icdist;
            y = (y0 - deltaY) * icdist;
        }

        // Save undistorted pixel coords:
        out_pixels[i].x = x * fx + cx;
        out_pixels[i].y = y * fy + cy;

    }  // end for i

    MRPT_END
}

/** Undistort one point given by its pixel coordinates and the camera
 * parameters.
 * \sa undistort_points
 */
void mrpt::vision::pinhole::undistort_point(
    const TPixelCoordf& inPt, TPixelCoordf& outPt,
    const mrpt::img::TCamera& cameraModel)
{
    MRPT_START

    // based on code from OpenCV 1.1.0, function cvUndistortPoints, file
    // cvundistort.cpp
    // Jose Luis: Great code clean up wrt opencv's since we assume C++ and
    // availability of MRPT's matrices.
    const double fx = cameraModel.fx();
    const double fy = cameraModel.fy();
    const double ifx = 1. / fx;
    const double ify = 1. / fy;
    const double cx = cameraModel.cx();
    const double cy = cameraModel.cy();

    double x = inPt.x;
    double y = inPt.y;

    double x0 = x = (x - cx) * ifx;
    double y0 = y = (y - cy) * ify;

    // compensate distortion iteratively
    for (unsigned int j = 0; j < 5; j++)
    {
        double r2 = x * x + y * y;
        double icdist =
            1. / (1 + ((cameraModel.dist[4] * r2 + cameraModel.dist[1]) * r2 +
                       cameraModel.dist[0]) *
                          r2);
        double deltaX = 2 * cameraModel.dist[2] * x * y +
                        cameraModel.dist[3] * (r2 + 2 * x * x);
        double deltaY = cameraModel.dist[2] * (r2 + 2 * y * y) +
                        2 * cameraModel.dist[3] * x * y;
        x = (x0 - deltaX) * icdist;
        y = (y0 - deltaY) * icdist;
    }

    // Save undistorted pixel coords:
    outPt.x = x * fx + cx;
    outPt.y = y * fy + cy;

    MRPT_END
}

void mrpt::vision::pinhole::projectPoints_with_distortion(
    const std::vector<mrpt::math::TPoint3D>& P,
    const mrpt::img::TCamera& params,
    const mrpt::poses::CPose3DQuat& cameraPose,
    std::vector<mrpt::img::TPixelCoordf>& pixels, bool accept_points_behind)
{
    MRPT_START

    pixels.resize(P.size());
    std::vector<mrpt::math::TPoint3D>::const_iterator itPoints;
    std::vector<mrpt::img::TPixelCoordf>::iterator itPixels;
    unsigned int k = 0;
    for (itPoints = P.begin(), itPixels = pixels.begin(); itPoints != P.end();
         ++itPoints, ++itPixels, ++k)
    {
        // Change the reference system to that wrt the camera
        TPoint3D nP;
        cameraPose.inverseComposePoint(
            itPoints->x, itPoints->y, itPoints->z, nP.x, nP.y, nP.z);

        // Pinhole model:
        const double x = nP.x / nP.z;
        const double y = nP.y / nP.z;

        // Radial distortion:
        const double r2 = square(x) + square(y);
        const double r4 = square(r2);
        const double r6 = r2 * r4;
        const double A =
            1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
        const double B = 2 * x * y;
        if (A > 0 && (accept_points_behind || nP.z > 0))
        {
            itPixels->x = params.cx() +
                          params.fx() * (x * A + params.dist[2] * B +
                                         params.dist[3] * (r2 + 2 * square(x)));
            itPixels->y = params.cy() +
                          params.fy() * (y * A + params.dist[3] * B +
                                         params.dist[2] * (r2 + 2 * square(y)));
        }
        else
        {
            itPixels->x = -1.0;
            itPixels->y = -1.0;
        }
    }  // end-for

    MRPT_END
}

/* -------------------------------------------------------
                projectPoint_with_distortion
   ------------------------------------------------------- */
void mrpt::vision::pinhole::projectPoint_with_distortion(
    const mrpt::math::TPoint3D& P, const mrpt::img::TCamera& params,
    mrpt::img::TPixelCoordf& pixel, bool accept_points_behind)
{
    MRPT_UNUSED_PARAM(accept_points_behind);
    // Pinhole model:
    const double x = P.x / P.z;
    const double y = P.y / P.z;

    // Radial distortion:
    const double r2 = square(x) + square(y);
    const double r4 = square(r2);
    const double r6 = r2 * r4;

    pixel.x = params.cx() +
              params.fx() * (x * (1 + params.dist[0] * r2 +
                                  params.dist[1] * r4 + params.dist[4] * r6) +
                             2 * params.dist[2] * x * y +
                             params.dist[3] * (r2 + 2 * square(x)));
    pixel.y = params.cy() +
              params.fy() * (y * (1 + params.dist[0] * r2 +
                                  params.dist[1] * r4 + params.dist[4] * r6) +
                             2 * params.dist[3] * x * y +
                             params.dist[2] * (r2 + 2 * square(y)));
}

/* -------------------------------------------------------
                    undistortPixels
   ------------------------------------------------------- */
// void mrpt::vision::pinhole::undistortPixels(
//  const std::vector<mrpt::img::TPixelCoordf>  &inputPixels,       /*
// distorted
// pixels in image */
//  const mrpt::math::CMatrixDouble33               &intrinsicParams,   /*
// intrinsic
// parameters of the camera */
//  const std::vector<double>                       &distortionParams,  /* k1 k2
// p1
// p2
//*/
//  const unsigned int                              &resX,              /*
// X-resolution
// of
// the
// image
//*/
//  const unsigned int                              &resY,              /*
// Y-resolution
// of
// the
// image
//*/
//  const double                                    &pixelSize,         /* pixel
// size
//(square)*/
//  std::vector<mrpt::img::TPixelCoordf>            &outputPixels       /*
// estimated
// undistorted pixels in image */
//    )
//{
//  MRPT_START
//
//  ASSERT_( distortionParams.size() >= 4 );
//  const double k1 = distortionParams[0];
//  const double k2 = distortionParams[1];
//  const double p1 = distortionParams[2];
//  const double p2 = distortionParams[3];
//
//  const double fx = intrinsicParams(0,0);
//  const double fy = intrinsicParams(1,1);
//  const double cx = intrinsicParams(0,2);
//  const double cy = intrinsicParams(1,2);
//
//  CMatrixFixed<double,43,43> dx, dy;
//
//  // Compute the undistortion params according to Heittilä code.
//  // Generate a regular meshgrid of size 43x43 and distort them
//  std::vector<mrpt::img::TPixelCoordf>                grid;           // The
// 43x43
// grid
// with distorted
//  std::vector<mrpt::img::TPixelCoordf>::iterator  itGrid;
//
//  grid.resize( 43 );
//  unsigned int c;
//  double px, py;
//  for( c = 0, itGrid = grid.begin(); itGrid != grid.end(); ++itGrid )
//  {
//      px = -resX/40 + c*resX/40;
//      for( unsigned int k = 0; k < 43; ++k )
//      {
//          py = -resY/40 + k*resY/40;
//          const double dx = ( px - cx )*pixelSize;
//          const double dy = ( py - cy )*pixelSize;
//
//          const double r2 = dx*dx + dy*dy;
//          const double delta = k1*r2 + k2*r2*r2;
//
//          const double ncx = dx*(1+delta)+2*p1*dx*dy+p2*(r2+2*dx*dx);
//          const double ncy = dy*(1+delta)+p1*(r2+2*dy*dy)+2*p2*dx*dy;
//
//          (*itGrid)->x = ncx/pixelSize + cx;
//          (*itGrid)->y = ncy/pixelSize + cy;
//      }
//  } // end-itGrid
//
//  // DISTORT POINTS
//  dx=(dp(:,1)-Cpx)*Sx/NDX/Asp;
//  dy=(dp(:,2)-Cpy)*Sy/NDY;
//
//  r2=dx.*dx+dy.*dy;
//  delta=Rad1*r2+Rad2*r2.*r2;
//
//  cx=dx.*(1+delta)+2*Tan1*dx.*dy+Tan2*(r2+2*dx.*dx);
//  cy=dy.*(1+delta)+Tan1*(r2+2*dy.*dy)+2*Tan2*dx.*dy;
//
//  p=NDX*Asp*cx/Sx+Cpx;
//  p(:,2)=NDY*cy/Sy+Cpy;
//
//
//  sys=configc(name);
//  NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4);
//  Asp=par(1); Foc=par(2);
//  Cpx=par(3); Cpy=par(4);
//  Rad1=par(5); Rad2=par(6);
//  Tan1=par(7); Tan2=par(8);
//
//  // Generate a meshgrid of points
//  [dx,dy]=meshgrid(-NDX/40:NDX/40:NDX+NDX/40,-NDY/40:NDY/40:NDY+NDY/40);
//  cc=imcorr(name,par,[dx(:) dy(:)]);
//  cx=(cc(:,1)-Cpx)/NDX*Sx/Asp;
//  cy=(cc(:,2)-Cpy)/NDY*Sy;
//
//  r2=cx.*cx+cy.*cy;
//  delta=Rad1*r2+Rad2*r2.*r2;
//
//  Q=1+(4*Rad1*r2+6*Rad2*r2.*r2+8*Tan1*cy+8*Tan2*cx);
//
//  dx=cx-(cx.*delta+2*Tan1*cx.*cy+Tan2*(r2+2*cx.*cx))./Q;
//  dy=cy-(cy.*delta+Tan1*(r2+2*cy.*cy)+2*Tan2*cx.*cy)./Q;
//
//
//  r2=dx.*dx+dy.*dy;
//
//  Tx=[dx.*r2 dx.*r2.*r2 2*dx.*dy r2+2*dx.*dx];
//  Ty=[dy.*r2 dy.*r2.*r2 r2+2*dy.*dy 2*dx.*dy];
//  T=[Tx;Ty];
//  e=[cx-dx;cy-dy];
//  a=pinv(T)*e;
//  par=par(:);
//  a=[par(1:4);a];
//
//  MRPT_END
//}