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