59 landmark_points.clear();
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);
87 landmark_points.clear();
89 if (observations.empty())
return;
97 it != observations.end(); ++it)
106 frame_poses.assign(max_fr_id + 1,
CPose3D(0, 0, 0, 0, 0, 0));
107 landmark_points.assign(max_pt_id + 1,
TPoint3D(0, 0, 1));
116 template <
bool POSES_INVERSE>
119 std::array<double, 2>& out_residual,
120 const TFramePosesVec::value_type& frame,
121 const TLandmarkLocationsVec::value_type& point,
double&
sum,
122 const bool use_robust_kernel,
const double kernel_param,
123 double* out_kernel_1st_deriv)
126 mrpt::vision::pinhole::projectPoint_no_distortion<POSES_INVERSE>(
127 camera_params, frame, point);
130 out_residual[0] = z_meas.
x - z_pred.
x;
131 out_residual[1] = z_meas.
y - z_pred.
y;
133 const double sum_2 =
square(out_residual[0]) +
square(out_residual[1]);
135 if (use_robust_kernel)
138 kernel.param_sq =
square(kernel_param);
139 double kernel_1st_deriv, kernel_2nd_deriv;
141 sum += kernel.eval(sum_2, kernel_1st_deriv, kernel_2nd_deriv);
142 if (out_kernel_1st_deriv) *out_kernel_1st_deriv = kernel_1st_deriv;
160 std::vector<std::array<double, 2>>& out_residuals,
161 const bool frame_poses_are_inverse,
const bool use_robust_kernel,
162 const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
168 const size_t N = observations.size();
169 out_residuals.resize(N);
170 if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
172 for (
size_t i = 0; i < N; i++)
181 ASSERTMSG_(itF != frame_poses.end(),
"Frame ID is not in list!");
182 ASSERTMSG_(itP != landmark_points.end(),
"Landmark ID is not in list!");
184 const TFramePosesMap::mapped_type& frame = itF->second;
185 const TLandmarkLocationsMap::mapped_type& point = itP->second;
187 double* ptr_1st_deriv =
188 out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) :
nullptr;
190 if (frame_poses_are_inverse)
191 reprojectionResidualsElement<true>(
192 camera_params, OBS, out_residuals[i], frame, point,
sum,
193 use_robust_kernel, kernel_param, ptr_1st_deriv);
195 reprojectionResidualsElement<false>(
196 camera_params, OBS, out_residuals[i], frame, point,
sum,
197 use_robust_kernel, kernel_param, ptr_1st_deriv);
209 std::vector<std::array<double, 2>>& out_residuals,
210 const bool frame_poses_are_inverse,
const bool use_robust_kernel,
211 const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
217 const size_t N = observations.size();
218 out_residuals.resize(N);
219 if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
221 for (
size_t i = 0; i < N; i++)
230 const TFramePosesVec::value_type& frame = frame_poses[i_f];
231 const TLandmarkLocationsVec::value_type& point = landmark_points[i_p];
233 double* ptr_1st_deriv =
234 out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) :
nullptr;
236 if (frame_poses_are_inverse)
237 reprojectionResidualsElement<true>(
238 camera_params, OBS, out_residuals[i], frame, point,
sum,
239 use_robust_kernel, kernel_param, ptr_1st_deriv);
241 reprojectionResidualsElement<false>(
242 camera_params, OBS, out_residuals[i], frame, point,
sum,
243 use_robust_kernel, kernel_param, ptr_1st_deriv);
253 const vector<std::array<double, 2>>& residual_vec,
259 const size_t num_fix_frames,
const size_t num_fix_points,
260 const vector<double>* kernel_1st_deriv)
264 const size_t N = observations.size();
265 const bool use_robust_kernel = (kernel_1st_deriv !=
nullptr);
267 for (
size_t i = 0; i < N; i++)
274 const Eigen::Matrix<double, 2, 1> RESID(&residual_vec[i][0]);
278 if (i_f >= num_fix_frames)
280 const size_t frame_id = i_f - num_fix_frames;
284 JtJ.multiply_AtA(JACOB.
J_frame);
289 if (!use_robust_kernel)
291 eps_frame[frame_id] += eps_delta;
295 const double rho_1st_der = (*kernel_1st_deriv)[i];
296 eps_frame[frame_id] += eps_delta * rho_1st_der;
300 if (i_p >= num_fix_points)
302 const size_t point_id = i_p - num_fix_points;
306 JtJ.multiply_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 eps_point[point_id] += eps_delta * rho_1st_der;
330 const size_t delta_first_idx,
const size_t delta_num_vals,
335 new_frame_poses.resize(frame_poses.size());
337 for (
size_t i = 0; i < num_fix_frames; ++i)
338 new_frame_poses[i] = frame_poses[i];
340 size_t delta_used_vals = 0;
341 const double* delta_val = &delta[delta_first_idx];
343 for (
size_t i = num_fix_frames; i < frame_poses.size(); i++)
345 const CPose3D& old_pose = frame_poses[i];
346 CPose3D& new_pose = new_frame_poses[i];
350 const CPose3D incrPose = CPose3D::exp(incr);
358 delta_used_vals += 6;
361 ASSERT_(delta_used_vals == delta_num_vals);
366 const size_t delta_first_idx,
const size_t delta_num_vals,
371 new_landmark_points.resize(landmark_points.size());
373 for (
size_t i = 0; i < num_fix_points; ++i)
374 new_landmark_points[i] = landmark_points[i];
376 size_t delta_used_vals = 0;
377 const double* delta_val = &delta[delta_first_idx];
379 for (
size_t i = num_fix_points; i < landmark_points.size(); i++)
381 const TPoint3D& old_point = landmark_points[i];
382 TPoint3D& new_point = new_landmark_points[i];
384 for (
size_t j = 0; j < 3; j++)
385 new_point[j] = old_point[j] + delta_val[j];
389 delta_used_vals += 3;
392 ASSERT_(delta_used_vals == delta_num_vals);