iSAM
Simple iSAM Example

The iSAM library represents general optimization problems as a factor graph, where nodes represent variables and factors represent measurements on sets of one or more nodes.

In this simple SLAM example, five poses are created and connected into a chain by odometry measurements. A landmark is added with a single measurement from one of the poses.

Note that the optimization problem described so far is not fully constrained as the complete graph can be shifted and rotated arbitrarily in the plane without changing the relative measurements described by odometry and landmark measurements. The graph needs to be constraints, and a common solution is to add a prior to the first pose to fix it at the origin.

The following code can be compiled and run with

make examples
bin/example

To compile without the iSAM cmake system:

cd examples
c++ example.cpp ../lib/libisam.a -lcholmod -I../include -I/usr/include/eigen3

examples/example.cpp :

00001 
00053 #include <isam/isam.h>
00054 
00055 using namespace std;
00056 using namespace isam;
00057 using namespace Eigen;
00058 
00059 int main() {
00060   // instance of the main class that manages and optimizes the pose graph
00061   Slam slam;
00062 
00063   // locally remember poses
00064   vector<Pose2d_Node*> pose_nodes;
00065 
00066   Noise noise3 = Information(100. * eye(3));
00067   Noise noise2 = Information(100. * eye(2));
00068 
00069   // create a first pose (a node)
00070   Pose2d_Node* new_pose_node = new Pose2d_Node();
00071   // add it to the graph
00072   slam.add_node(new_pose_node);
00073   // also remember it locally
00074   pose_nodes.push_back(new_pose_node);
00075 
00076   // create a prior measurement (a factor)
00077   Pose2d origin(0., 0., 0.);
00078   Pose2d_Factor* prior = new Pose2d_Factor(pose_nodes[0], origin, noise3);
00079   // add it to the graph
00080   slam.add_factor(prior);
00081 
00082   for (int i=1; i<4; i++) {
00083     // next pose
00084     Pose2d_Node* new_pose_node = new Pose2d_Node();
00085     slam.add_node(new_pose_node);
00086     pose_nodes.push_back(new_pose_node);
00087 
00088     // connect to previous with odometry measurement
00089     Pose2d odometry(1., 0., 0.); // x,y,theta
00090     Pose2d_Pose2d_Factor* constraint = new Pose2d_Pose2d_Factor(pose_nodes[i-1], pose_nodes[i], odometry, noise3);
00091     slam.add_factor(constraint);
00092   }
00093 
00094   // create a landmark
00095   Point2d_Node* new_point_node = new Point2d_Node();
00096   slam.add_node(new_point_node);
00097 
00098   // create a pose and the landmark by a measurement
00099   Point2d measure(5., 3.); // x,y
00100   Pose2d_Point2d_Factor* measurement =
00101     new Pose2d_Point2d_Factor(pose_nodes[1], new_point_node, measure, noise2);
00102   slam.add_factor(measurement);
00103 
00104   // optimize the graph
00105   slam.batch_optimization();
00106 
00107   // accessing the current estimate of a specific pose
00108   cout << "Pose 2: " << pose_nodes[2]->value() << endl;
00109 
00110   // printing the complete graph
00111   cout << endl << "Full graph:" << endl;
00112   slam.write(cout);
00113 
00114   return 0;
00115 }
 All Classes Files Functions Variables