12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix initialization

release/4.3a0
Varun Agrawal 2025-02-06 16:58:49 -05:00
parent d511779f40
commit 7f8796c223
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/ISAM2_City10000.cpp
View File

@ -53,19 +53,20 @@ class Experiment {
const bool isWithAmbiguity = false; const bool isWithAmbiguity = false;
private: private:
ISAM2Params parameters; ISAM2 isam2_;
parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2 isam2_(parameters);
NonlinearFactorGraph graph_; NonlinearFactorGraph graph_;
Values initial_; Values initial_;
Values results; Values results;
public: public:
/// Construct with filename of experiment to run /// Construct with filename of experiment to run
explicit Experiment(const std::string& filename) : dataset_(filename) {} explicit Experiment(const std::string& filename) : dataset_(filename) {
ISAM2Params parameters;
parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
isam2_ = ISAM2(parameters);
}
/// @brief Run the main experiment with a given maxLoopCount. /// @brief Run the main experiment with a given maxLoopCount.
void run() { void run() {
@ -118,7 +119,7 @@ class Experiment {
int id = index % numMeasurements; int id = index % numMeasurements;
if (isWithAmbiguity && id % 2 == 0) { if (isWithAmbiguity && id % 2 == 0) {
graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
kPoseNoiseModel)); kPoseNoiseModel));
} else { } else {
graph_.add(BetweenFactor<Pose2>( graph_.add(BetweenFactor<Pose2>(
X(keyS), X(keyT), odomPose, X(keyS), X(keyT), odomPose,