isam2 优化pose graph

gtsam里面只有一个isam2的例子，那个例子里面没有添加位姿闭环约束，主要是视觉BA。而通过闭环优化位姿的gtsam程序主要是Pose2SLAMExample.cpp等，这种用法类似g2o，不能体现isam2的增量优化特性，因此我仿照Pose2SLAMExample里的数据写了一个增量优化位姿的isam2程序，用法上还是有isam2的特性，特别注意graph里的只有isam2优化以后新加的约束，具体见代码。

/ * A simple 2D pose slam example * - The robot moves in a 2 meter square * - The robot moves 2 meters each step, turning 90 degrees after each step * - The robot initially faces along the X axis (horizontal, to the right in 2D) * - We have full odometry between pose * - We have a loop closure constraint when the robot returns to the first position * x5 -- x4 * | | * x1 -- x2 -- x3 */ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols #include 
    // We want to use iSAM2 to solve the pose optimazation problem incrementally, so // include iSAM2 here #include 
    // iSAM2 requires as input a set set of new factors to be added stored in a factor graph, // and initial guesses for any new variables used in the added factors #include 
    #include 
    #include 
    // In planar SLAM example we use Pose2 variables (x, y, theta) to represent the robot poses #include 
    // We will use simple integer Keys to refer to the robot poses. #include 
    // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // We will also use a Between Factor to encode the loop closure constraint // Also, we will initialize the robot at the origin using a Prior factor. #include 
    #include 
    // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the // nonlinear functions around an initial linearization point, then solve the linear system // to update the linearization point. This happens repeatedly until the solver converges // to a consistent set of variable values. This requires us to specify an initial guess // for each variable, held in a Values container. #include 
    using namespace std; using namespace gtsam; int main() { //　向量保存好模拟的位姿和测量，到时候一个个往isam2里填加 std::vector< BetweenFactor 
    > gra; std::vector< Pose2 > initPose; // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); gra.push_back(BetweenFactor 
  
    ( 
   1, 
   2, Pose2( 
   2, 
   0, 
   0 ), model)); gra.push_back(BetweenFactor 
   
     ( 
    2, 
    3, Pose2( 
    2, 
    0, M_PI_2), model)); gra.push_back(BetweenFactor 
    
      ( 
     3, 
     4, Pose2( 
     2, 
     0, M_PI_2), model)); gra.push_back(BetweenFactor 
     
       ( 
      4, 
      5, Pose2( 
      2, 
      0, M_PI_2), model)); gra.push_back(BetweenFactor 
      
        ( 
       5, 
       2, Pose2( 
       2, 
       0, M_PI_2), model)); initPose.push_back(Pose2( 
       0.5, 
       0.0, 
       0.2 )); initPose.push_back( Pose2( 
       2.3, 
       0.1, - 
       0.2 )); initPose.push_back( Pose2( 
       4.1, 
       0.1, M_PI_2)); initPose.push_back( Pose2( 
       4.0, 
       2.0, M_PI )); initPose.push_back( Pose2( 
       2.1, 
       2.1, -M_PI_2)); 
       // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization 
       // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter 
       // structure is available that allows the user to set various properties, such as the relinearization threshold 
       // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result 
       // will approach the batch result. ISAM2Params parameters; parameters.relinearizeThreshold = 
       0.01; parameters.relinearizeSkip = 
       1; ISAM2 isam(parameters); 
       // Create a Factor Graph and Values to hold the new data 
       // 注意isam2的graph里只添加isam2更新状态以后新测量到的约束 NonlinearFactorGraph graph; Values initialEstimate; 
       // the first pose don't need to update 
       for( 
       int i = 
       0; i< 
       5 ;i++) { 
       // Add an initial guess for the current pose initialEstimate.insert(i+ 
       1, initPose[i]); 
       if(i == 
       0) { 
       // Add a prior on the first pose, setting it to the origin 
       // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal:: 
       shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3( 
       0.3, 
       0.3, 
       0.1)); graph.push_back(PriorFactor 
       
         ( 
        1, Pose2( 
        0, 
        0, 
        0), priorNoise)); } 
        else { graph.push_back(gra[i- 
        1]); 
        // ie: when i = 1 , robot at pos2, there is a edge gra[0] between pos1 and pos2 
        if(i == 
        4) { graph.push_back(gra[ 
        4]); 
        // when robot at pos5, there two edge, one is pos4 ->pos5, another is pos5->pos2 (grad[4]) } isam.update(graph, initialEstimate); isam.update(); Values currentEstimate = isam.calculateEstimate(); 
        cout << 
        "" << endl; 
        cout << 
        "Frame " << i << 
        ": " << endl; currentEstimate.print( 
        "Current estimate: "); 
        // Clear the factor graph and values for the next iteration 
        // 特别重要，update以后，清空原来的约束。已经加入到isam2的那些会用bayes tree保管，你不用操心了。 graph.resize( 
        0); initialEstimate.clear(); } } 
        return 
        0; } 
        
       
      
     
    
  

这个程序的优化结果和Pose2SLAMExample.cpp几乎一样，但是使用的是增量优化的方式，有其独特的优点。