namespace mrpt::vision
Classes for computer vision, detectors, features, etc.
Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2023, 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¤t_frame_estimate, const mrpt::vision::TLandmarkLocationsVec¤t_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, size_t delta_first_idx, size_t delta_num_vals, mrpt::vision::TFramePosesVec& new_frame_poses, size_t num_fix_frames ); void add_3d_deltas_to_points( const mrpt::vision::TLandmarkLocationsVec& landmark_points, const mrpt::math::CVectorDouble& delta, size_t delta_first_idx, size_t delta_num_vals, mrpt::vision::TLandmarkLocationsVec& new_landmark_points, 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 registerAllClasses_mrpt_vision(); 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 ); 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, size_t num_fix_frames, 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, size_t num_fix_frames, 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¤t_frame_estimate, const mrpt::vision::TLandmarkLocationsVec¤t_landmark_estimate)> TBundleAdjustmentFeedbackFunctor
A functor type for BA methods.
See also:
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, size_t num_fix_frames, 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.