simulations/isam/isam/isam.cpp
/**
 * Calculate covariances for both points and poses up to the given time step.
 */
Calculate covariances for both points and poses up to the given time step.
Calculate covariances for both points and poses up to the given time step .
void covariances(unsigned int step, list<MatrixXd>& point_marginals,
    list<MatrixXd>& pose_marginals) {
  point_marginals.clear();
  pose_marginals.clear();
  Covariances::node_lists_t node_lists;
  for (unsigned int i = 0; i < step; i++) {
    list<Node*> entry;
    entry.push_back(loader->pose_nodes()[i]);
    node_lists.push_back(entry);
  }
  for (unsigned int i = 0; i < loader->num_points(step); i++) {
    list<Node*> entry;
    entry.push_back(loader->point_nodes()[i]);
    node_lists.push_back(entry);
  }
  pose_marginals = slam.covariances().marginal(node_lists);
  if (pose_marginals.size() > 0) {
    list<MatrixXd>::iterator center = pose_marginals.begin();
    for (unsigned int i = 0; i < step; i++, center++)
      ;
    point_marginals.splice(point_marginals.begin(), pose_marginals, center,
        pose_marginals.end());
  }
}
void covariances ( unsigned int step , list < MatrixXd > & point_marginals , list < MatrixXd > & pose_marginals ) { point_marginals . clear ( ) ; pose_marginals . clear ( ) ; Covariances :: node_lists_t node_lists ; for ( unsigned int i = 0 ; i < step ; i ++ ) { list < Node * > entry ; entry . push_back ( loader -> pose_nodes ( ) [ i ] ) ; node_lists . push_back ( entry ) ; } for ( unsigned int i = 0 ; i < loader -> num_points ( step ) ; i ++ ) { list < Node * > entry ; entry . push_back ( loader -> point_nodes ( ) [ i ] ) ; node_lists . push_back ( entry ) ; } pose_marginals = slam . covariances ( ) . marginal ( node_lists ) ; if ( pose_marginals . size ( ) > 0 ) { list < MatrixXd > :: iterator center = pose_marginals . begin ( ) ; for ( unsigned int i = 0 ; i < step ; i ++ , center ++ ) ; point_marginals . splice ( point_marginals . begin ( ) , pose_marginals , center , pose_marginals . end ( ) ) ; } }
Calculate covariances for both points and poses up to the given time step.
Calculate covariances for both points and poses up to the given time step .
// make sure return arguments are empty // combining everything into one call is faster, // as it avoids recalculating commonly needed entries // split into points and poses