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;
108 for (
typename gst::graph_t::global_poses_t::const_iterator it=graph.nodes.begin();it!=graph.nodes.end();++it)
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())
148 typename gst::graph_t::global_poses_t::iterator itP1 = graph.nodes.find(ids.first);
149 typename gst::graph_t::global_poses_t::iterator itP2 = graph.nodes.find(ids.second);
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");
198 vector<pair<size_t,size_t> > observationIndex_to_relatedFreeNodeIndex;
199 observationIndex_to_relatedFreeNodeIndex.reserve(nObservations);
200 ASSERTDEB_(lstJacobians.size()==nObservations)
201 for (
typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ=lstJacobians.begin();itJ!=lstJacobians.end();++itJ)
203 const TNodeID id1 = itJ->first.first;
204 const TNodeID id2 = itJ->first.second;
205 observationIndex_to_relatedFreeNodeIndex.push_back(
215 vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
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)
232 if (have_to_recompute_H_and_grad)
234 have_to_recompute_H_and_grad =
false;
242 profiler.
enter(
"optimize_graph_spa_levmarq.grad");
250 typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ;
252 for (idx_obs=0, itJ=lstJacobians.begin();
253 itJ!=lstJacobians.end();
256 ASSERTDEB_(itJ->first==lstObservationData[idx_obs].edge->first)
263 const size_t idx1 = observationIndex_to_relatedFreeNodeIndex[idx_obs].first;
264 const size_t idx2 = observationIndex_to_relatedFreeNodeIndex[idx_obs].second;
266 if (idx1!=string::npos)
269 lstObservationData[idx_obs].edge ,
274 if (idx2!=string::npos)
277 lstObservationData[idx_obs].edge ,
285 ::memcpy(&grad[0],&grad_parts[0], nFreeNodes*DIMS_POSE*
sizeof(grad[0]));
286 grad /= SCALE_HESSIAN;
287 profiler.
leave(
"optimize_graph_spa_levmarq.grad");
291 if (grad_norm_inf<=e1)
299 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:build map");
311 typename gst::map_pairIDs_pairJacobs_t::const_iterator itJacobPair;
313 for (idxObs=0, itJacobPair=lstJacobians.begin();
314 idxObs<nObservations;
315 ++itJacobPair,++idxObs)
318 const bool edge_straight = itJacobPair->first.first < itJacobPair->first.second;
321 const size_t idx_i = edge_straight ? observationIndex_to_relatedFreeNodeIndex[idxObs].first : observationIndex_to_relatedFreeNodeIndex[idxObs].second;
322 const size_t idx_j = edge_straight ? observationIndex_to_relatedFreeNodeIndex[idxObs].second : observationIndex_to_relatedFreeNodeIndex[idxObs].first;
324 const bool is_i_free_node = idx_i!=string::npos;
325 const bool is_j_free_node = idx_j!=string::npos;
330 const typename gst::matrix_VxV_t &J1 = edge_straight ? itJacobPair->second.first : itJacobPair->second.second;
331 const typename gst::matrix_VxV_t &J2 = edge_straight ? itJacobPair->second.second : itJacobPair->second.first;
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)
352 H_map[idx_j][idx_i] += JtJ;
356 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:build map");
359 if (lambda<=0 && iter==0)
361 profiler.
enter(
"optimize_graph_spa_levmarq.lambda_init");
362 double H_diagonal_max = 0;
363 for (
size_t i=0;i<nFreeNodes;i++)
364 for (
typename map_ID2matrix_VxV_t::const_iterator it=H_map[i].
begin();it!=H_map[i].end();++it)
366 const size_t j = it->first;
369 for (
size_t k=0;k<DIMS_POSE;k++)
373 lambda = tau * H_diagonal_max;
375 profiler.
leave(
"optimize_graph_spa_levmarq.lambda_init");
390 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;
393 if (functor_feedback)
395 (*functor_feedback)(graph,iter,max_iters,total_sqr_err);
399 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:build");
402 CSparseMatrix sp_H(nFreeNodes*DIMS_POSE,nFreeNodes*DIMS_POSE);
403 for (
size_t i=0;i<nFreeNodes;i++)
405 const size_t i_offset = i*DIMS_POSE;
407 for (
typename map_ID2matrix_VxV_t::const_iterator it=H_map[i].
begin();it!=H_map[i].end();++it)
409 const size_t j = it->first;
410 const size_t j_offset = j*DIMS_POSE;
416 for (
size_t r=0;r<DIMS_POSE;r++)
419 sp_H.
insert_entry_fast(j_offset+r,i_offset+r, it->second.get_unsafe(r,r) + lambda );
421 for (
size_t c=r+1;c<DIMS_POSE;c++)
433 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:build");
442 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:chol");
445 else ptrCh.get()->update(sp_H);
446 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:chol");
448 profiler.
enter(
"optimize_graph_spa_levmarq.sp_H:backsub");
449 ptrCh->backsub(grad,delta);
450 profiler.
leave(
"optimize_graph_spa_levmarq.sp_H:backsub");
455 if (verbose ) cout <<
"["<<
__CURRENT_FUNCTION_NAME__<<
"] Got non-definite positive matrix, retrying with a larger lambda...\n";
466 profiler.
enter(
"optimize_graph_spa_levmarq.delta_norm");
468 profiler.
leave(
"optimize_graph_spa_levmarq.delta_norm");
471 profiler.
enter(
"optimize_graph_spa_levmarq.x_norm" );
474 for (set<TNodeID>::const_iterator it=nodes_to_optimize->begin();it!=nodes_to_optimize->end();++it)
476 typename gst::graph_t::global_poses_t::const_iterator itP = graph.nodes.find(*it);
477 const typename gst::graph_t::constraint_t::type_value &P = itP->second;
478 for (
size_t i=0;i<DIMS_POSE;i++)
479 x_norm+=square(P[i]);
481 x_norm=std::sqrt(x_norm);
483 profiler.
leave(
"optimize_graph_spa_levmarq.x_norm" );
486 const double thres_norm = e2*(x_norm+e2);
487 if (delta_norm<thres_norm)
499 typename gst::graph_t::global_poses_t old_poses_backup;
502 ASSERTDEB_(delta.size()==nodes_to_optimize->size()*DIMS_POSE)
503 const double *delta_ptr = &delta[0];
504 for (set<TNodeID>::const_iterator it=nodes_to_optimize->begin();it!=nodes_to_optimize->end();++it)
507 typename gst::graph_t::constraint_t::type_value exp_delta_pose(UNINITIALIZED_POSE);
508 typename gst::Array_O exp_delta;
509 for (
size_t i=0;i<DIMS_POSE;i++)
510 exp_delta[i]= - *delta_ptr++;
511 gst::SE_TYPE::exp(exp_delta,exp_delta_pose);
514 typename gst::graph_t::global_poses_t::iterator it_old_value = graph.nodes.find(*it);
515 old_poses_backup[*it] = it_old_value->second;
516 it_old_value->second.composeFrom(exp_delta_pose, it_old_value->second);
523 typename gst::map_pairIDs_pairJacobs_t new_lstJacobians;
526 profiler.
enter(
"optimize_graph_spa_levmarq.Jacobians&err");
527 double new_total_sqr_err = computeJacobiansAndErrors<GRAPH_T>(
528 graph, lstObservationData,
529 new_lstJacobians, new_errs);
530 profiler.
leave(
"optimize_graph_spa_levmarq.Jacobians&err");
533 if (new_total_sqr_err < total_sqr_err)
536 new_lstJacobians.swap(lstJacobians);
538 std::swap( new_total_sqr_err, total_sqr_err);
541 have_to_recompute_H_and_grad =
true;
547 for (
typename gst::graph_t::global_poses_t::const_iterator it=old_poses_backup.begin();it!=old_poses_backup.end();++it)
548 graph.nodes[it->first] = it->second;
550 if (verbose ) cout <<
"["<<
__CURRENT_FUNCTION_NAME__<<
"] Got larger error=" << new_total_sqr_err <<
", retrying with a larger lambda...\n";
560 profiler.
leave(
"optimize_graph_spa_levmarq (entire)");