namespace mrpt::vision

Classes for computer vision, detectors, features, etc.

Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2022, Individual contributors, see AUTHORS file See: https://www.mrpt.org/Authors - All rights reserved.

mrpt_vision_grp

bundle_adjuster.h is part of RobotVision.

RobotVision is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation, either version 3 of the License, or any later version.

RobotVision is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU General Public License and the GNU Lesser General Public License along with this program. If not, see http://www.gnu.org/licenses/.

namespace vision {

// namespaces

namespace mrpt::vision::detail;
namespace mrpt::vision::pinhole;
namespace mrpt::vision::pnp;

// typedefs

typedef std::function<void(const size_t cur_iter, const double cur_total_sq_error, const size_t max_iters, const mrpt::vision::TSequenceFeatureObservations&input_observations, const mrpt::vision::TFramePosesVec&current_frame_estimate, const mrpt::vision::TLandmarkLocationsVec&current_landmark_estimate)> TBundleAdjustmentFeedbackFunctor;
typedef std::map<std::string, TImageCalibData> TCalibrationImageList;
typedef void(*)(const TImageStereoCallbackData&d, void*user_data) TSteroCalibCallbackFunctor;
typedef std::vector<TImageStereoCalibData> TCalibrationStereoImageList;
typedef TKeyPoint_templ<mrpt::img::TPixelCoord> TKeyPoint;
typedef TKeyPoint_templ<mrpt::img::TPixelCoordf> TKeyPointf;
typedef TKeyPointList_templ<TKeyPoint> TKeyPointList;
typedef TKeyPointList_templ<TKeyPointf> TKeyPointfList;
typedef std::unique_ptr<CGenericFeatureTracker> CGenericFeatureTrackerAutoPtr;
typedef uint64_t TFeatureID;
typedef uint64_t TLandmarkID;
typedef uint64_t TCameraPoseID;
typedef std::map<TCameraPoseID, mrpt::poses::CPose3D> TFramePosesMap;
typedef std::vector<mrpt::poses::CPose3D> TFramePosesVec;
typedef std::map<TLandmarkID, mrpt::math::TPoint3D> TLandmarkLocationsMap;
typedef std::vector<mrpt::math::TPoint3D> TLandmarkLocationsVec;
typedef std::map<mrpt::vision::TFeatureID, TRelativeFeaturePos> TRelativeFeaturePosMap;
typedef std::vector<mrpt::aligned_std_vector<TResidJacobElement>> TResidualJacobianList;

// enums

enum TDescriptorType;
enum TFeatureTrackStatus;
enum TKeyPointMethod;
enum TListIdx;

// structs

struct CFeatureTracker_KL;
struct CGenericFeatureTracker;

template <int FrameDof, int PointDof, int ObsDim>
struct JacData;

template <typename FEATURE_LIST>
struct KeypointResponseSorter;

struct TFeatureObservation;
struct TImageCalibData;
struct TImageROI;
struct TImageStereoCalibData;
struct TImageStereoCallbackData;

template <typename FEATURE>
struct TKeyPointList_templ;

template <>
struct TKeyPointTraits<TKeyPointf>;

template <typename FEATURE>
struct TKeyPointTraits;

template <>
struct TKeyPointTraits<TKeyPoint>;

template <typename PIXEL_COORD_TYPE>
struct TKeyPoint_templ;

struct TMatchingOptions;
struct TMultiResDescMatchOptions;
struct TMultiResDescOptions;
struct TMultiResMatchingOutput;
struct TROI;
struct TRelativeFeaturePos;
struct TResidJacobElement;

template <
    typename distance_t,
    class metric_t = nanoflann::L2_Simple_Adaptor<uint8_t ,     detail::TSIFTDesc2KDTree_Adaptor<distance_t>, distance_t>
    >
struct TSIFTDescriptorsKDTreeIndex;

template <
    typename distance_t,
    class metric_t = nanoflann::L2_Simple_Adaptor<float ,       detail::TSURFDesc2KDTree_Adaptor<distance_t>, distance_t>
    >
struct TSURFDescriptorsKDTreeIndex;

struct TSequenceFeatureObservations;
struct TStereoCalibParams;
struct TStereoCalibResults;
struct TStereoSystemParams;
struct lm_stat_t;

// classes

class CDifodo;
class CFeature;
class CFeatureExtraction;
class CFeatureLines;
class CFeatureList;

template <typename FEAT>
class CFeatureListKDTree;

class CImagePyramid;
class CMatchedFeatureList;
class CStereoRectifyMap;
class CUndistortMap;
class CVideoFileWriter;

// global functions

double bundle_adj_full(
    const mrpt::vision::TSequenceFeatureObservations& observations,
    const mrpt::img::TCamera& camera_params,
    mrpt::vision::TFramePosesVec& frame_poses,
    mrpt::vision::TLandmarkLocationsVec& landmark_points,
    const mrpt::containers::yaml& extra_params = {},
    const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback = mrpt::vision::TBundleAdjustmentFeedbackFunctor()
    );

void ba_initial_estimate(
    const mrpt::vision::TSequenceFeatureObservations& observations,
    const mrpt::img::TCamera& camera_params,
    mrpt::vision::TFramePosesVec& frame_poses,
    mrpt::vision::TLandmarkLocationsVec& landmark_points
    );

void ba_initial_estimate(
    const mrpt::vision::TSequenceFeatureObservations& observations,
    const mrpt::img::TCamera& camera_params,
    mrpt::vision::TFramePosesMap& frame_poses,
    mrpt::vision::TLandmarkLocationsMap& landmark_points
    );

double reprojectionResiduals(
    const mrpt::vision::TSequenceFeatureObservations& observations,
    const mrpt::img::TCamera& camera_params,
    const mrpt::vision::TFramePosesVec& frame_poses,
    const mrpt::vision::TLandmarkLocationsVec& landmark_points,
    std::vector<std::array<double, 2>>& out_residuals,
    const bool frame_poses_are_inverse,
    const bool use_robust_kernel = true,
    const double kernel_param = 3.0,
    std::vector<double>* out_kernel_1st_deriv = nullptr
    );

double reprojectionResiduals(
    const mrpt::vision::TSequenceFeatureObservations& observations,
    const mrpt::img::TCamera& camera_params,
    const mrpt::vision::TFramePosesMap& frame_poses,
    const mrpt::vision::TLandmarkLocationsMap& landmark_points,
    std::vector<std::array<double, 2>>& out_residuals,
    const bool frame_poses_are_inverse,
    const bool use_robust_kernel = true,
    const double kernel_param = 3.0,
    std::vector<double>* out_kernel_1st_deriv = nullptr
    );

void add_se3_deltas_to_frames(
    const mrpt::vision::TFramePosesVec& frame_poses,
    const mrpt::math::CVectorDouble& delta,
    const size_t delta_first_idx,
    const size_t delta_num_vals,
    mrpt::vision::TFramePosesVec& new_frame_poses,
    const size_t num_fix_frames
    );

void add_3d_deltas_to_points(
    const mrpt::vision::TLandmarkLocationsVec& landmark_points,
    const mrpt::math::CVectorDouble& delta,
    const size_t delta_first_idx,
    const size_t delta_num_vals,
    mrpt::vision::TLandmarkLocationsVec& new_landmark_points,
    const size_t num_fix_points
    );

bool checkerBoardCameraCalibration(
    TCalibrationImageList& images,
    unsigned int check_size_x,
    unsigned int check_size_y,
    double check_squares_length_X_meters,
    double check_squares_length_Y_meters,
    mrpt::img::TCamera& out_camera_params,
    bool normalize_image = true,
    double* out_MSE = nullptr,
    bool skipDrawDetectedImgs = false,
    bool useScaramuzzaAlternativeDetector = false
    );

bool checkerBoardCameraCalibration(
    TCalibrationImageList& images,
    unsigned int check_size_x,
    unsigned int check_size_y,
    double check_squares_length_X_meters,
    double check_squares_length_Y_meters,
    mrpt::math::CMatrixDouble33& intrinsicParams,
    std::vector<double>& distortionParams,
    bool normalize_image = true,
    double* out_MSE = nullptr,
    bool skipDrawDetectedImgs = false,
    bool useScaramuzzaAlternativeDetector = false
    );

bool findChessboardCorners(
    const mrpt::img::CImage& img,
    std::vector<mrpt::img::TPixelCoordf>& cornerCoords,
    unsigned int check_size_x,
    unsigned int check_size_y,
    bool normalize_image = true,
    bool useScaramuzzaMethod = false
    );

void findMultipleChessboardsCorners(
    const mrpt::img::CImage& img,
    std::vector<std::vector<mrpt::img::TPixelCoordf>>& cornerCoords,
    unsigned int check_size_x,
    unsigned int check_size_y
    );

bool checkerBoardStereoCalibration(TCalibrationStereoImageList& images, const TStereoCalibParams& params, TStereoCalibResults& out_results);

template <class DESCRIPTOR_KDTREE>
size_t find_descriptor_pairings(
    std::vector<std::vector<size_t>>* pairings_1_to_multi_2,
    std::vector<std::pair<size_t, size_t>>* pairings_1_to_2,
    const CFeatureList& feats_img1,
    const DESCRIPTOR_KDTREE& feats_img2_kdtree,
    const mrpt::vision::TDescriptorType descriptor = descSIFT,
    const size_t max_neighbors = 4,
    const double max_relative_distance = 1.2,
    const typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType max_distance = std::numeric_limits<typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType>::max()
    );

void openCV_cross_correlation(
    const mrpt::img::CImage& img,
    const mrpt::img::CImage& patch_img,
    size_t& x_max,
    size_t& y_max,
    double& max_val,
    int x_search_ini = -1,
    int y_search_ini = -1,
    int x_search_size = -1,
    int y_search_size = -1
    );

mrpt::math::TPoint3D pixelTo3D(const mrpt::img::TPixelCoordf& xy, const mrpt::math::CMatrixDouble33& A);

mrpt::math::CMatrixDouble33 buildIntrinsicParamsMatrix(
    const double focalLengthX,
    const double focalLengthY,
    const double centerX,
    const double centerY
    );

mrpt::math::CMatrixDouble33 defaultIntrinsicParamsMatrix(
    unsigned int camIndex = 0,
    unsigned int resolutionX = 320,
    unsigned int resolutionY = 240
    );

double computeMsd(const mrpt::tfest::TMatchingPairList& list, const poses::CPose3D& Rt);
void cloudsToMatchedList(const mrpt::obs::CObservationVisualLandmarks& cloud1, const mrpt::obs::CObservationVisualLandmarks& cloud2, mrpt::tfest::TMatchingPairList& outList);
float computeMainOrientation(const mrpt::img::CImage& image, unsigned int x, unsigned int y);
void normalizeImage(const mrpt::img::CImage& image, mrpt::img::CImage& nimage);

size_t matchFeatures(
    const CFeatureList& list1,
    const CFeatureList& list2,
    CMatchedFeatureList& matches,
    const TMatchingOptions& options = TMatchingOptions(),
    const TStereoSystemParams& params = TStereoSystemParams()
    );

void generateMask(const CMatchedFeatureList& mList, mrpt::math::CMatrixBool& mask1, mrpt::math::CMatrixBool& mask2, int wSize = 10);
double computeSAD(const mrpt::img::CImage& patch1, const mrpt::img::CImage& patch2);
void addFeaturesToImage(const mrpt::img::CImage& inImg, const CFeatureList& theList, mrpt::img::CImage& outImg);

void projectMatchedFeatures(
    const CMatchedFeatureList& matches,
    const mrpt::img::TStereoCamera& stereo_camera,
    std::vector<mrpt::math::TPoint3D>& out_points
    );

void projectMatchedFeatures(
    const CFeatureList& leftList,
    const CFeatureList& rightList,
    std::vector<mrpt::math::TPoint3D>& vP3D,
    const TStereoSystemParams& params = TStereoSystemParams()
    );

void projectMatchedFeature(const CFeature& leftFeat, const CFeature& rightFeat, mrpt::math::TPoint3D& p3D, const TStereoSystemParams& params = TStereoSystemParams());
void projectMatchedFeatures(CMatchedFeatureList& mfList, const TStereoSystemParams& param, mrpt::maps::CLandmarksMap& landmarks);
void projectMatchedFeatures(CFeatureList& leftList, CFeatureList& rightList, const TStereoSystemParams& param, mrpt::maps::CLandmarksMap& landmarks);
void StereoObs2BRObs(const mrpt::obs::CObservationStereoImages& inObs, const std::vector<double>& sg, mrpt::obs::CObservationBearingRange& outObs);

void StereoObs2BRObs(
    const CMatchedFeatureList& inMatches,
    const mrpt::math::CMatrixDouble33& intrinsicParams,
    double baseline,
    const mrpt::poses::CPose3D& sensorPose,
    const std::vector<double>& sg,
    mrpt::obs::CObservationBearingRange& outObs
    );

void StereoObs2BRObs(const mrpt::obs::CObservationVisualLandmarks& inObs, mrpt::obs::CObservationBearingRange& outObs);

void computeStereoRectificationMaps(
    const mrpt::img::TCamera& cam1,
    const mrpt::img::TCamera& cam2,
    const mrpt::poses::CPose3D& rightCameraPose,
    void* outMap1x,
    void* outMap1y,
    void* outMap2x,
    void* outMap2y
    );

double recompute_errors_and_Jacobians(
    const lm_stat_t& lm_stat,
    TResidualJacobianList& res_jac,
    bool use_robust_kernel,
    double kernel_param
    );

void build_linear_system(
    const TResidualJacobianList& res_jac,
    const std::vector<size_t>& var_indxs,
    mrpt::math::CVectorDynamic<double>& minus_g,
    mrpt::math::CMatrixDouble& H
    );

void add_lm_increment(
    const mrpt::math::CVectorDynamic<double>& eps,
    const std::vector<size_t>& var_indxs,
    lm_stat_t& new_lm_stat
    );

template <bool POSES_ARE_INVERSE>
void frameJac(
    const mrpt::img::TCamera& camera_params,
    const mrpt::poses::CPose3D& cam_pose,
    const mrpt::math::TPoint3D& landmark_global,
    mrpt::math::CMatrixFixed<double, 2, 6>& out_J
    );

template <bool POSES_ARE_INVERSE>
void pointJac(
    const mrpt::img::TCamera& camera_params,
    const mrpt::poses::CPose3D& cam_pose,
    const mrpt::math::TPoint3D& landmark_global,
    mrpt::math::CMatrixFixed<double, 2, 3>& out_J
    );

template <bool POSES_ARE_INVERSE>
void ba_compute_Jacobians(
    const TFramePosesVec& frame_poses,
    const TLandmarkLocationsVec& landmark_points,
    const mrpt::img::TCamera& camera_params,
    std::vector<JacData<6, 3, 2>>& jac_data_vec,
    const size_t num_fix_frames,
    const size_t num_fix_points
    );

void ba_build_gradient_Hessians(
    const TSequenceFeatureObservations& observations,
    const std::vector<std::array<double, 2>>& residual_vec,
    const std::vector<JacData<6, 3, 2>>& jac_data_vec,
    std::vector<mrpt::math::CMatrixFixed<double, 6, 6>>& U,
    std::vector<CVectorFixedDouble<6>>& eps_frame,
    std::vector<mrpt::math::CMatrixFixed<double, 3, 3>>& V,
    std::vector<CVectorFixedDouble<3>>& eps_point,
    const size_t num_fix_frames,
    const size_t num_fix_points,
    const vector<double>* kernel_1st_deriv
    );

} // namespace vision

Typedefs

typedef std::function<void(const size_t cur_iter, const double cur_total_sq_error, const size_t max_iters, const mrpt::vision::TSequenceFeatureObservations&input_observations, const mrpt::vision::TFramePosesVec&current_frame_estimate, const mrpt::vision::TLandmarkLocationsVec&current_landmark_estimate)> TBundleAdjustmentFeedbackFunctor

A functor type for BA methods.

See also:

bundle_adj_full

Global Functions

template <bool POSES_ARE_INVERSE>
void frameJac(
    const mrpt::img::TCamera& camera_params,
    const mrpt::poses::CPose3D& cam_pose,
    const mrpt::math::TPoint3D& landmark_global,
    mrpt::math::CMatrixFixed<double, 2, 6>& out_J
    )

The projective camera 2x6 Jacobian \(\frac{\partial h(f,p)}{\partial p}\) (wrt the 6D camera pose)

Jacobians as described in https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty (Appendix A)

template <bool POSES_ARE_INVERSE>
void pointJac(
    const mrpt::img::TCamera& camera_params,
    const mrpt::poses::CPose3D& cam_pose,
    const mrpt::math::TPoint3D& landmark_global,
    mrpt::math::CMatrixFixed<double, 2, 3>& out_J
    )

Jacobians wrt the point.

Jacobians as described in https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty (Appendix A)

void ba_build_gradient_Hessians(
    const TSequenceFeatureObservations& observations,
    const std::vector<std::array<double, 2>>& residual_vec,
    const std::vector<JacData<6, 3, 2>>& jac_data_vec,
    std::vector<mrpt::math::CMatrixFixed<double, 6, 6>>& U,
    std::vector<CVectorFixedDouble<6>>& eps_frame,
    std::vector<mrpt::math::CMatrixFixed<double, 3, 3>>& V,
    std::vector<CVectorFixedDouble<3>>& eps_point,
    const size_t num_fix_frames,
    const size_t num_fix_points,
    const vector<double>* kernel_1st_deriv
    )

Construct the BA linear system.

Set kernel_1st_deriv!=nullptr if using robust kernel.