pinhole.h

Go to the documentation of this file.

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

#pragma once

#include <mrpt/img/TCamera.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/vision/utils.h>

namespace mrpt
{
namespace vision
{
/** Functions related to pinhole camera models, point projections, etc. \ingroup
 * mrpt_vision_grp */
namespace pinhole
{
/** \addtogroup mrpt_vision_grp
 * @{ */

/** Project a set of 3D points into a camera at an arbitrary 6D pose using its
 * calibration matrix (undistorted projection model)
 * \param in_points_3D [IN] The list of 3D points in world coordinates (meters)
 * to project.
 * \param cameraPose [IN] The pose of the camera in the world.
 * \param intrinsicParams [IN] The 3x3 calibration matrix. See
 * https://www.mrpt.org/Camera_Parameters
 * \param projectedPoints [OUT] The list of image coordinates (in pixels) for
 * the projected points. At output this list is resized to the same number of
 * input points.
 * \param accept_points_behind [IN] See the note below.
 *
 * \note Points "behind" the camera (which couldn't be physically seen in the
 * real world) are marked with pixel coordinates (-1,-1) to detect them as
 * invalid, unless accept_points_behind is true. In that case they'll be
 * projected normally.
 *
 * \sa projectPoints_with_distortion, projectPoint_no_distortion
 */
void projectPoints_no_distortion(
    const std::vector<mrpt::math::TPoint3D>& in_points_3D,
    const mrpt::poses::CPose3D& cameraPose,
    const mrpt::math::CMatrixDouble33& intrinsicParams,
    std::vector<mrpt::img::TPixelCoordf>& projectedPoints,
    bool accept_points_behind = false);

/** Project a single 3D point with global coordinates P into a camera at pose F,
 *without distortion parameters.
 *  The template argument INVERSE_CAM_POSE is related on how the camera pose
 *"F" is stored:
 *      - INVERSE_CAM_POSE:false -> The local coordinates of the feature wrt the
 *camera F are: \f$ P \ominus F \f$
 *      - INVERSE_CAM_POSE:true  -> The local coordinates of the feature wrt the
 *camera F are: \f$ F \oplus P \f$
 */
template <bool INVERSE_CAM_POSE>
inline mrpt::img::TPixelCoordf projectPoint_no_distortion(
    const mrpt::img::TCamera& cam_params, const mrpt::poses::CPose3D& F,
    const mrpt::math::TPoint3D& P)
{
    double x, y, z;  // wrt cam (local coords)
    if (INVERSE_CAM_POSE)
        F.composePoint(P.x, P.y, P.z, x, y, z);
    else
        F.inverseComposePoint(P.x, P.y, P.z, x, y, z);
    ASSERT_(z != 0);
    // Pinhole model:
    return mrpt::img::TPixelCoordf(
        cam_params.cx() + cam_params.fx() * x / z,
        cam_params.cy() + cam_params.fy() * y / z);
}

//! \overload
template <typename POINT>
inline void projectPoint_no_distortion(
    const POINT& in_point_wrt_cam, const mrpt::img::TCamera& cam_params,
    mrpt::img::TPixelCoordf& out_projectedPoints)
{
    ASSERT_(in_point_wrt_cam.z != 0);
    // Pinhole model:
    out_projectedPoints.x = cam_params.cx() + cam_params.fx() *
                                                  in_point_wrt_cam.x /
                                                  in_point_wrt_cam.z;
    out_projectedPoints.y = cam_params.cy() + cam_params.fy() *
                                                  in_point_wrt_cam.y /
                                                  in_point_wrt_cam.z;
}

/** Project a set of 3D points into a camera at an arbitrary 6D pose using its
 * calibration matrix and distortion parameters (radial and tangential
 * distortions projection model)
 * \param in_points_3D [IN] The list of 3D points in world coordinates (meters)
 * to project.
 * \param cameraPose [IN] The pose of the camera in the world.
 * \param intrinsicParams [IN] The 3x3 calibration matrix. See
 * https://www.mrpt.org/Camera_Parameters
 * \param distortionParams [IN] The 4-length vector with the distortion
 * parameters [k1 k2 p1 p2]. See https://www.mrpt.org/Camera_Parameters
 * \param projectedPoints [OUT] The list of image coordinates (in pixels) for
 * the projected points. At output this list is resized to the same number of
 * input points.
 * \param accept_points_behind [IN] See the note below.
 *
 * \note Points "behind" the camera (which couldn't be physically seen in the
 * real world) are marked with pixel coordinates (-1,-1) to detect them as
 * invalid, unless accept_points_behind is true. In that case they'll be
 * projected normally.
 *
 * \sa projectPoint_with_distortion, projectPoints_no_distortion
 */
void 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 = false);

/** Project one 3D point into a camera using its calibration matrix and
 * distortion parameters (radial and tangential distortions projection model)
 * \param in_point_wrt_cam [IN] The 3D point wrt the camera focus, with
 * +Z=optical axis, +X=righthand in the image plane, +Y=downward in the image
 * plane.
 * \param in_cam_params [IN] The camera parameters. See
 * https://www.mrpt.org/Camera_Parameters
 * \param out_projectedPoints [OUT] The projected point, in pixel units.
 * \param accept_points_behind [IN] See the note below.
 *
 * \note Points "behind" the camera (which couldn't be physically seen in the
 * real world) are marked with pixel coordinates (-1,-1) to detect them as
 * invalid, unless accept_points_behind is true. In that case they'll be
 * projected normally.
 *
 * \sa projectPoints_with_distortion
 */
void projectPoint_with_distortion(
    const mrpt::math::TPoint3D& in_point_wrt_cam,
    const mrpt::img::TCamera& in_cam_params,
    mrpt::img::TPixelCoordf& out_projectedPoints,
    bool accept_points_behind = false);

//! \overload
void 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 = false);

/** Undistort a list of points given by their pixel coordinates, provided the
 * camera matrix and distortion coefficients.
 * \param srcDistortedPixels [IN] The pixel coordinates as in the distorted
 * image.
 * \param dstUndistortedPixels [OUT] The computed pixel coordinates without
 * distortion.
 * \param intrinsicParams [IN] The 3x3 calibration matrix. See
 * https://www.mrpt.org/Camera_Parameters
 * \param distortionParams [IN] The 4-length vector with the distortion
 * parameters [k1 k2 p1 p2]. See https://www.mrpt.org/Camera_Parameters
 * \sa undistort_point
 */
void undistort_points(
    const std::vector<mrpt::img::TPixelCoordf>& srcDistortedPixels,
    std::vector<mrpt::img::TPixelCoordf>& dstUndistortedPixels,
    const mrpt::math::CMatrixDouble33& intrinsicParams,
    const std::vector<double>& distortionParams);

/** Undistort a list of points given by their pixel coordinates, provided the
 * camera matrix and distortion coefficients.
 * \param srcDistortedPixels [IN] The pixel coordinates as in the distorted
 * image.
 * \param dstUndistortedPixels [OUT] The computed pixel coordinates without
 * distortion.
 * \param cameraModel [IN] The camera parameters.
 * \sa undistort_point
 */
void undistort_points(
    const std::vector<mrpt::img::TPixelCoordf>& srcDistortedPixels,
    std::vector<mrpt::img::TPixelCoordf>& dstUndistortedPixels,
    const mrpt::img::TCamera& cameraModel);

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

/** @} */  // end of grouping
}  // namespace pinhole
}  // namespace vision
}  // namespace mrpt