38 const size_t N = in_correspondences.size();
45 th[0] = th[1] = th[2] =
params.ransac_threshold_lin;
47 th[3] = th[4] = th[5] =
params.ransac_threshold_ang;
49 th[6] =
params.ransac_threshold_scale;
55 std::numeric_limits<double>::max();
62 N *
params.ransac_maxSetSizePct);
65 params.ransac_nmaxSimulations;
69 "Minimum number of points to be considered a good set is < Minimum " 70 "number of points to fit the model");
75 for (
size_t iterations = 0; iterations < max_it; iterations++)
80 std::vector<uint32_t> rub, mbSet, cSet;
87 for (
size_t i = 0; mbInliers.size() <
n && i < N; i++)
89 const size_t idx = mbSet[i];
92 if (
params.user_individual_compat_callback)
95 pm.
idx_this = in_correspondences[idx].this_idx;
96 pm.
idx_other = in_correspondences[idx].other_idx;
97 if (!
params.user_individual_compat_callback(pm))
101 mbInliers.push_back(in_correspondences[idx]);
109 std::cerr <<
"[tfest::se3_l2_robust] Iter " << iterations
110 <<
": It was not possible to find the min no of " 111 "(compatible) matching pairs.\n";
117 mbInliers, mbOutQuat,
scale,
params.forceScaleToUnity);
120 std::cerr <<
"[tfest::se3_l2_robust] tfest::se3_l2() returned " 121 "false for tentative subset during RANSAC " 129 mbOut_vec[0] = mbOut.
x();
130 mbOut_vec[1] = mbOut.
y();
131 mbOut_vec[2] = mbOut.z();
133 mbOut_vec[3] = mbOut.
yaw();
134 mbOut_vec[4] = mbOut.
pitch();
135 mbOut_vec[5] = mbOut.
roll();
137 mbOut_vec[6] =
scale;
140 for (
size_t k =
n; k < N; k++)
142 const size_t idx = mbSet[k];
145 if (
params.user_individual_compat_callback)
148 pm.
idx_this = in_correspondences[idx].this_idx;
149 pm.
idx_other = in_correspondences[idx].other_idx;
150 if (!
params.user_individual_compat_callback(pm))
156 mbInliers.push_back(in_correspondences[idx]);
158 mbInliers, csOutQuat,
scale,
params.forceScaleToUnity);
159 mbInliers.erase(mbInliers.end() - 1);
163 std::cerr <<
"[tfest::se3_l2_robust] tfest::se3_l2() returned " 164 "false for tentative subset during RANSAC " 172 if (fabs(mbOut_vec[0] - csOut.
x()) < th[0] &&
173 fabs(mbOut_vec[1] - csOut.
y()) < th[1] &&
174 fabs(mbOut_vec[2] - csOut.z()) < th[2] &&
175 fabs(mbOut_vec[3] - csOut.
yaw()) < th[3] &&
176 fabs(mbOut_vec[4] - csOut.
pitch()) < th[4] &&
177 fabs(mbOut_vec[5] - csOut.
roll()) < th[5] &&
178 fabs(mbOut_vec[6] -
scale) < th[6])
191 if (cSet.size() >= d)
195 cSetInliers.resize(cSet.size());
196 for (
unsigned int m = 0; m < cSet.size(); m++)
197 cSetInliers[m] = in_correspondences[cSet[m]];
202 cSetInliers, cIOutQuat,
scale,
203 params.forceScaleToUnity);
206 "tfest::se3_l2() returned false for tentative subset during " 207 "RANSAC iteration!");
211 const double err = std::sqrt(
212 square(mbOut_vec[0] - cIOut.
x()) +
213 square(mbOut_vec[1] - cIOut.
y()) +
214 square(mbOut_vec[2] - cIOut.z()) +
221 if (err < min_err && cSet.size() >= max_size)
224 max_size = cSet.size();
225 results.transformation = cIOutQuat;
241 std::cerr <<
"[se3_l2_robust] maximum size is == 0!\n";
A namespace of pseudo-random numbers generators of diferent distributions.
void permuteVector(const VEC &in_vector, VEC &out_result)
Returns a random permutation of a vector: all the elements of the input vector are in the output but ...
A compile-time fixed-size numeric matrix container.
Template for column vectors of dynamic size, compatible with Eigen.
Parameters for se3_l2_robust().
GLenum GLenum GLenum GLenum GLenum scale
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
T square(const T x)
Inline function for the square of a number.
This base provides a set of functions for maths stuff.
map< string, CVectorDouble > results
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
double x() const
Common members of all points & poses classes.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
void linspace(T first, T last, size_t count, VECTOR &out_vector)
Generates an equidistant sequence of numbers given the first one, the last one and the desired number...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
bool se3_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
bool se3_l2_robust(const mrpt::tfest::TMatchingPairList &in_correspondences, const TSE3RobustParams &in_params, TSE3RobustResult &out_results)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
For each individual-compatibility (IC) test, the indices of the candidate match between elements in b...
Output placeholder for se3_l2_robust()
int round(const T value)
Returns the closer integer (int) to x.