Main MRPT website > C++ reference
MRPT logo
srba/include/mrpt/srba/impl/jacobians.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
12 #include <mrpt/math/jacobians.h>
13 #include <mrpt/poses/SE_traits.h>
15 
16 namespace mrpt { namespace srba {
17 
18 #define SRBA_USE_NUMERIC_JACOBIANS 0
19 #define SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS 0
20 
21 #define SRBA_COMPUTE_NUMERIC_JACOBIANS (SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS || SRBA_USE_NUMERIC_JACOBIANS)
22 #define SRBA_COMPUTE_ANALYTIC_JACOBIANS (SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS || !SRBA_USE_NUMERIC_JACOBIANS)
23 
24 #define DEBUG_JACOBIANS_SUPER_VERBOSE 0
25 #define DEBUG_NOT_UPDATED_ENTRIES 0 // Extremely slow, just for debug during development! This checks that all the expected Jacobians are actually updated
26 
27 /** Numeric implementation of the partial Jacobian dh_dAp */
28 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
30 {
32  mrpt::poses::SE_traits<pose_t::rotation_dimensions>::pseudo_exp(x,incr);
33 
35  if (!params.is_inverse_dir)
36  {
37  if (params.pose_d1_wrt_obs) // "A" in papers
38  base_from_obs.composeFrom(*params.pose_d1_wrt_obs,incr);
39  else base_from_obs = incr;
40  base_from_obs.composeFrom(base_from_obs,params.pose_base_wrt_d1);
41  }
42  else
43  {
44  // Changes due to the inverse pose:
45  // D becomes D' = p_d^{d+1} (+) D
46  ASSERT_(params.k2k_edge_id<params.k2k_edges.size())
47  const pose_t & p_d_d1 = params.k2k_edges[params.k2k_edge_id]
48 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
49  ->
50 #else
51  .
52 #endif
53  inv_pose;
54 
55  pose_t pose_base_wrt_d_prime(mrpt::poses::UNINITIALIZED_POSE); // D' in papers
56  pose_base_wrt_d_prime.composeFrom( p_d_d1, params.pose_base_wrt_d1 );
57 
59  p_d_d1_mod.composeFrom(incr, p_d_d1);
60  p_d_d1_mod.inverse();
61 
62  // total pose: base from obs = pose_d1_wrt_obs (+) inv(p_d_d1_mod) (+) D'
63  if (params.pose_d1_wrt_obs) // "A" in papers
64  base_from_obs = *params.pose_d1_wrt_obs + p_d_d1_mod + pose_base_wrt_d_prime;
65  else base_from_obs = p_d_d1_mod + pose_base_wrt_d_prime;
66  }
67 
68  // pose_robot2sensor(): pose wrt sensor = pose_wrt_robot (-) sensor_pose_on_the_robot
70  RBA_OPTIONS::sensor_pose_on_robot_t::pose_robot2sensor( base_from_obs, base_pose_wrt_sensor, params.sensor_pose );
71 
72  // Generate observation:
73  array_obs_t z_zero;
74  z_zero.setZero();
75 
76  sensor_model_t::observe_error(y,z_zero,base_pose_wrt_sensor,params.xji_i, params.sensor_params);
77  y=-y; // because the method above evals: "z_zero - h(x)"
78 }
79 
80 #if DEBUG_NOT_UPDATED_ENTRIES
81 
82 struct TNumSTData
83 {
84  TKeyFrameID from, to;
85 };
86 
87 TNumSTData check_num_st_entry_exists(
88  const pose_flag_t * entry,
89  const TRBA_Problem_state::TSpanningTree & st)
90 {
91  for (TRelativePosesForEachTarget::const_iterator it_tree=st.num.begin();it_tree!=st.num.end();++it_tree)
92  {
93  const TKeyFrameID id_from = it_tree->first;
94  const frameid2pose_map_t & tree = it_tree->second;
95 
96  for (frameid2pose_map_t::const_iterator it=tree.begin();it!=tree.end();++it)
97  {
98  const TKeyFrameID id_to = it->first;
99  const pose_flag_t & e = it->second;
100  if (&e == entry)
101  {
102  TNumSTData d;
103  d.from = id_from;
104  d.to = id_to;
105  return d;
106  }
107  }
108  }
109  ASSERT_(false)
110 }
111 
112 #endif
113 
114 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
116 {
117  static const pose_t my_aux_null_pose;
118 
119  const array_landmark_t x_local = params.xji_i + x;
120  const pose_t * pos_cam = params.pose_base_wrt_obs!=NULL ? params.pose_base_wrt_obs : &my_aux_null_pose;
121 
122  // pose_robot2sensor(): pose wrt sensor = pose_wrt_robot (-) sensor_pose_on_the_robot
124  RBA_OPTIONS::sensor_pose_on_robot_t::pose_robot2sensor( *pos_cam, base_pose_wrt_sensor, params.sensor_pose );
125 
126  // Generate observation:
127  array_obs_t z_zero;
128  z_zero.setZero();
129  sensor_model_t::observe_error(y,z_zero,base_pose_wrt_sensor,x_local, params.sensor_params);
130  y=-y; // because the method above evals: "z_zero - h(x)"
131 }
132 
133 
134 /** Auxiliary sub-jacobian used in compute_jacobian_dh_dp() (it's a static method within specializations of this struct) */
135 template <landmark_jacob_family_t JACOB_FAMILY, size_t POINT_DIMS, size_t POSE_DIMS, class RBA_ENGINE_T>
137 
138 // ====================================================================
139 // j,i lm_id,base_id
140 // \partial h \partial h
141 // l obs_frame_id
142 // dh_dp = ------------------ = ---------------------------------
143 // d+1 cur_id
144 // \partial p \partial p
145 // d stp.next_node
146 //
147 // See tech report:
148 // "A tutorial on SE(3) transformation parameterizations and
149 // on-manifold optimization", Jose-Luis Blanco.
150 // ====================================================================
151 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
153  typename TSparseBlocksJacobians_dh_dAp::TEntry &jacob,
154  const k2f_edge_t & observation,
155  const k2k_edges_deque_t &k2k_edges,
156  std::vector<const pose_flag_t*> *out_list_of_required_num_poses) const
157 {
158  ASSERT_(observation.obs.kf_id!=jacob.sym.kf_base)
159 
160  if (! *jacob.sym.is_valid )
161  return; // Another block of the same Jacobian row said this observation was invalid for some reason.
162 
163  // And x^{j,i}_l = pose_of_i_wrt_l (+) x^{j,i}_i
164 
165  // Handle the special case when d==obs, so rel_pose_d1_from_obs==NULL, and its pose is the origin:
166  const pose_flag_t * pose_d1_wrt_obs = jacob.sym.rel_pose_d1_from_obs; // "A" in papers
167  const pose_flag_t & pose_base_wrt_d1 = *jacob.sym.rel_pose_base_from_d1;
168  const bool is_inverse_edge_jacobian = !jacob.sym.edge_normal_dir; // If edge points in the opposite direction than as assumed in mathematical derivation.
169 
170  if (out_list_of_required_num_poses)
171  {
172  if (jacob.sym.rel_pose_d1_from_obs)
173  out_list_of_required_num_poses->push_back(jacob.sym.rel_pose_d1_from_obs);
174  out_list_of_required_num_poses->push_back(jacob.sym.rel_pose_base_from_d1);
175  }
176 
177  // make sure the numeric spanning tree is working and updating all that we need:
178 #if DEBUG_NOT_UPDATED_ENTRIES
179  TNumSTData d1 = check_num_st_entry_exists(&pose_base_wrt_d1, rba_state.spanning_tree);
180 #endif
181 
182  if (!pose_base_wrt_d1.updated)
183  {
184  std::cerr << " kf_d+1: " << jacob.sym.kf_d << ", base_id: "<< jacob.sym.kf_base << std::endl;
185  rba_state.spanning_tree.save_as_dot_file("_debug_jacob_error_all_STs.dot");
186  ASSERT_(pose_base_wrt_d1.updated)
187  }
188 
189  if (pose_d1_wrt_obs)
190  {
191 #if DEBUG_NOT_UPDATED_ENTRIES
192  TNumSTData d2 = check_num_st_entry_exists(pose_d1_wrt_obs, rba_state.spanning_tree);
193 #endif
194  if (!pose_d1_wrt_obs->updated)
195  {
196  std::cerr << " kf_d+1: " << jacob.sym.kf_d << ", obs_frame_id: "<< observation.obs.kf_id << std::endl;
197  rba_state.spanning_tree.save_as_dot_file("_debug_jacob_error_all_STs.dot");
198  }
199  ASSERT_(pose_d1_wrt_obs->updated)
200  }
201 
202 
203  // i<-l = d <- obs/l (+) base/i <- d
205  if (pose_d1_wrt_obs!=NULL)
206  pose_i_wrt_l.composeFrom( pose_d1_wrt_obs->pose, pose_base_wrt_d1.pose);
207  else pose_i_wrt_l = pose_base_wrt_d1.pose;
208 
209  // First, we need x^{j,i}_i:
210  // LM parameters in: jacob.sym.feat_rel_pos->pos[0:N-1]
211  const array_landmark_t &xji_i = jacob.sym.feat_rel_pos->pos;
212 
213  // xji_l = pose_i_wrt_l (+) xji_i
214  array_landmark_t xji_l = xji_i; //
215  LM_TYPE::composePosePoint(xji_l, pose_i_wrt_l);
216 
217 #if DEBUG_JACOBIANS_SUPER_VERBOSE // Debug:
218  {
219  const TKeyFrameID obs_frame_id = observation.obs.kf_id;
220  const TKeyFrameID base_id = jacob.sym.kf_base;
221  const TKeyFrameID d_plus_1_id = jacob.sym.kf_d;
222  cout << "compute_jacobian_dh_dp: "
223  << " obs_frame_id: " << obs_frame_id
224  << " base_id: " << base_id
225  << " kf_d+1: " << d_plus_1_id
226  << " k2k_edge_id: " << jacob.sym.k2k_edge_id
227  << " is_inverse: " << is_inverse_edge_jacobian << endl
228  << " pose_base_wrt_d1 (D): "<<pose_base_wrt_d1.pose << endl;
229  if (pose_d1_wrt_obs) cout << " pose_d1_wrt_obs (A): "<< pose_d1_wrt_obs->pose<< endl;
230  // Observations from the "base_id" of a fixed,known landmark are NOT considered observations for optimization:
231  //mrpt::system::pause();
232  }
233 #endif
234 
235 #if SRBA_COMPUTE_NUMERIC_JACOBIANS
236  // Numeric jacobians
237  typename TSparseBlocksJacobians_dh_dAp::matrix_t num_jacob;
238 
239  array_pose_t x;
240  x.setZero(); // Evaluate Jacobian at manifold incr around origin
241  array_pose_t x_incrs;
242  x_incrs.setConstant(1e-4);
243 
244  const TNumeric_dh_dAp_params num_params(jacob.sym.k2k_edge_id,&pose_d1_wrt_obs->pose, pose_base_wrt_d1.pose,jacob.sym.feat_rel_pos->pos, is_inverse_edge_jacobian,k2k_edges,this->parameters.sensor,this->parameters.sensor_pose);
245 
246  mrpt::math::jacobians::jacob_numeric_estimate(x,&numeric_dh_dAp,x_incrs,num_params,num_jacob);
247 
248 #endif // SRBA_COMPUTE_NUMERIC_JACOBIANS
249 
250 
251 #if SRBA_COMPUTE_ANALYTIC_JACOBIANS
252  // d h(x^{j,i}_l) d h(x') d x^{j,i}_l
253  // --------------- = --------- * ----------------
254  // d p^{d+1}_d d x' d epsilon
255  //
256  // With: x' = x^{j,i}_l
257 
258  // First jacobian: (uses xji_l)
259  // -----------------------------
260  Eigen::Matrix<double,OBS_DIMS,LM_DIMS> dh_dx;
261 
262  // Converts a point relative to the robot coordinate frame (P) into a point relative to the sensor (RES = P \ominus POSE_IN_ROBOT )
263  RBA_OPTIONS::sensor_pose_on_robot_t::template point_robot2sensor<LM_TYPE,array_landmark_t>(xji_l,xji_l,this->parameters.sensor_pose );
264 
265  // Invoke sensor model:
266  if (!sensor_model_t::eval_jacob_dh_dx(dh_dx,xji_l, this->parameters.sensor))
267  {
268  // Invalid Jacobian:
269  *jacob.sym.is_valid = 0;
270  jacob.num.setZero();
271  return;
272  }
273 
274  // take into account the possible displacement of the sensor wrt the keyframe:
275  RBA_OPTIONS::sensor_pose_on_robot_t::jacob_dh_dx_rotate( dh_dx, this->parameters.sensor_pose );
276 
277  // Second Jacobian: (uses xji_i)
278  // ------------------------------
279  compute_jacobian_dAepsDx_deps<LM_TYPE::jacob_family,LM_DIMS,REL_POSE_DIMS,rba_engine_t>::eval(jacob.num,dh_dx,is_inverse_edge_jacobian,xji_i, pose_d1_wrt_obs, pose_base_wrt_d1,jacob.sym,k2k_edges,rba_state.all_observations);
280 
281 #endif // SRBA_COMPUTE_ANALYTIC_JACOBIANS
282 
283 
284 #if SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS
285  // Check jacob.num vs. num_jacob
286  const double MAX_REL_ERROR = 0.1;
287  if ((jacob.num-num_jacob).array().abs().maxCoeff()>MAX_REL_ERROR*num_jacob.array().maxCoeff())
288  {
289  std::cerr << "NUMERIC VS. ANALYTIC JACOBIAN dh_dAp FAILED:"
290  << "\njacob.num:\n" << jacob.num
291  << "\nnum_jacob:\n" << num_jacob
292  << "\nDiff:\n" << jacob.num-num_jacob << endl << endl;
293  }
294 #endif
295 
296 #if SRBA_USE_NUMERIC_JACOBIANS
297  jacob.num = num_jacob;
298 #endif
299 }
300 
301 // ====================================================================
302 // Auxiliary sub-Jacobian used in compute_jacobian_dh_dp()
303 // These are specializations of the template, for each of the cases:
304 //template <size_t , size_t , class MATRIX, class POINT>
305 // ====================================================================
306 
307 // Case: 3D point, SE(3) poses:
308 template <class RBA_ENGINE_T>
309 struct compute_jacobian_dAepsDx_deps<jacob_point_landmark /* Jacobian family: this LM is a point */, 3 /*POINT_DIMS*/,6 /*POSE_DIMS*/,RBA_ENGINE_T>
310 {
311  template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
312  static void eval(
313  MATRIX & jacob,
314  const MATRIX_DH_DX & dh_dx,
315  const bool is_inverse_edge_jacobian,
316  const POINT & xji_i,
317  const pose_flag_t * pose_d1_wrt_obs, // "A" in handwritten notes
318  const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
319  const JACOB_SYM_T & jacob_sym,
320  const K2K_EDGES_T & k2k_edges,
321  const OBS_VECTOR & all_obs
322  )
323  {
324  // See section 10.3.7 of technical report on SE(3) poses [http://mapir.isa.uma.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf]
325  if (!is_inverse_edge_jacobian)
326  { // Normal formulation: unknown is pose "d+1 -> d"
327 
328  // This is "D" in my handwritten notes:
329  // pose_i_wrt_dplus1 -> pose_base_wrt_d1
330 
331  const mrpt::math::CMatrixDouble33 & ROTD = pose_base_wrt_d1.pose.getRotationMatrix();
332 
333  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0,0,0,0):
334  Eigen::Matrix<double,RBA_ENGINE_T::OBS_DIMS,3> H_ROTA;
335  if (pose_d1_wrt_obs!=NULL)
336  {
337  // pose_d_plus_1_wrt_l -> pose_d1_wrt_obs
338 
339  // 3x3 term: H*R(A)
340  H_ROTA = dh_dx * pose_d1_wrt_obs->pose.getRotationMatrix();
341  }
342  else
343  {
344  // 3x3 term: H*R(A)
345  H_ROTA = dh_dx;
346  }
347 
348  // First 2x3 block:
349  jacob.block(0,0,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA;
350 
351  // Second 2x3 block:
352  // compute aux vector "v":
353  Eigen::Matrix<double,3,1> v;
354  v[0] = -pose_base_wrt_d1.pose.x() - xji_i[0]*ROTD.coeff(0,0) - xji_i[1]*ROTD.coeff(0,1) - xji_i[2]*ROTD.coeff(0,2);
355  v[1] = -pose_base_wrt_d1.pose.y() - xji_i[0]*ROTD.coeff(1,0) - xji_i[1]*ROTD.coeff(1,1) - xji_i[2]*ROTD.coeff(1,2);
356  v[2] = -pose_base_wrt_d1.pose.z() - xji_i[0]*ROTD.coeff(2,0) - xji_i[1]*ROTD.coeff(2,1) - xji_i[2]*ROTD.coeff(2,2);
357 
358  Eigen::Matrix<double,3,3> aux;
359 
360  aux.coeffRef(0,0)=0;
361  aux.coeffRef(1,1)=0;
362  aux.coeffRef(2,2)=0;
363 
364  aux.coeffRef(1,2)=-v[0];
365  aux.coeffRef(2,1)= v[0];
366  aux.coeffRef(2,0)=-v[1];
367  aux.coeffRef(0,2)= v[1];
368  aux.coeffRef(0,1)=-v[2];
369  aux.coeffRef(1,0)= v[2];
370 
371  jacob.block(0,3,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA * aux;
372  }
373  else
374  { // Inverse formulation: unknown is pose "d -> d+1"
375 
376  // Changes due to the inverse pose:
377  // D becomes D' = p_d^{d+1} (+) D
378 
379  ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
380  const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id]
381 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
382  ->
383 #else
384  .
385 #endif
386  inv_pose;
387 
388  typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
389  pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );
390 
391  const mrpt::math::CMatrixDouble33 & ROTD = pose_base_wrt_d1_prime.getRotationMatrix();
392 
393  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0,0,0,0):
394  Eigen::Matrix<double,RBA_ENGINE_T::OBS_DIMS,3> H_ROTA;
395  if (pose_d1_wrt_obs!=NULL)
396  {
397  // pose_d_plus_1_wrt_l -> pose_d1_wrt_obs
398 
399  // In inverse edges, A (which is "pose_d1_wrt_obs") becomes A * (p_d_d1)^-1 =>
400  // So: ROT_A' = ROT_A * ROT_d_d1^t
401 
402  // 3x3 term: H*R(A')
403  H_ROTA = dh_dx * pose_d1_wrt_obs->pose.getRotationMatrix() * p_d_d1.getRotationMatrix().transpose();
404  }
405  else
406  {
407  // Was in the normal edge: H_ROTA = dh_dx;
408 
409  // 3x3 term: H*R(A')
410  H_ROTA = dh_dx * p_d_d1.getRotationMatrix().transpose();
411  }
412 
413  // First 2x3 block:
414  jacob.block(0,0,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA;
415 
416  // Second 2x3 block:
417  // compute aux vector "v":
418  Eigen::Matrix<double,3,1> v;
419  v[0] = -pose_base_wrt_d1_prime.x() - xji_i[0]*ROTD.coeff(0,0) - xji_i[1]*ROTD.coeff(0,1) - xji_i[2]*ROTD.coeff(0,2);
420  v[1] = -pose_base_wrt_d1_prime.y() - xji_i[0]*ROTD.coeff(1,0) - xji_i[1]*ROTD.coeff(1,1) - xji_i[2]*ROTD.coeff(1,2);
421  v[2] = -pose_base_wrt_d1_prime.z() - xji_i[0]*ROTD.coeff(2,0) - xji_i[1]*ROTD.coeff(2,1) - xji_i[2]*ROTD.coeff(2,2);
422 
423  Eigen::Matrix<double,3,3> aux;
424 
425  aux.coeffRef(0,0)=0;
426  aux.coeffRef(1,1)=0;
427  aux.coeffRef(2,2)=0;
428 
429  aux.coeffRef(1,2)=-v[0];
430  aux.coeffRef(2,1)= v[0];
431  aux.coeffRef(2,0)=-v[1];
432  aux.coeffRef(0,2)= v[1];
433  aux.coeffRef(0,1)=-v[2];
434  aux.coeffRef(1,0)= v[2];
435 
436  jacob.block(0,3,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA * aux;
437 
438  // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
439  jacob = -jacob;
440 
441  } // end inverse edge case
442 
443  }
444 }; // end of specialization of "compute_jacobian_dAepsDx_deps"
445 
446 
447 /** Case: 2D or 3D points, SE(2) poses
448  * Both cases are grouped because a SE(2) pose doesn't transform the "z" of 3D points, so both sets of Jacobians are almost identical.
449  */
450 template <size_t POINT_DIMS, class RBA_ENGINE_T>
452 {
453  template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
454  static void eval(
455  MATRIX & jacob,
456  const MATRIX_DH_DX & dh_dx,
457  const bool is_inverse_edge_jacobian,
458  const POINT & xji_i,
459  const pose_flag_t * pose_d1_wrt_obs, // "A" in handwritten notes
460  const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
461  const JACOB_SYM_T & jacob_sym,
462  const K2K_EDGES_T & k2k_edges,
463  const OBS_VECTOR & all_obs
464  )
465  {
466  MRPT_COMPILE_TIME_ASSERT(POINT_DIMS==2 || POINT_DIMS==3)
467 
468  if (!is_inverse_edge_jacobian)
469  { // Normal formulation: unknown is pose "d+1 -> d"
470 
471  const double Xd=pose_base_wrt_d1.pose.x();
472  const double Yd=pose_base_wrt_d1.pose.y();
473 
474  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
475  double PHIa=0; // Xa, Ya: Are not really needed, since they don't appear in the Jacobian.
477  if (pose_d1_wrt_obs!=NULL)
478  {
479  // pose_d_plus_1_wrt_l -> pose_d1_wrt_obs
480  //Xa = pose_d1_wrt_obs->pose.x(); Ya = pose_d1_wrt_obs->pose.y(); // Not needed
481  PHIa = pose_d1_wrt_obs->pose.phi();
482  AD.composeFrom(pose_d1_wrt_obs->pose,pose_base_wrt_d1.pose);
483  }
484  else
485  {
486  AD = pose_base_wrt_d1.pose; // A=0 -> A(+)D is simply D
487  }
488 
489  // d(P (+) x) / dP, P = A*D
490  Eigen::Matrix<double,POINT_DIMS/* 2 or 3*/,3 /* x,y,phi*/> dPx_P;
491  const double ccos_ad = cos(AD.phi());
492  const double ssin_ad = sin(AD.phi());
493  dPx_P(0,0) = 1; dPx_P(0,1) = 0; dPx_P(0,2) = -xji_i[0]*ssin_ad - xji_i[1]*ccos_ad;
494  dPx_P(1,0) = 0; dPx_P(1,1) = 1; dPx_P(1,2) = xji_i[0]*ccos_ad - xji_i[1]*ssin_ad;
495  if (POINT_DIMS==3) {
496  dPx_P(2,0) = 0; dPx_P(2,1) = 0; dPx_P(2,2) = 1;
497  }
498 
499  // d(A*exp(eps)*D) / deps
500  Eigen::Matrix<double,3,3> dAD_deps;
501  const double ccos_a = cos(PHIa);
502  const double ssin_a = sin(PHIa);
503  dAD_deps(0,0) = ccos_a; dAD_deps(0,1) = -ssin_a;
504  dAD_deps(1,0) = ssin_a; dAD_deps(1,1) = ccos_a;
505  dAD_deps(2,0) = 0; dAD_deps(2,1) = 0;
506 
507  dAD_deps(0,2) = -ssin_a*Xd - ccos_a*Yd;
508  dAD_deps(1,2) = ccos_a*Xd - ssin_a*Yd;
509  dAD_deps(2,2) = 1;
510 
511  // Chain rule:
512  jacob.noalias() = dh_dx * dPx_P * dAD_deps;
513  }
514  else
515  { // Inverse formulation: unknown is pose "d -> d+1"
516 
517  // Changes due to the inverse pose:
518  // D becomes D' = p_d^{d+1} (+) D
519  // and A (which is "pose_d1_wrt_obs") becomes A' = A (+) (p_d_d1)^-1
520 
521  ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
522  const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id]
523 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
524  ->
525 #else
526  .
527 #endif
528  inv_pose;
529 
530  typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
531  pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );
532 
533  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
534  const typename RBA_ENGINE_T::pose_t p_d_d1_inv = -p_d_d1;
535 
536  typename RBA_ENGINE_T::pose_t A_prime = (pose_d1_wrt_obs!=NULL) ?
537  (pose_d1_wrt_obs->pose + p_d_d1_inv)
538  :
539  p_d_d1_inv;
540 
542  AD.composeFrom(A_prime,pose_base_wrt_d1_prime);
543 
544  const double Xd=pose_base_wrt_d1_prime.x();
545  const double Yd=pose_base_wrt_d1_prime.y();
546 
547  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
548  const double PHIa=A_prime.phi();
549 
550 
551  // d(P (+) x) / dP, P = A*D
552  Eigen::Matrix<double,POINT_DIMS/* 2 or 3*/,3 /* x,y,phi*/> dPx_P;
553  const double ccos_ad = cos(AD.phi());
554  const double ssin_ad = sin(AD.phi());
555  dPx_P(0,0) = 1; dPx_P(0,1) = 0; dPx_P(0,2) = -xji_i[0]*ssin_ad - xji_i[1]*ccos_ad;
556  dPx_P(1,0) = 0; dPx_P(1,1) = 1; dPx_P(1,2) = xji_i[0]*ccos_ad - xji_i[1]*ssin_ad;
557  if (POINT_DIMS==3) {
558  dPx_P(2,0) = 0; dPx_P(2,1) = 0; dPx_P(2,2) = 1;
559  }
560 
561  // d(A*exp(eps)*D) / deps
562  Eigen::Matrix<double,3,3> dAD_deps;
563  const double ccos_a = cos(PHIa);
564  const double ssin_a = sin(PHIa);
565  dAD_deps(0,0) = ccos_a; dAD_deps(0,1) = -ssin_a;
566  dAD_deps(1,0) = ssin_a; dAD_deps(1,1) = ccos_a;
567  dAD_deps(2,0) = 0; dAD_deps(2,1) = 0;
568 
569  dAD_deps(0,2) = -ssin_a*Xd - ccos_a*Yd;
570  dAD_deps(1,2) = ccos_a*Xd - ssin_a*Yd;
571  dAD_deps(2,2) = 1;
572 
573  // Chain rule:
574  jacob.noalias() = dh_dx * dPx_P * dAD_deps;
575 
576  // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
577  jacob = -jacob;
578 
579  } // end inverse edge case
580 
581  }
582 }; // end of "compute_jacobian_dAepsDx_deps_SE2"
583 
584 // Case: 2D point, SE(2) poses: (derived from generic SE2 implementation above)
585 template <class RBA_ENGINE_T>
586 struct compute_jacobian_dAepsDx_deps<jacob_point_landmark /* Jacobian family: this LM is a point */, 2 /*POINT_DIMS*/,3 /*POSE_DIMS*/,RBA_ENGINE_T>
587  : public compute_jacobian_dAepsDx_deps_SE2<2 /*POINT_DIMS*/,RBA_ENGINE_T>
588 {
589 };
590 
591 // Case: 3D point, SE(2) poses: (derived from generic SE2 implementation above)
592 template <class RBA_ENGINE_T>
593 struct compute_jacobian_dAepsDx_deps<jacob_point_landmark /* Jacobian family: this LM is a point */, 3 /*POINT_DIMS*/,3 /*POSE_DIMS*/,RBA_ENGINE_T>
594  : public compute_jacobian_dAepsDx_deps_SE2<3 /*POINT_DIMS*/,RBA_ENGINE_T>
595 {
596 };
597 
598 // Case: SE(2) relative-poses, SE(2) poses:
599 template <class RBA_ENGINE_T>
600 struct compute_jacobian_dAepsDx_deps<jacob_relpose_landmark /* Jacobian family: this LM is a point */, 3 /*POINT_DIMS*/,3 /*POSE_DIMS*/,RBA_ENGINE_T>
601 {
602  template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
603  static void eval(
604  MATRIX & jacob,
605  const MATRIX_DH_DX & dh_dx,
606  const bool is_inverse_edge_jacobian,
607  const POINT & xji_i,
608  const pose_flag_t * pose_d1_wrt_obs, // "A" in handwritten notes
609  const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
610  const JACOB_SYM_T & jacob_sym,
611  const K2K_EDGES_T & k2k_edges,
612  const OBS_VECTOR & all_obs
613  )
614  {
615  double Xd,Yd,PHIa;
617 
618  if (!is_inverse_edge_jacobian)
619  { // Normal formulation: unknown is pose "d+1 -> d"
620 
621  Xd=pose_base_wrt_d1.pose.x();
622  Yd=pose_base_wrt_d1.pose.y();
623 
624  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
625  PHIa=0; // Xa, Ya: Are not really needed, since they don't appear in the Jacobian.
626  if (pose_d1_wrt_obs!=NULL)
627  {
628  // pose_d_plus_1_wrt_l -> pose_d1_wrt_obs
629  PHIa = pose_d1_wrt_obs->pose.phi();
630  base_wrt_obs.composeFrom(pose_d1_wrt_obs->pose, pose_base_wrt_d1.pose); // A (+) D
631  }
632  else
633  {
634  base_wrt_obs = pose_base_wrt_d1.pose; // A (+) D
635  }
636  }
637  else
638  { // Inverse formulation: unknown is pose "d -> d+1"
639 
640  // Changes due to the inverse pose:
641  // D becomes D' = p_d^{d+1} (+) D
642  // and A (which is "pose_d1_wrt_obs") becomes A' = A (+) (p_d_d1)^-1
643 
644  ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
645  const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id]
646 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
647  ->
648 #else
649  .
650 #endif
651  inv_pose;
652 
653  typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
654  pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );
655 
656  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
657  const typename RBA_ENGINE_T::pose_t p_d_d1_inv = -p_d_d1;
658 
659  typename RBA_ENGINE_T::pose_t A_prime = (pose_d1_wrt_obs!=NULL) ?
660  (pose_d1_wrt_obs->pose + p_d_d1_inv)
661  :
662  p_d_d1_inv;
663 
664  Xd=pose_base_wrt_d1_prime.x();
665  Yd=pose_base_wrt_d1_prime.y();
666 
667  // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
668  PHIa=A_prime.phi();
669 
670  base_wrt_obs.composeFrom(A_prime, pose_base_wrt_d1_prime); // A (+) D
671  }
672 
673  //const mrpt::poses::CPose2D base_wrt_obs_inv = -base_wrt_obs;
674  const double PHIad = base_wrt_obs.phi();
675 
676  //const mrpt::poses::CPose2D p_obs(0,0,0);
677  //all_obs[jacob_sym.obs_idx].obs.obs.obs_data.x,
678  //all_obs[jacob_sym.obs_idx].obs.obs.obs_data.y,
679  //all_obs[jacob_sym.obs_idx].obs.obs.obs_data.yaw);
680 
681  const double ccos_ad = cos(PHIad), ssin_ad=sin(PHIad);
682  const double ccos_a = cos(PHIa), ssin_a=sin(PHIa);
683  //const double ccos_obs = cos(p_obs.phi()), ssin_obs=sin(p_obs.phi());
684 
685  //const double Ax = base_wrt_obs.x()-p_obs.x();
686  //const double Ay = base_wrt_obs.y()-p_obs.y();
687 
688  Eigen::Matrix<double,3,3> J0; // -d(\ominus p)_dp, with p=A*D
689  J0(0,0)= ccos_ad; J0(0,1)= ssin_ad; J0(0,2)= 0;
690  J0(1,0)=-ssin_ad; J0(1,1)= ccos_ad; J0(1,2)= 0;
691  J0(2,0)=0; J0(2,1)=0; J0(2,2)= 1;
692 
693  Eigen::Matrix<double,3,3> J1; // dAD_dA;
694  J1(0,0)=1; J1(0,1)=0; J1(0,2)= -Xd*ssin_a-Yd*ccos_a;
695  J1(1,0)=0; J1(1,1)=1; J1(1,2)= Xd*ccos_a-Yd*ssin_a;
696  J1(2,0)=0; J1(2,1)=0; J1(2,2)= 1;
697 
698  Eigen::Matrix<double,3,3> J2; // dAe_de
699  J2(0,0)=ccos_a; J2(0,1)=-ssin_a; J2(0,2)=0;
700  J2(1,0)=ssin_a; J2(1,1)= ccos_a; J2(1,2)=0;
701  J2(2,0)=0; J2(2,1)=0; J2(2,2)=1;
702 
703  // Chain rule:
704  jacob.noalias() = dh_dx * J0* J1 * J2;
705 
706  if (is_inverse_edge_jacobian)
707  {
708  // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
709  jacob = -jacob;
710  }
711  }
712 }; // end of "compute_jacobian_dAepsDx_deps", Case: SE(2) relative-poses, SE(2) poses:
713 
714 
715 
716 // ====================================================================
717 // j,i lm_id,base_id
718 // \partial h \partial h
719 // l obs_frame_id
720 // dh_df = ------------------ = ---------------------------------
721 //
722 // \partial f \partial f
723 //
724 // Note: f=relative position of landmark with respect to its base kf
725 // ====================================================================
726 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
728  typename TSparseBlocksJacobians_dh_df::TEntry &jacob,
729  const k2f_edge_t & observation,
730  std::vector<const pose_flag_t*> *out_list_of_required_num_poses) const
731 {
732  if (! *jacob.sym.is_valid )
733  return; // Another block of the same Jacobian row said this observation was invalid for some reason.
734 
735  // Handle the special case when obs==base, for which rel_pose_base_from_obs==NULL
736  const pose_flag_t * rel_pose_base_from_obs = jacob.sym.rel_pose_base_from_obs;
737 
738  if (out_list_of_required_num_poses && rel_pose_base_from_obs)
739  out_list_of_required_num_poses->push_back(jacob.sym.rel_pose_base_from_obs);
740 
741  // make sure the numeric spanning tree is working and updating all that we need:
742  if (rel_pose_base_from_obs)
743  {
744 #if DEBUG_NOT_UPDATED_ENTRIES
745  TNumSTData d1 = check_num_st_entry_exists(rel_pose_base_from_obs, rba_state.spanning_tree);
746 #endif
747  if (!rel_pose_base_from_obs->updated)
748  {
749 #if DEBUG_NOT_UPDATED_ENTRIES
750  cout << "not updated ST entry for: from=" << d1.from << ", to=" << d1.to << endl;
751 #endif
752  rba_state.spanning_tree.save_as_dot_file("_debug_jacob_error_all_STs.dot");
753  ASSERT_(rel_pose_base_from_obs->updated)
754  }
755  }
756 
757  // First, we need x^{j,i}_i:
758  //const TPoint3D & xji_i = jacob.sym.feat_rel_pos->pos;
759  const array_landmark_t &xji_i = jacob.sym.feat_rel_pos->pos;
760 
761  array_landmark_t xji_l = xji_i; //
762 
763  if (rel_pose_base_from_obs!=NULL)
764  {
765 #if 0
766  cout << "dh_df(ft_id="<< observation.obs.obs.feat_id << ", obs_kf="<< observation.obs.kf_id << "): o2b=" << *rel_pose_base_from_obs << endl;
767 #endif
768  // xji_l = rel_pose_base_from_obs (+) xji_i
769  //rel_pose_base_from_obs->pose.composePoint(xji_i, xji_l);
770  LM_TYPE::composePosePoint(xji_l, rel_pose_base_from_obs->pose);
771  }
772  else
773  {
774  // I'm observing from the same base key-frame: xji_l = xji_i (already done above)
775  }
776 
777 
778 #if SRBA_COMPUTE_NUMERIC_JACOBIANS
779  // Numeric jacobians
780  typename TSparseBlocksJacobians_dh_df::matrix_t num_jacob;
781 
783  x.setZero(); // Evaluate Jacobian at incr around origin
784  array_landmark_t x_incrs;
785  x_incrs.setConstant(1e-3);
786 
787  const TNumeric_dh_df_params num_params(&rel_pose_base_from_obs->pose,jacob.sym.feat_rel_pos->pos,this->parameters.sensor,this->parameters.sensor_pose);
788 
789  mrpt::math::jacobians::jacob_numeric_estimate(x,&numeric_dh_df,x_incrs,num_params,num_jacob);
790 
791 #endif // SRBA_COMPUTE_NUMERIC_JACOBIANS
792 
793 
794 #if SRBA_COMPUTE_ANALYTIC_JACOBIANS
795  // d h(x^{j,i}_l) d h(x') d x'
796  // --------------- = --------- * ----------
797  // d f d x' d x
798  //
799  // With: x' = x^{j,i}_l
800 
801  // First jacobian: (uses xji_l)
802  // -----------------------------
803  Eigen::Matrix<double,OBS_DIMS,LM_DIMS> dh_dx;
804 
805  // Converts a point relative to the robot coordinate frame (P) into a point relative to the sensor (RES = P \ominus POSE_IN_ROBOT )
806  RBA_OPTIONS::sensor_pose_on_robot_t::template point_robot2sensor<LM_TYPE,array_landmark_t>(xji_l,xji_l,this->parameters.sensor_pose );
807 
808  // Invoke sensor model:
809  if (!sensor_model_t::eval_jacob_dh_dx(dh_dx,xji_l, this->parameters.sensor))
810  {
811  // Invalid Jacobian:
812  *jacob.sym.is_valid = 0;
813  jacob.num.setZero();
814  return;
815  }
816 
817  // take into account the possible displacement of the sensor wrt the keyframe:
818  RBA_OPTIONS::sensor_pose_on_robot_t::jacob_dh_dx_rotate( dh_dx, this->parameters.sensor_pose );
819 
820  // Second Jacobian: Simply the 2x2 or 3x3 rotation matrix of base wrt observing
821  // ------------------------------
822  if (rel_pose_base_from_obs!=NULL)
823  {
825  rel_pose_base_from_obs->pose.getRotationMatrix(R);
826  jacob.num.noalias() = dh_dx * R;
827  }
828  else
829  {
830  // if observing from the same base kf, we're done:
831  jacob.num.noalias() = dh_dx;
832  }
833 #endif // SRBA_COMPUTE_ANALYTIC_JACOBIANS
834 
835 
836 #if SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS
837  // Check jacob.num vs. num_jacob
838  const double MAX_REL_ERROR = 0.1;
839  if ((jacob.num-num_jacob).array().abs().maxCoeff()>MAX_REL_ERROR*num_jacob.array().maxCoeff())
840  {
841  std::cerr << "NUMERIC VS. ANALYTIC JACOBIAN dh_df FAILED:"
842  << "\njacob.num:\n" << jacob.num
843  << "\nnum_jacob:\n" << num_jacob
844  << "\nDiff:\n" << jacob.num-num_jacob << endl << endl;
845  }
846 #endif
847 
848 #if SRBA_USE_NUMERIC_JACOBIANS
849  jacob.num = num_jacob;
850 #endif
851 }
852 
853 
854 // ------------------------------------------------------------------------
855 // prepare_Jacobians_required_tree_roots()
856 // ------------------------------------------------------------------------
857 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
859  std::set<TKeyFrameID> & lst,
860  const std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> &lst_JacobCols_dAp,
861  const std::vector<typename TSparseBlocksJacobians_dh_df::col_t*> &lst_JacobCols_df )
862 {
863  lst.clear();
864 
865  const size_t nUnknowns_k2k = lst_JacobCols_dAp.size();
866  const size_t nUnknowns_k2f = lst_JacobCols_df.size();
867 
868  // k2k edges ------------------------------------------------------
869  for (size_t i=0;i<nUnknowns_k2k;i++)
870  {
871  // For each column, process each nonzero block:
872  const typename TSparseBlocksJacobians_dh_dAp::col_t *col = lst_JacobCols_dAp[i];
873 
874  for (typename TSparseBlocksJacobians_dh_dAp::col_t::const_iterator it=col->begin();it!=col->end();++it)
875  {
876  // For each dh_dAp block we need:
877  // * (d+1) -> obs
878  // * (d+1) -> base
879 
880  const size_t obs_idx = it->first;
881  const typename TSparseBlocksJacobians_dh_dAp::TEntry & jacob_entry = it->second;
882 
883 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
884  const TKeyFrameID obs_id = rba_state.all_observations[obs_idx]->obs.kf_id;
885 #else
886  const TKeyFrameID obs_id = rba_state.all_observations[obs_idx].obs.kf_id;
887 #endif
888  const TKeyFrameID d1_id = jacob_entry.sym.kf_d;
889  const TKeyFrameID base_id = jacob_entry.sym.kf_base;
890 
891  add_edge_ij_to_list_needed_roots(lst, d1_id , obs_id );
892  add_edge_ij_to_list_needed_roots(lst, base_id, d1_id );
893  }
894  }
895 
896  // k2f edges ------------------------------------------------------
897  for (size_t i=0;i<nUnknowns_k2f;i++)
898  {
899  // For each column, process each nonzero block:
900  const typename TSparseBlocksJacobians_dh_df::col_t *col = lst_JacobCols_df[i];
901 
902  for (typename TSparseBlocksJacobians_dh_df::col_t::const_iterator it=col->begin();it!=col->end();++it)
903  {
904  // For each dh_df block we need:
905  // * obs -> base
906  const size_t obs_idx = it->first;
907  const k2f_edge_t &k2f =
908 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
909  *
910 #endif
911  rba_state.all_observations[obs_idx];
912 
913  ASSERT_(k2f.feat_rel_pos)
914 
915  const TKeyFrameID obs_id = k2f.obs.kf_id;
916  const TKeyFrameID base_id = k2f.feat_rel_pos->id_frame_base;
917 
918  add_edge_ij_to_list_needed_roots(lst, base_id, obs_id );
919  }
920  }
921 }
922 
923 
924 // ------------------------------------------------------------------------
925 // recompute_all_Jacobians(): Re-evaluate all Jacobians numerically
926 // ------------------------------------------------------------------------
927 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
929  std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> &lst_JacobCols_dAp,
930  std::vector<typename TSparseBlocksJacobians_dh_df::col_t*> &lst_JacobCols_df,
931  std::vector<const typename kf2kf_pose_traits<KF2KF_POSE_TYPE>::pose_flag_t*> * out_list_of_required_num_poses )
932 {
933  size_t nJacobs=0;
934  if (out_list_of_required_num_poses) out_list_of_required_num_poses->clear();
935 
936  const size_t nUnknowns_k2k = lst_JacobCols_dAp.size();
937  const size_t nUnknowns_k2f = lst_JacobCols_df.size();
938 
939  // k2k edges ------------------------------------------------------
940  for (size_t i=0;i<nUnknowns_k2k;i++)
941  {
942  // For each column, process each nonzero block:
943  typename TSparseBlocksJacobians_dh_dAp::col_t *col = lst_JacobCols_dAp[i];
944 
945  for (typename TSparseBlocksJacobians_dh_dAp::col_t::iterator it=col->begin();it!=col->end();++it)
946  {
947  const size_t obs_idx = it->first;
948  typename TSparseBlocksJacobians_dh_dAp::TEntry & jacob_entry = it->second;
949  compute_jacobian_dh_dp(
950  jacob_entry,
951 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
952  *rba_state.all_observations[obs_idx],
953 #else
954  rba_state.all_observations[obs_idx],
955 #endif
956  rba_state.k2k_edges,
957  out_list_of_required_num_poses );
958  nJacobs++;
959  }
960  }
961 
962  // k2f edges ------------------------------------------------------
963  for (size_t i=0;i<nUnknowns_k2f;i++)
964  {
965  // For each column, process each nonzero block:
966  typename TSparseBlocksJacobians_dh_df::col_t *col = lst_JacobCols_df[i];
967 
968  for (typename TSparseBlocksJacobians_dh_df::col_t::iterator it=col->begin();it!=col->end();++it)
969  {
970  const size_t obs_idx = it->first;
971  typename TSparseBlocksJacobians_dh_df::TEntry & jacob_entry = it->second;
972  compute_jacobian_dh_df(
973  jacob_entry,
974 #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
975  *rba_state.all_observations[obs_idx],
976 #else
977  rba_state.all_observations[obs_idx],
978 #endif
979  out_list_of_required_num_poses );
980  nJacobs++;
981  }
982  }
983 
984  return nJacobs;
985 } // end of recompute_all_Jacobians()
986 
987 
988 } } // end of namespace
Auxiliary sub-jacobian used in compute_jacobian_dh_dp() (it&#39;s a static method within specializations ...
obs_traits_t::observation_t obs
Observation data.
Definition: srba_types.h:430
Typedefs for determining whether the result of combining a KF pose (+) a sensor pose leads to a SE(2)...
const RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t & sensor_pose
Definition: RbaEngine.h:679
Scalar * iterator
Definition: eigen_plugins.h:23
rba_problem_state_t::k2k_edges_deque_t k2k_edges_deque_t
A list (deque) of KF-to-KF edges (unknown relative poses).
Definition: RbaEngine.h:92
const Scalar * const_iterator
Definition: eigen_plugins.h:24
kf2kf_pose_traits_t::pose_flag_t pose_flag_t
Definition: RbaEngine.h:94
KF2KF_POSE_TYPE::pose_t pose_t
The type of relative poses (e.g. mrpt::poses::CPose3D)
Definition: RbaEngine.h:87
static void numeric_dh_df(const array_landmark_t &x, const TNumeric_dh_df_params &params, array_obs_t &y)
Auxiliary method for numeric Jacobian: numerically evaluates the new observation "y" for a small incr...
lm_traits_t::TRelativeLandmarkPos * feat_rel_pos
Pointer to the known/unknown rel.pos. (always!=NULL)
Definition: srba_types.h:444
kf2kf_pose_traits_t::array_pose_t array_pose_t
Definition: RbaEngine.h:103
const OBS_TYPE::TObservationParams & sensor_params
Definition: RbaEngine.h:699
A numeric matrix of compile-time fixed size.
The argument "POSE_TRAITS" can be any of those defined in srba/models/kf2kf_poses.h (typically, either kf2kf_poses::SE3 or kf2kf_poses::SE2).
Definition: srba_types.h:47
static void numeric_dh_dAp(const array_pose_t &x, const TNumeric_dh_dAp_params &params, array_obs_t &y)
Auxiliary method for numeric Jacobian: numerically evaluates the new observation "y" for a small incr...
#define MRPT_COMPILE_TIME_ASSERT(f)
static void eval(MATRIX &jacob, const MATRIX_DH_DX &dh_dx, const bool is_inverse_edge_jacobian, const POINT &xji_i, const pose_flag_t *pose_d1_wrt_obs, const pose_flag_t &pose_base_wrt_d1, const JACOB_SYM_T &jacob_sym, const K2K_EDGES_T &k2k_edges, const OBS_VECTOR &all_obs)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void prepare_Jacobians_required_tree_roots(std::set< TKeyFrameID > &kfs_num_spantrees_to_update, const std::vector< typename TSparseBlocksJacobians_dh_dAp::col_t *> &lst_JacobCols_dAp, const std::vector< typename TSparseBlocksJacobians_dh_df::col_t *> &lst_JacobCols_df)
Prepare the list of all required KF roots whose spanning trees need numeric updates with each optimiz...
A class used to store a 2D pose.
Definition: CPose2D.h:35
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:83
const RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t & sensor_pose
Definition: RbaEngine.h:700
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
#define ASSERT_(f)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArray.h:354
Case: 2D or 3D points, SE(2) poses Both cases are grouped because a SE(2) pose doesn&#39;t transform the ...
void jacob_numeric_estimate(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
"fake landmarks" used for relative pose observations
const OBS_TYPE::TObservationParams & sensor_params
Definition: RbaEngine.h:678
void compute_jacobian_dh_dp(typename TSparseBlocksJacobians_dh_dAp::TEntry &jacob, const k2f_edge_t &observation, const k2k_edges_deque_t &k2k_edges, std::vector< const pose_flag_t *> *out_list_of_required_num_poses) const
==================================================================== j,i lm_id,base_id h h l obs_fr...
Keyframe-to-feature edge: observation data stored for each keyframe.
Definition: srba_types.h:439
void compute_jacobian_dh_df(typename TSparseBlocksJacobians_dh_df::TEntry &jacob, const k2f_edge_t &observation, std::vector< const pose_flag_t *> *out_list_of_required_num_poses) const
==================================================================== j,i lm_id,base_id h h l obs_fr...
static void eval(MATRIX &jacob, const MATRIX_DH_DX &dh_dx, const bool is_inverse_edge_jacobian, const POINT &xji_i, const pose_flag_t *pose_d1_wrt_obs, const pose_flag_t &pose_base_wrt_d1, const JACOB_SYM_T &jacob_sym, const K2K_EDGES_T &k2k_edges, const OBS_VECTOR &all_obs)
size_t recompute_all_Jacobians(std::vector< typename TSparseBlocksJacobians_dh_dAp::col_t *> &lst_JacobCols_dAp, std::vector< typename TSparseBlocksJacobians_dh_df::col_t *> &lst_JacobCols_df, std::vector< const pose_flag_t *> *out_list_of_required_num_poses=NULL)
Re-evaluate all Jacobians numerically using their symbolic info.
static void eval(MATRIX &jacob, const MATRIX_DH_DX &dh_dx, const bool is_inverse_edge_jacobian, const POINT &xji_i, const pose_flag_t *pose_d1_wrt_obs, const pose_flag_t &pose_base_wrt_d1, const JACOB_SYM_T &jacob_sym, const K2K_EDGES_T &k2k_edges, const OBS_VECTOR &all_obs)
uint64_t TKeyFrameID
Numeric IDs for key-frames (KFs)
Definition: srba_types.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo