Add files via upload

gtsam/CMakeLists.txt

@@ -0,0 +1,38 @@
+# 声明要求的cmake最低版本
+cmake_minimum_required( VERSION 2.8 )
+
+# 声明一个cmake工程
+project( helloworld )
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
+
+find_package(GTSAM REQUIRED QUIET)
+
+# include directories
+include_directories(
+	${GTSAM_INCLUDE_DIR}
+)
+
+# link directories
+link_directories(
+  ${GTSAM_LIBRARY_DIRS}
+)
+# 添加一个可执行程序
+# 语法为： add_executable( 程序名  源代码文件 ）
+add_executable(gtsam_demo1 GTSAM_demo1.cpp)
+target_link_libraries(gtsam_demo1 gtsam)
+
+add_executable(gtsam_demo2 GTSAM_demo2.cpp)
+target_link_libraries(gtsam_demo2 gtsam)
+
+add_executable(gtsam_demo3 GTSAM_demo3.cpp)
+target_link_libraries(gtsam_demo3 gtsam)
+
+add_executable(gtsam_demo4 GTSAM_demo4.cpp)
+target_link_libraries(gtsam_demo4 gtsam)
+
+
+
+

gtsam/GTSAM_demo1.cpp

@@ -0,0 +1,66 @@
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+using namespace std;
+using namespace gtsam;
+    /**                           @5**********@4
+     *                            *           *             
+     *                            *           *
+     *                            *           *
+     *      **********@1**********@2**********@3
+     * 
+     */ 
+int main(int argc, char** argv) 
+{
+    NonlinearFactorGraph graph;
+
+    Values initials;
+
+    // 给5个状态因子赋初值（相当于没有进行图优化前的状态因子）
+    initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
+    initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
+    initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
+    initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
+    initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));
+    initials.print("\nInitial Values:\n"); 
+
+    //固定第一个顶点，在gtsam中相当于添加一个先验因子（相当于起始位置x1）
+    noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(Vector3(1.0, 1.0, 0.1));
+    graph.add(PriorFactor<Pose2>(Symbol('x', 1), Pose2(0, 0, 0), priorModel));
+
+    //二元位姿因子（相当于状态约束）
+    noiseModel::Diagonal::shared_ptr odomModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
+    graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
+    graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
+    graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
+    graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));
+
+    //二元回环因子（相当于回环约束）
+    noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
+    graph.add(BetweenFactor<Pose2>(Symbol('x', 5), Symbol('x', 2), Pose2(5, 0, -M_PI_2), loopModel));
+    graph.print("\nFactor Graph:\n"); 
+
+
+    GaussNewtonParams parameters;
+    parameters.setVerbosity("ERROR");
+    parameters.setMaxIterations(20);
+    parameters.setLinearSolverType("MULTIFRONTAL_QR");
+    GaussNewtonOptimizer optimizer(graph, initials, parameters);
+    Values results = optimizer.optimize();
+    results.print("Final Result:\n");
+
+    Marginals marginals(graph, results);
+    cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
+    cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
+    cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
+    cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
+    cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;
+
+    return 0;
+}
+

gtsam/GTSAM_demo2.cpp

@@ -0,0 +1,88 @@
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <vector>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/Values.h>
+
+using namespace std;
+using namespace gtsam;
+
+    /**                           @5**********@4
+     *                            *           *             
+     *                            *           *
+     *                            *           *
+     *      **********@1**********@2**********@3
+     * 
+     */ 
+int main()
+{
+
+  //　向量保存好模拟的位姿和测量，到时候一个个往isam2里填加
+  std::vector< BetweenFactor<Pose2> > gra;
+  std::vector< Pose2 > initPose;
+
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+
+  gra.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
+  gra.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
+  gra.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
+  gra.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
+  gra.push_back(BetweenFactor<Pose2>(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));
+
+  ISAM2Params parameters;
+  parameters.relinearizeThreshold = 0.01;
+  parameters.relinearizeSkip = 1;
+  ISAM2 isam(parameters);
+
+  // 注意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<Pose2>(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; 
+}

gtsam/GTSAM_demo3.cpp

@@ -0,0 +1,59 @@
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+using namespace std;
+using namespace gtsam;
+int main(int argc, char** argv) 
+{
+    NonlinearFactorGraph graph;
+
+    Values initials;
+    initials.insert(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0.1, -0.1, 0)));
+    initials.insert(Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(6, 0.1, -1.1)));
+    initials.insert(Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10.1, -0.1, 0.9)));
+    initials.insert(Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10, 5, 0)));
+    initials.insert(Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5.1, 4.9, -0.9)));
+    initials.print("\nInitial Values:\n"); 
+    //固定第一个顶点，在gtsam中相当于添加一个先验因子
+    noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
+          (Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); 
+    graph.add(PriorFactor<Pose3>(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));
+
+    //二元位姿因子
+    noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances(
+          (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+    graph.add(BetweenFactor<Pose3>(Symbol('x', 1), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), odometryNoise));
+    graph.add(BetweenFactor<Pose3>(Symbol('x', 2), Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), odometryNoise));
+    graph.add(BetweenFactor<Pose3>(Symbol('x', 3), Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), odometryNoise));
+    graph.add(BetweenFactor<Pose3>(Symbol('x', 4), Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(-5, 0, -1)), odometryNoise));
+
+    //二元回环因子
+    noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Variances(
+          (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+    graph.add(BetweenFactor<Pose3>(Symbol('x', 5), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, -5, 0)), loopModel));
+    graph.print("\nFactor Graph:\n"); 
+
+
+    GaussNewtonParams parameters;
+    parameters.setVerbosity("ERROR");
+    parameters.setMaxIterations(20);
+    parameters.setLinearSolverType("MULTIFRONTAL_QR");
+    GaussNewtonOptimizer optimizer(graph, initials, parameters);
+    Values results = optimizer.optimize();
+    results.print("Final Result:\n");
+
+    Marginals marginals(graph, results);
+    cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
+    cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
+    cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
+    cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
+    cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;
+
+    return 0;
+}

gtsam/GTSAM_demo4.cpp

@@ -0,0 +1,80 @@
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <vector>
+#include <gtsam/inference/Key.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Pose3.h>
+
+using namespace std;
+using namespace gtsam;
+
+int main()
+{
+
+  //　向量保存好模拟的位姿和测量，到时候一个个往isam2里填加
+  std::vector< BetweenFactor<Pose3> > gra;
+  std::vector< Pose3 > initPose;
+
+  // For simplicity, we will use the same noise model for odometry and loop closures
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(
+          (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+
+  gra.push_back(BetweenFactor<Pose3>(1, 2, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), model));
+  gra.push_back(BetweenFactor<Pose3>(2, 3, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), model));
+  gra.push_back(BetweenFactor<Pose3>(3, 4, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), model));
+  gra.push_back(BetweenFactor<Pose3>(4, 5, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(-5, 0, -1)), model));
+  gra.push_back(BetweenFactor<Pose3>(5, 2, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, -5, 0)), model));
+
+  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0.1, -0.1, 0)));
+  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(6, 0.1, -1.1)));
+  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10.1, -0.1, 0.9)));
+  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10, 5, 0)));
+  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5.1, 4.9, -0.9)));
+
+  ISAM2Params parameters;
+  parameters.relinearizeThreshold = 0.01;
+  parameters.relinearizeSkip = 1;
+  ISAM2 isam(parameters);
+
+  NonlinearFactorGraph graph;
+  Values initialEstimate;
+
+  for( int i =0; i<5 ;i++)
+  {
+           initialEstimate.insert(i+1, initPose[i]);
+
+           if(i == 0)
+           {
+               noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
+		(Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); 
+               graph.push_back(PriorFactor<Pose3>(1, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(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; 
+}

gtsam/helloworld.cpp

@@ -0,0 +1,7 @@
+#include <iostream>
+using namespace std;
+int main()
+{
+    cout <<"Hello world!"<< endl;
+    return 0;
+}