10 #ifndef mrpt_vision_ba_H 11 #define mrpt_vision_ba_H 36 const size_t cur_iter,
const double cur_total_sq_error,
37 const size_t max_iters,
147 std::vector<std::array<double, 2>>& out_residuals,
148 const bool frame_poses_are_inverse,
const bool use_robust_kernel =
true,
149 const double kernel_param = 3.0,
150 std::vector<double>* out_kernel_1st_deriv =
nullptr);
158 std::vector<std::array<double, 2>>& out_residuals,
159 const bool frame_poses_are_inverse,
const bool use_robust_kernel =
true,
160 const double kernel_param = 3.0,
161 std::vector<double>* out_kernel_1st_deriv =
nullptr);
183 const size_t num_fix_frames);
199 const size_t delta_num_vals,
201 const size_t num_fix_points);
mrpt::aligned_containers< TCameraPoseID, mrpt::poses::CPose3D >::map_t TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
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)
For each pose in the vector frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
A complete sequence of observations of features from different camera frames (poses).
mrpt::aligned_containers< mrpt::poses::CPose3D >::vector_t TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
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)
For each pose in the vector frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
double bundle_adj_full(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback=mrpt::vision::TBundleAdjustmentFeedbackFunctor())
Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landm...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
double reprojectionResiduals(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::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)
Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in gen...
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
std::map< TLandmarkID, mrpt::math::TPoint3D > TLandmarkLocationsMap
A list of landmarks (3D points) indexed by unique IDs.
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.
void ba_initial_estimate(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points)
Fills the frames & landmark points maps with an initial gross estimate from the sequence observations...
Structure to hold the parameters of a pinhole camera model.