void print_all(Slam& slam) {
list<Node*> ids = slam.get_nodes();
for (list<Node*>::iterator iter = ids.begin(); iter != ids.end(); iter++) {
Pose2d_Node* pose = dynamic_cast<Pose2d_Node*>(*iter);
cout << pose->value() << endl;
}
}
Covariances::Covariances(Slam& slam) : _slam(NULL), _R(slam._R) {
// update pointers into matrix before making a copy
slam.update_starts();
const list<Node*>& nodes = slam.get_nodes();
for (list<Node*>::const_iterator it = nodes.begin(); it!=nodes.end(); it++) {
Node* node = *it;
int start = node->_start;
int dim = node->dim();
_index[node] = make_pair(start, dim);
}
}
/**
* Incrementally process factors.
*/
void incremental_slam() {
unsigned int step = 0;
unsigned int next_step = step;
// step by step after reading log file
for (; loader->more_data(&next_step); step = next_step) {
check_quit();
double t0 = tic();
tic("setup");
// add new variables and constraints for current step
for (unsigned int s = step; s < next_step; s++) {
for (list<Node*>::const_iterator it = loader->nodes(s).begin();
it != loader->nodes(s).end(); it++) {
if (prop.verbose)
cout << **it << endl;
slam.add_node(*it);
}
for (list<Factor*>::const_iterator it = loader->factors(s).begin();
it != loader->factors(s).end(); it++) {
if (prop.verbose)
cout << **it << endl;
slam.add_factor(*it);
}
}
toc("setup");
tic("incremental");
if (!(batch_processing || no_optimization)) {
slam.update();
}
toc("incremental");
if (save_stats) {
stats.resize(step + 1);
stats[step].time = toc(t0);
stats[step].chi2 = slam.normalized_chi2();
stats[step].nnz = slam.get_R().nnz();
stats[step].local_chi2 = slam.local_chi2(100); // last 100 constraints
stats[step].nnodes = slam.get_nodes().size();
stats[step].nconstraints = slam.get_factors().size();
}
// visualization is not counted in timing
if (!(batch_processing || no_optimization)) {
visualize(step);
}
}
visualize(step - 1);
if (!no_optimization) {
if (batch_processing) {
tic("batch");
slam.batch_optimization();
toc("batch");
} else {
// end with a batch step/relinearization
prop.mod_batch = 1;
slam.set_properties(prop);
tic("final");
slam.update();
toc("final");
}
}
visualize(step - 1);
}
int main(int argc, const char* argv[]) {
Pose2d origin;
Noise noise = SqrtInformation(10. * eye(3));
Pose2d_Node* pose_node_1 = new Pose2d_Node(); // create node
slam.add_node(pose_node_1); // add it to the Slam graph
Pose2d_Factor* prior = new Pose2d_Factor(pose_node_1, origin, noise); // create prior measurement, an factor
slam.add_factor(prior); // add it to the Slam graph
Pose2d_Node* pose_node_2 = new Pose2d_Node(); // create node
slam.add_node(pose_node_2); // add it to the Slam graph
Pose2d delta(1., 0., 0.);
Pose2d_Pose2d_Factor* odo = new Pose2d_Pose2d_Factor(pose_node_1, pose_node_2, delta, noise);
slam.add_factor(odo);
slam.batch_optimization();
#if 0
const Covariances& covariances = slam.covariances();
#else
// operate on a copy (just an example, cloning is useful for running
// covariance recovery in a separate thread)
const Covariances& covariances = slam.covariances().clone();
#endif
// recovering the full covariance matrix
cout << "Full covariance matrix:" << endl;
MatrixXd cov_full = covariances.marginal(slam.get_nodes());
cout << cov_full << endl << endl;
// sanity checking by inverting the information matrix, not using R
SparseSystem Js = slam.jacobian();
MatrixXd J(Js.num_cols(), Js.num_cols());
for (int r=0; r<Js.num_cols(); r++) {
for (int c=0; c<Js.num_cols(); c++) {
J(r,c) = Js(r,c);
}
}
MatrixXd H = J.transpose() * J;
MatrixXd cov_full2 = H.inverse();
cout << cov_full2 << endl;
// recovering the block-diagonals only of the full covariance matrix
cout << "Block-diagonals only:" << endl;
Covariances::node_lists_t node_lists;
list<Node*> nodes;
nodes.push_back(pose_node_1);
node_lists.push_back(nodes);
nodes.clear();
nodes.push_back(pose_node_2);
node_lists.push_back(nodes);
list<MatrixXd> cov_blocks = covariances.marginal(node_lists);
int i = 1;
for (list<MatrixXd>::iterator it = cov_blocks.begin(); it!=cov_blocks.end(); it++, i++) {
cout << "block " << i << endl;
cout << *it << endl;
}
// recovering individual entries, here the right block column
cout << "Right block column:" << endl;
Covariances::node_pair_list_t node_pair_list;
node_pair_list.push_back(make_pair(pose_node_1, pose_node_2));
node_pair_list.push_back(make_pair(pose_node_2, pose_node_2));
list<MatrixXd> cov_entries = covariances.access(node_pair_list);
for (list<MatrixXd>::iterator it = cov_entries.begin(); it!=cov_entries.end(); it++) {
cout << *it << endl;
}
}