Main MRPT website > C++ reference
MRPT logo
sparse_hessian_build_symbolic.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-2015, 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 namespace mrpt { namespace srba {
13 
14 
15 /** Rebuild the Hessian symbolic information from the given Jacobians
16  * Example of the expected template types:
17  * - HESS_Apf: MatrixBlockSparseCols<double,6,3,THessianSymbolicInfo<double,2,6,3>, false >
18  * - JACOB_COLUMN_dh_dAp: TSparseBlocksJacobians_dh_dAp::col_t = MatrixBlockSparseCols<double,2,6,TJacobianSymbolicInfo_dh_dAp, false>::col_t
19  */
20 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
21 template <class HESS_Ap, class HESS_f,class HESS_Apf, class JACOB_COLUMN_dh_dAp,class JACOB_COLUMN_dh_df>
22 void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::sparse_hessian_build_symbolic(
23  HESS_Ap & HAp,
24  HESS_f & Hf,
25  HESS_Apf & HApf,
26  const std::vector<JACOB_COLUMN_dh_dAp*> & dh_dAp,
27  const std::vector<JACOB_COLUMN_dh_df*> & dh_df)
28 {
29  typedef typename HESS_Ap::symbolic_t::THessianSymbolicInfoEntry hess_Ap_sym_entry_t; // These are concrete instances of THessianSymbolicInfo<...>::THessianSymbolicInfoEntry
30  typedef typename HESS_f::symbolic_t::THessianSymbolicInfoEntry hess_f_sym_entry_t;
31  typedef typename HESS_Apf::symbolic_t::THessianSymbolicInfoEntry hess_Apf_sym_entry_t;
32 
33  const size_t nUnknowns_k2k = dh_dAp.size();
34  const size_t nUnknowns_k2f = dh_df.size();
35 
36  // --------------------------------------------------------------------
37  // (1) HAp = J_Ap^t * J_Ap
38  // --------------------------------------------------------------------
39  // Only upper-triangular half of U (what is used by Cholesky):
40  HAp.setColCount (nUnknowns_k2k);
41  for (size_t i=0;i<nUnknowns_k2k;i++)
42  {
43  const JACOB_COLUMN_dh_dAp & col_i = *dh_dAp[i];
44  ASSERT_(!col_i.empty()) // I guess this shouldn't happen...
45 
46  // j=i
47  // -----
48  {
49  typename HESS_Ap::symbolic_t & Hii_sym = HAp.getCol(i)[i].sym;
50 
51  for (typename JACOB_COLUMN_dh_dAp::const_iterator it=col_i.begin();it!=col_i.end();++it)
52  {
53  const size_t obs_idx = it->first;
54 
55  Hii_sym.lst_jacob_blocks.push_back(
56  hess_Ap_sym_entry_t(
57  &it->second.num, &it->second.num, // J1, J2,
58  it->second.sym.is_valid,it->second.sym.is_valid, // J1_valid, J2_valid,
59  obs_idx
60  ) );
61  }
62  }
63 
64  // j=[i+1, nUnknowns-1]
65  // -------------------
66  for (size_t j=i+1;j<nUnknowns_k2k;j++)
67  {
68  const JACOB_COLUMN_dh_dAp & col_j = *dh_dAp[j];
69  ASSERTMSG_(!col_j.empty(),mrpt::format("col_j,i=%u,j=%u empty (nUnknowns_k2k=%u)!",static_cast<unsigned int>(i),static_cast<unsigned int>(j),static_cast<unsigned int>(nUnknowns_k2k))) // I guess this shouldn't happen...
70 
71  // code below based on "std::set_intersection"
72  typename JACOB_COLUMN_dh_dAp::const_iterator it_i = col_i.begin();
73  typename JACOB_COLUMN_dh_dAp::const_iterator it_j = col_j.begin();
74  const typename JACOB_COLUMN_dh_dAp::const_iterator it_i_end = col_i.end();
75  const typename JACOB_COLUMN_dh_dAp::const_iterator it_j_end = col_j.end();
76 
77  // Don't create the entry in the sparse Hessian until we're sure it's nonzero:
78  typename HESS_Ap::symbolic_t Hij_sym;
79 
80  while (it_i!=it_i_end && it_j!=it_j_end)
81  {
82  if ( it_i->first < it_j->first ) ++it_i;
83  else if ( it_j->first < it_i->first ) ++it_j;
84  else
85  {
86  // match between: it_i->first == it_j->first
87  const size_t obs_idx = it_i->first;
88 
89  Hij_sym.lst_jacob_blocks.push_back( //std::make_pair(&it_i->second.num, &it_j->second.num) );
90  hess_Ap_sym_entry_t(
91  &it_i->second.num, &it_j->second.num, // J1, J2,
92  it_i->second.sym.is_valid,it_j->second.sym.is_valid, // J1_valid, J2_valid,
93  obs_idx
94  ) );
95 
96  // Move:
97  ++it_i; ++it_j;
98  }
99  }
100 
101  // If it was nonzero, move (swap) to its actual place, created with the [] operator in the std::map<>:
102  if (!Hij_sym.lst_jacob_blocks.empty())
103  Hij_sym.lst_jacob_blocks.swap( HAp.getCol(j)[i].sym.lst_jacob_blocks ); // Caution!! Look: at (j,i), in that order, OK?
104 
105  } // end for j
106  } // end for i
107 
108  // --------------------------------------------------------------------
109  // (2) Hf = J_f^t * J_f
110  // --------------------------------------------------------------------
111  // Only upper-triangular half of U (what is used by Cholesky):
112  Hf.setColCount (nUnknowns_k2f);
113  for (size_t i=0;i<nUnknowns_k2f;i++)
114  {
115  const JACOB_COLUMN_dh_df & col_i = *dh_df[i];
116  ASSERT_(!col_i.empty()) // I guess this shouldn't happen...
117 
118  // j=i
119  // -----
120  {
121  typename HESS_f::symbolic_t & Hii_sym = Hf.getCol(i)[i].sym;
122 
123  for (typename JACOB_COLUMN_dh_df::const_iterator it=col_i.begin();it!=col_i.end();++it)
124  {
125  const size_t obs_idx = it->first;
126 
127  Hii_sym.lst_jacob_blocks.push_back( //std::make_pair(&it->second.num, &it->second.num) );
128  hess_f_sym_entry_t(
129  &it->second.num, &it->second.num, // J1, J2,
130  it->second.sym.is_valid,it->second.sym.is_valid, // J1_valid, J2_valid,
131  obs_idx
132  ) );
133  }
134  }
135 
136  // j=[i+1, nUnknowns-1]
137  // -------------------
138  for (size_t j=i+1;j<nUnknowns_k2f;j++)
139  {
140  const JACOB_COLUMN_dh_df & col_j = *dh_df[j];
141  ASSERT_(!col_j.empty()) // I guess this shouldn't happen...
142 
143  // code below based on "std::set_intersection"
144  typename JACOB_COLUMN_dh_df::const_iterator it_i = col_i.begin();
145  typename JACOB_COLUMN_dh_df::const_iterator it_j = col_j.begin();
146  const typename JACOB_COLUMN_dh_df::const_iterator it_i_end = col_i.end();
147  const typename JACOB_COLUMN_dh_df::const_iterator it_j_end = col_j.end();
148 
149  // Don't create the entry in the sparse Hessian until we're sure it's nonzero:
150  typename HESS_f::symbolic_t Hij_sym;
151 
152  while (it_i!=it_i_end && it_j!=it_j_end)
153  {
154  if ( it_i->first < it_j->first ) ++it_i;
155  else if ( it_j->first < it_i->first ) ++it_j;
156  else
157  {
158  // match between: it_i->first == it_j->first
159  const size_t obs_idx = it_i->first;
160 
161  Hij_sym.lst_jacob_blocks.push_back( //std::make_pair(&it_i->second.num, &it_j->second.num) );
162  hess_f_sym_entry_t(
163  &it_i->second.num, &it_j->second.num, // J1, J2,
164  it_i->second.sym.is_valid,it_j->second.sym.is_valid, // J1_valid, J2_valid,
165  obs_idx
166  ) );
167 
168  // Move:
169  ++it_i; ++it_j;
170  }
171  }
172 
173  // If it was nonzero, move (swap) to its actual place, created with the [] operator in the std::map<>:
174  if (!Hij_sym.lst_jacob_blocks.empty())
175  Hij_sym.lst_jacob_blocks.swap( Hf.getCol(j)[i].sym.lst_jacob_blocks ); // Caution!! Look: at (j,i), in that order, OK?
176 
177  } // end for j
178  } // end for i
179 
180  // --------------------------------------------------------------------
181  // (3) HApf = J_Ap^t * J_f
182  // --------------------------------------------------------------------
183  // The entire rectangular matrix (it's NOT symetric!)
184 
185  // *NOTE* HApf will be stored indices by rows instead of columns!!
186 
187  //HApf.setColCount(nUnknowns_k2f);
188  HApf.setColCount(nUnknowns_k2k); // # of ROWS
189  for (size_t i=0;i<nUnknowns_k2k;i++) // i:row = [0,nUnknowns_k2k-1]
190  {
191  const JACOB_COLUMN_dh_dAp & col_i = *dh_dAp[i];
192  ASSERT_(!col_i.empty()) // I guess this shouldn't happen...
193 
194  // j:col = [0,nUnknowns_k2f-1]
195  for (size_t j=0;j<nUnknowns_k2f;j++)
196  {
197  const JACOB_COLUMN_dh_df & col_j = *dh_df[j];
198  ASSERT_(!col_j.empty()) // I guess this shouldn't happen...
199 
200  // code below based on "std::set_intersection"
201  typename JACOB_COLUMN_dh_dAp::const_iterator it_i = col_i.begin();
202  typename JACOB_COLUMN_dh_df::const_iterator it_j = col_j.begin();
203  const typename JACOB_COLUMN_dh_dAp::const_iterator it_i_end = col_i.end();
204  const typename JACOB_COLUMN_dh_df::const_iterator it_j_end = col_j.end();
205 
206  // Don't create the entry in the sparse Hessian until we're sure it's nonzero:
207  typename HESS_Apf::symbolic_t Hij_sym;
208 
209  while (it_i!=it_i_end && it_j!=it_j_end)
210  {
211  if ( it_i->first < it_j->first ) ++it_i;
212  else if ( it_j->first < it_i->first ) ++it_j;
213  else
214  {
215  // match between: it_i->first == it_j->first
216  const size_t obs_idx = it_i->first;
217 
218  Hij_sym.lst_jacob_blocks.push_back(
219  hess_Apf_sym_entry_t(
220  &it_i->second.num, &it_j->second.num, // J1, J2,
221  it_i->second.sym.is_valid,it_j->second.sym.is_valid, // J1_valid, J2_valid,
222  obs_idx
223  ) );
224 
225  // Move:
226  ++it_i; ++it_j;
227  }
228  }
229 
230  // If it was nonzero, move (swap) to its actual place, created with the [] operator in the std::map<>:
231  if (!Hij_sym.lst_jacob_blocks.empty())
232  Hij_sym.lst_jacob_blocks.swap( HApf.getCol(i)[j].sym.lst_jacob_blocks ); // (i,j) because it's stored indices by rows instead of columns!
233 
234  } // end for j
235  } // end for i
236 
237 }
238 
239 } } // end NS
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::srba::RbaEngine::sparse_hessian_build_symbolic
static void sparse_hessian_build_symbolic(HESS_Ap &HAp, HESS_f &Hf, HESS_Apf &HApf, const std::vector< JACOB_COLUMN_dh_dAp * > &dh_dAp, const std::vector< JACOB_COLUMN_dh_df * > &dh_df)
Rebuild the Hessian symbolic information from the given Jacobians.
Definition: sparse_hessian_build_symbolic.h:22
mrpt::mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:260
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: maps/CColouredPointsMap.h:259


Page generated by Doxygen 1.8.9.1 for MRPT 1.3.0 SVN: at Sun Sep 13 03:55:12 UTC 2015