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

make isWithAmbiguity a constructor arg

release/4.3a0
Varun Agrawal 2025-02-06 17:02:39 -05:00
parent 7f8796c223
commit 54c603d627
1 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/ISAM2_City10000.cpp
View File

@ -50,7 +50,7 @@ class Experiment {
// false: run original iSAM2 without ambiguities // false: run original iSAM2 without ambiguities
// true: run original iSAM2 with ambiguities // true: run original iSAM2 with ambiguities
const bool isWithAmbiguity = false; const bool isWithAmbiguity_;
private: private:
ISAM2 isam2_; ISAM2 isam2_;
@ -60,7 +60,8 @@ class Experiment {
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, bool isWithAmbiguity = false)
: dataset_(filename), isWithAmbiguity_(isWithAmbiguity) {
ISAM2Params parameters; ISAM2Params parameters;
parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
parameters.relinearizeThreshold = 0.01; parameters.relinearizeThreshold = 0.01;
@ -71,7 +72,7 @@ class Experiment {
/// @brief Run the main experiment with a given maxLoopCount. /// @brief Run the main experiment with a given maxLoopCount.
void run() { void run() {
// Initialize local variables // Initialize local variables
size_t poseCount = 0, index = 0; size_t index = 0;
std::list<double> timeList; std::list<double> timeList;
@ -79,7 +80,6 @@ class Experiment {
Pose2 priorPose(0, 0, 0); Pose2 priorPose(0, 0, 0);
initial_.insert(X(0), priorPose); initial_.insert(X(0), priorPose);
graph_.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel); graph_.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
poseCount++;
// Initial update // Initial update
isam2_.update(graph_, initial_); isam2_.update(graph_, initial_);
@ -101,7 +101,7 @@ class Experiment {
size_t numMeasurements = poseArray.size(); size_t numMeasurements = poseArray.size();
Pose2 odomPose; Pose2 odomPose;
if (isWithAmbiguity) { if (isWithAmbiguity_) {
// Get wrong intentionally // Get wrong intentionally
int id = index % numMeasurements; int id = index % numMeasurements;
odomPose = Pose2(poseArray[id]); odomPose = Pose2(poseArray[id]);
@ -113,11 +113,10 @@ class Experiment {
initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose); initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose);
graph_.add( graph_.add(
BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel)); BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel));
poseCount++;
} else { // loop } else { // loop
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 {