9 #ifndef GRAPH_SLAM_LEVMARQ_H 10 #define GRAPH_SLAM_LEVMARQ_H 55 template <
class GRAPH_T>
59 const std::set<mrpt::utils::TNodeID> * in_nodes_to_optimize = NULL,
76 typename gst::Array_O array_O_zeros; array_O_zeros.fill(0);
79 static const unsigned int DIMS_POSE = gst::SE_TYPE::VECTOR_SIZE;
82 const bool verbose = 0!=extra_params.getWithDefaultVal(
"verbose",0);
83 const size_t max_iters = extra_params.getWithDefaultVal(
"max_iterations",100);
84 const bool enable_profiler = 0!=extra_params.getWithDefaultVal(
"profiler",0);
86 const double initial_lambda = extra_params.getWithDefaultVal(
"initial_lambda", 0);
87 const double tau = extra_params.getWithDefaultVal(
"tau", 1e-3);
88 const double e1 = extra_params.getWithDefaultVal(
"e1",1e-6);
89 const double e2 = extra_params.getWithDefaultVal(
"e2",1e-6);
91 const double SCALE_HESSIAN = extra_params.getWithDefaultVal(
"scale_hessian",1);
95 profiler.
enter(
"optimize_graph_spa_levmarq (entire)");
98 profiler.
enter(
"optimize_graph_spa_levmarq.list_IDs");
99 const set<TNodeID> * nodes_to_optimize;
100 set<TNodeID> nodes_to_optimize_auxlist;
101 if (in_nodes_to_optimize)
103 nodes_to_optimize = in_nodes_to_optimize;
109 if (it->first!=graph.root)
110 nodes_to_optimize_auxlist.insert(nodes_to_optimize_auxlist.end(), it->first );
111 nodes_to_optimize = &nodes_to_optimize_auxlist;
113 profiler.
leave(
"optimize_graph_spa_levmarq.list_IDs");
116 const size_t nFreeNodes = nodes_to_optimize->size();
124 ostream_iterator<TNodeID> out_it (cout,
", ");
125 std::copy(nodes_to_optimize->begin(), nodes_to_optimize->end(), out_it );
132 typedef typename gst::observation_info_t observation_info_t;
133 vector<observation_info_t> lstObservationData;
138 for (
typename gst::edge_const_iterator it=graph.edges.begin();it!=graph.edges.end();++it)
141 const typename gst::graph_t::edge_t &edge = it->second;
143 if (nodes_to_optimize->find(
ids.first)==nodes_to_optimize->end() &&
144 nodes_to_optimize->find(
ids.second)==nodes_to_optimize->end())
150 ASSERTDEBMSG_(itP1!=graph.nodes.end(),
"Node1 in an edge does not have a global pose in 'graph.nodes'.")
151 ASSERTDEBMSG_(itP2!=graph.nodes.end(),
"Node2 in an edge does not have a global pose in 'graph.nodes'.")
153 const typename gst::graph_t::constraint_t::type_value &EDGE_POSE = edge.getPoseMean();
156 typename gst::observation_info_t new_entry;
158 new_entry.edge_mean = &EDGE_POSE;
159 new_entry.P1 = &itP1->second;
160 new_entry.P2 = &itP2->second;
162 lstObservationData.push_back(new_entry);
166 const size_t nObservations = lstObservationData.size();
171 typedef std::unique_ptr<CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr;
173 typedef std::auto_ptr<CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr;
175 SparseCholeskyDecompPtr ptrCh;
181 typename gst::map_pairIDs_pairJacobs_t lstJacobians;
188 profiler.
enter(
"optimize_graph_spa_levmarq.Jacobians&err");
189 double total_sqr_err = computeJacobiansAndErrors<GRAPH_T>(
190 graph, lstObservationData,
192 profiler.
leave(
"optimize_graph_spa_levmarq.Jacobians&err");
199 vector<pair<size_t,size_t> > obsIdx2fnIdx;
200 obsIdx2fnIdx.reserve(nObservations);
201 ASSERTDEB_(lstJacobians.size()==nObservations)
204 const TNodeID id1 = itJ->first.first;
205 const TNodeID id2 = itJ->first.second;
206 obsIdx2fnIdx.push_back(
217 double lambda = initial_lambda;
219 bool have_to_recompute_H_and_grad =
true;
226 size_t last_iter = 0;
228 for (
size_t iter=0;iter<max_iters;++iter)
231 vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
233 if (have_to_recompute_H_and_grad)
235 have_to_recompute_H_and_grad =
false;
243 profiler.
enter(
"optimize_graph_spa_levmarq.grad");
253 for (idx_obs=0, itJ=lstJacobians.begin();
254 itJ!=lstJacobians.end();
257 ASSERTDEB_(itJ->first==lstObservationData[idx_obs].edge->first)
264 const size_t idx1 = obsIdx2fnIdx[idx_obs].first;
265 const size_t idx2 = obsIdx2fnIdx[idx_obs].second;
267 if (idx1!=string::npos)
270 lstObservationData[idx_obs].edge ,
275 if (idx2!=string::npos)
278 lstObservationData[idx_obs].edge ,
286 ::memcpy(&grad[0],&grad_parts[0], nFreeNodes*DIMS_POSE*
sizeof(grad[0]));
287 grad /= SCALE_HESSIAN;
288 profiler.
leave(
"optimize_graph_spa_levmarq.grad");
292 if (grad_norm_inf<=e1)
300 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:build map");
314 for (idxObs=0, itJacobPair=lstJacobians.begin();
315 idxObs<nObservations;
316 ++itJacobPair,++idxObs)
318 const bool Hij_upper_triang =
319 itJacobPair->first.first < itJacobPair->first.second;
322 const size_t idx_i = obsIdx2fnIdx[idxObs].first;
323 const size_t idx_j = obsIdx2fnIdx[idxObs].second;
325 const bool is_i_free_node = idx_i!=string::npos;
326 const bool is_j_free_node = idx_j!=string::npos;
330 const typename gst::matrix_VxV_t& J1 = itJacobPair->second.first;
331 const typename gst::matrix_VxV_t& J2 = itJacobPair->second.second;
338 H_map[idx_i][idx_i] += JtJ;
345 H_map[idx_j][idx_j] += JtJ;
348 if (is_i_free_node && is_j_free_node)
354 if (Hij_upper_triang)
355 H_map[idx_j][idx_i] += JtJ;
357 H_map[idx_i][idx_j] += JtJ.transpose();
361 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:build map");
364 if (lambda<=0 && iter==0)
366 profiler.
enter(
"optimize_graph_spa_levmarq.lambda_init");
367 double H_diagonal_max = 0;
368 for (
size_t i=0;i<nFreeNodes;i++)
371 const size_t j = it->first;
374 for (
size_t k=0;k<DIMS_POSE;k++)
378 lambda = tau * H_diagonal_max;
380 profiler.
leave(
"optimize_graph_spa_levmarq.lambda_init");
395 cout <<
"["<<
__CURRENT_FUNCTION_NAME__<<
"] Iter: " << iter <<
" ,total sqr. err: " << total_sqr_err <<
", avrg. err per edge: " << std::sqrt(total_sqr_err/nObservations) <<
" lambda: " << lambda << endl;
398 if (functor_feedback)
400 (*functor_feedback)(graph,iter,max_iters,total_sqr_err);
404 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:build");
407 CSparseMatrix sp_H(nFreeNodes*DIMS_POSE,nFreeNodes*DIMS_POSE);
408 for (
size_t i=0;i<nFreeNodes;i++)
410 const size_t i_offset = i*DIMS_POSE;
414 const size_t j = it->first;
415 const size_t j_offset = j*DIMS_POSE;
421 for (
size_t r=0;
r<DIMS_POSE;
r++)
426 for (
size_t c=
r+1;
c<DIMS_POSE;
c++)
438 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:build");
447 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:chol");
450 else ptrCh.get()->update(sp_H);
451 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:chol");
453 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:backsub");
454 ptrCh->backsub(grad,delta);
455 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:backsub");
460 if (verbose ) cout <<
"["<<
__CURRENT_FUNCTION_NAME__<<
"] Got non-definite positive matrix, retrying with a larger lambda...\n";
471 profiler.
enter(
"optimize_graph_spa_levmarq.delta_norm");
473 profiler.
leave(
"optimize_graph_spa_levmarq.delta_norm");
476 profiler.
enter(
"optimize_graph_spa_levmarq.x_norm" );
482 const typename gst::graph_t::constraint_t::type_value &P = itP->second;
483 for (
size_t i=0;i<DIMS_POSE;i++)
486 x_norm=std::sqrt(x_norm);
488 profiler.
leave(
"optimize_graph_spa_levmarq.x_norm" );
491 const double thres_norm = e2*(x_norm+e2);
492 if (delta_norm<thres_norm)
504 typename gst::graph_t::global_poses_t old_poses_backup;
507 ASSERTDEB_(delta.size()==nodes_to_optimize->size()*DIMS_POSE)
508 const double *delta_ptr = &delta[0];
512 typename gst::Array_O exp_delta;
513 for (
size_t i=0;i<DIMS_POSE;i++)
514 exp_delta[i]= - *delta_ptr++;
518 old_poses_backup[*it] = it_old_value->second;
526 typename gst::map_pairIDs_pairJacobs_t new_lstJacobians;
529 profiler.
enter(
"optimize_graph_spa_levmarq.Jacobians&err");
530 double new_total_sqr_err = computeJacobiansAndErrors<GRAPH_T>(
531 graph, lstObservationData,
532 new_lstJacobians, new_errs);
533 profiler.
leave(
"optimize_graph_spa_levmarq.Jacobians&err");
536 if (new_total_sqr_err < total_sqr_err)
539 new_lstJacobians.swap(lstJacobians);
541 std::swap( new_total_sqr_err, total_sqr_err);
544 have_to_recompute_H_and_grad =
true;
551 graph.nodes[it->first] = it->second;
553 if (verbose ) cout <<
"["<<
__CURRENT_FUNCTION_NAME__<<
"] Got larger error=" << new_total_sqr_err <<
", retrying with a larger lambda...\n";
563 profiler.
leave(
"optimize_graph_spa_levmarq (entire)");
#define ASSERT_EQUAL_(__A, __B)
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::utils::TNodeID > *in_nodes_to_optimize=NULL, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), typename graphslam_traits< GRAPH_T >::TFunctorFeedback functor_feedback=NULL)
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
#define ASSERT_ABOVE_(__A, __B)
void insert_entry_fast(const size_t row, const size_t col, const double val)
This was an optimized version, but is now equivalent to insert_entry() due to the need to be compatib...
EIGEN_STRONG_INLINE iterator begin()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
size_t num_iters
The number of LM iterations executed.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
A sparse matrix structure, wrapping T.
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
const Scalar * const_iterator
T square(const T x)
Inline function for the square of a number.
Used in mrpt::math::CSparseMatrix.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
SLAM methods related to graphs of pose constraints.
This base provides a set of functions for maths stuff.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
uint64_t TNodeID
The type for node IDs in graphs of different types.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Auxiliary traits template for use among graph-slam problems to make life easier with these complicate...
CONTAINER::Scalar norm_inf(const CONTAINER &v)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
GLdouble GLdouble GLdouble r
void insert_submatrix(const size_t row, const size_t col, const MATRIX &M)
ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of ...
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
double leave(const char *func_name)
End of a named section.
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
void enter(const char *func_name)
Start of a named section.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
double final_total_sq_error
The sum of all the squared errors for every constraint involved in the problem.
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
CONTAINER::Scalar norm(const CONTAINER &v)