pinhole.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/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::vision::pinhole;
using namespace mrpt::utils;
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::utils::TPixelCoordf> &projectedPoints,
        bool accept_points_behind
        )
{
        MRPT_START
#if MRPT_HAS_OPENCV
 
        ASSERT_(size(intrinsicParams,1)==3);
        ASSERT_(size(intrinsicParams,2)==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
        CVectorDouble proj_matrix(9);
        proj_matrix[0] = intrinsicParams.get_unsafe(0,0);
        proj_matrix[4] = intrinsicParams.get_unsafe(1,1);
        proj_matrix[2] = intrinsicParams.get_unsafe(0,2);
        proj_matrix[5] = intrinsicParams.get_unsafe(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::utils::TPixelCoordf>  &in_dist_pixels,
        std::vector<mrpt::utils::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.static_size)
        for (size_t i=0;i<cam.dist.static_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::utils::TPixelCoordf>  &in_dist_pixels,
        std::vector<mrpt::utils::TPixelCoordf> &out_pixels,
        const mrpt::utils::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::utils::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::utils::TCamera  &params,
        const mrpt::poses::CPose3DQuat &cameraPose,
        std::vector<mrpt::utils::TPixelCoordf>  &pixels,
        bool accept_points_behind
        )
{
        MRPT_START
 
        pixels.resize( P.size() );
        std::vector<mrpt::math::TPoint3D>::const_iterator itPoints;
        std::vector<mrpt::utils::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::utils::TCamera  &params,
        mrpt::utils::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::utils::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::utils::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);
//
//      CMatrixFixedNumeric<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::utils::TPixelCoordf>                          grid;                   // The 43x43 grid with distorted
//      std::vector<mrpt::utils::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
//}