1/**
2 * @file Pose2SLAMStressTest.cpp
3 * @brief Test GTSAM on large open-loop chains
4 * @date May 23, 2018
5 * @author Wenqiang Zhou
6 */
7
8// Create N 3D poses, add relative motion between each consecutive poses. (The
9// relative motion is simply a unit translation(1, 0, 0), no rotation). For each
10// each pose, add some random noise to the x value of the translation part.
11// Use gtsam to create a prior factor for the first pose and N-1 between factors
12// and run optimization.
13
14#include <gtsam/geometry/Cal3_S2Stereo.h>
15#include <gtsam/geometry/Pose3.h>
16#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
17#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
18#include <gtsam/nonlinear/NonlinearEquality.h>
19#include <gtsam/nonlinear/NonlinearFactorGraph.h>
20#include <gtsam/nonlinear/Values.h>
21#include <gtsam/slam/BetweenFactor.h>
22#include <gtsam/slam/StereoFactor.h>
23
24#include <random>
25
26using namespace std;
27using namespace gtsam;
28
29void testGtsam(int numberNodes) {
30 std::random_device rd;
31 std::mt19937 e2(rd());
32 std::uniform_real_distribution<> dist(0, 1);
33
34 vector<Pose3> poses;
35 for (int i = 0; i < numberNodes; ++i) {
36 Matrix4 M;
37 double r = dist(e2);
38 r = (r - 0.5) / 10 + i;
39 M << 1, 0, 0, r, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
40 poses.push_back(x: Pose3(M));
41 }
42
43 // prior factor for the first pose
44 auto priorModel = noiseModel::Isotropic::Variance(dim: 6, variance: 1e-4);
45 Matrix4 first_M;
46 first_M << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
47 Pose3 first = Pose3(first_M);
48
49 NonlinearFactorGraph graph;
50 graph.addPrior(key: 0, prior: first, model: priorModel);
51
52 // vo noise model
53 auto VOCovarianceModel = noiseModel::Isotropic::Variance(dim: 6, variance: 1e-3);
54
55 // relative VO motion
56 Matrix4 vo_M;
57 vo_M << 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
58 Pose3 relativeMotion(vo_M);
59 for (int i = 0; i < numberNodes - 1; ++i) {
60 graph.add(
61 factorOrContainer: BetweenFactor<Pose3>(i, i + 1, relativeMotion, VOCovarianceModel));
62 }
63
64 // inital values
65 Values initial;
66 for (int i = 0; i < numberNodes; ++i) {
67 initial.insert(j: i, val: poses[i]);
68 }
69
70 LevenbergMarquardtParams params;
71 params.setVerbosity("ERROR");
72 params.setOrderingType("METIS");
73 params.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
74 LevenbergMarquardtOptimizer optimizer(graph, initial, params);
75 auto result = optimizer.optimize();
76}
77
78int main(int args, char* argv[]) {
79 int numberNodes = stoi(str: argv[1]);
80 cout << "number of_nodes: " << numberNodes << endl;
81 testGtsam(numberNodes);
82 return 0;
83}
84