59 landmark_points.clear();
62 for (TSequenceFeatureObservations::const_iterator it = observations.begin();
63 it != observations.end(); ++it)
68 frame_poses[frame_ID] =
CPose3D(0, 0, 0, 0, 0, 0);
69 landmark_points[feat_ID] =
TPoint3D(0, 0, 1);
86 landmark_points.clear();
88 if (observations.empty())
return;
95 for (TSequenceFeatureObservations::const_iterator it = observations.begin();
96 it != observations.end(); ++it)
105 frame_poses.assign(max_fr_id + 1,
CPose3D(0, 0, 0, 0, 0, 0));
106 landmark_points.assign(max_pt_id + 1,
TPoint3D(0, 0, 1));
115 template <
bool POSES_INVERSE>
118 std::array<double, 2>& out_residual,
119 const TFramePosesVec::value_type& frame,
120 const TLandmarkLocationsVec::value_type& point,
double&
sum,
121 const bool use_robust_kernel,
const double kernel_param,
122 double* out_kernel_1st_deriv)
125 mrpt::vision::pinhole::projectPoint_no_distortion<POSES_INVERSE>(
126 camera_params, frame, point);
129 out_residual[0] = z_meas.
x - z_pred.
x;
130 out_residual[1] = z_meas.
y - z_pred.
y;
132 const double sum_2 =
square(out_residual[0]) +
square(out_residual[1]);
134 if (use_robust_kernel)
137 kernel.param_sq =
square(kernel_param);
138 double kernel_1st_deriv, kernel_2nd_deriv;
140 sum += kernel.eval(sum_2, kernel_1st_deriv, kernel_2nd_deriv);
141 if (out_kernel_1st_deriv) *out_kernel_1st_deriv = kernel_1st_deriv;
159 std::vector<std::array<double, 2>>& out_residuals,
160 const bool frame_poses_are_inverse,
const bool use_robust_kernel,
161 const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
167 const size_t N = observations.size();
168 out_residuals.resize(N);
169 if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
171 for (
size_t i = 0; i < N; i++)
178 TFramePosesMap::const_iterator itF = frame_poses.find(i_f);
179 TLandmarkLocationsMap::const_iterator itP = landmark_points.find(i_p);
180 ASSERTMSG_(itF != frame_poses.end(),
"Frame ID is not in list!");
181 ASSERTMSG_(itP != landmark_points.end(),
"Landmark ID is not in list!");
183 const TFramePosesMap::mapped_type& frame = itF->second;
184 const TLandmarkLocationsMap::mapped_type& point = itP->second;
186 double* ptr_1st_deriv =
187 out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) :
nullptr;
189 if (frame_poses_are_inverse)
190 reprojectionResidualsElement<true>(
191 camera_params, OBS, out_residuals[i], frame, point,
sum,
192 use_robust_kernel, kernel_param, ptr_1st_deriv);
194 reprojectionResidualsElement<false>(
195 camera_params, OBS, out_residuals[i], frame, point,
sum,
196 use_robust_kernel, kernel_param, ptr_1st_deriv);
208 std::vector<std::array<double, 2>>& out_residuals,
209 const bool frame_poses_are_inverse,
const bool use_robust_kernel,
210 const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
216 const size_t N = observations.size();
217 out_residuals.resize(N);
218 if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
220 for (
size_t i = 0; i < N; i++)
229 const TFramePosesVec::value_type& frame = frame_poses[i_f];
230 const TLandmarkLocationsVec::value_type& point = landmark_points[i_p];
232 double* ptr_1st_deriv =
233 out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) :
nullptr;
235 if (frame_poses_are_inverse)
236 reprojectionResidualsElement<true>(
237 camera_params, OBS, out_residuals[i], frame, point,
sum,
238 use_robust_kernel, kernel_param, ptr_1st_deriv);
240 reprojectionResidualsElement<false>(
241 camera_params, OBS, out_residuals[i], frame, point,
sum,
242 use_robust_kernel, kernel_param, ptr_1st_deriv);
252 const vector<std::array<double, 2>>& residual_vec,
258 const size_t num_fix_points,
const vector<double>* kernel_1st_deriv)
262 const size_t N = observations.size();
263 const bool use_robust_kernel = (kernel_1st_deriv !=
nullptr);
265 for (
size_t i = 0; i < N; i++)
276 if (i_f >= num_fix_frames)
278 const size_t frame_id = i_f - num_fix_frames;
282 JtJ.matProductOf_AtA(JACOB.
J_frame);
287 if (!use_robust_kernel)
289 eps_frame[frame_id] += eps_delta;
293 const double rho_1st_der = (*kernel_1st_deriv)[i];
294 auto scaled_eps = eps_delta;
295 scaled_eps *= rho_1st_der;
296 eps_frame[frame_id] += scaled_eps;
300 if (i_p >= num_fix_points)
302 const size_t point_id = i_p - num_fix_points;
306 JtJ.matProductOf_AtA(JACOB.
J_point);
311 if (!use_robust_kernel)
313 eps_point[point_id] += eps_delta;
317 const double rho_1st_der = (*kernel_1st_deriv)[i];
318 auto scaled_eps = eps_delta;
319 scaled_eps *= rho_1st_der;
320 eps_point[point_id] += scaled_eps;
332 const size_t delta_first_idx,
const size_t delta_num_vals,
337 new_frame_poses.resize(frame_poses.size());
339 for (
size_t i = 0; i < num_fix_frames; ++i)
340 new_frame_poses[i] = frame_poses[i];
342 size_t delta_used_vals = 0;
343 const double* delta_val = &delta[delta_first_idx];
345 for (
size_t i = num_fix_frames; i < frame_poses.size(); i++)
347 const CPose3D& old_pose = frame_poses[i];
348 CPose3D& new_pose = new_frame_poses[i];
360 delta_used_vals += 6;
363 ASSERT_(delta_used_vals == delta_num_vals);
368 const size_t delta_first_idx,
const size_t delta_num_vals,
373 new_landmark_points.resize(landmark_points.size());
375 for (
size_t i = 0; i < num_fix_points; ++i)
376 new_landmark_points[i] = landmark_points[i];
378 size_t delta_used_vals = 0;
379 const double* delta_val = &delta[delta_first_idx];
381 for (
size_t i = num_fix_points; i < landmark_points.size(); i++)
383 const TPoint3D& old_point = landmark_points[i];
384 TPoint3D& new_point = new_landmark_points[i];
386 for (
size_t j = 0; j < 3; j++)
387 new_point[j] = old_point[j] + delta_val[j];
391 delta_used_vals += 3;
394 ASSERT_(delta_used_vals == delta_num_vals);