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 (
size_t 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()) +
218 square(mbOut_vec[6] - scale));
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";