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

Arguments

release/4.3a0
Frank Dellaert 2025-01-31 15:47:42 -05:00
parent 3f11c5dd01
commit ee2f3c5c2b
1 changed files with 70 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
examples/Hybrid_City10000.cpp
View File

@ -31,11 +31,12 @@
#include <boost/algorithm/string/classification.hpp> #include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp> #include <boost/algorithm/string/split.hpp>
#include <cstdlib>
#include <fstream> #include <fstream>
#include <iostream>
#include <string> #include <string>
#include <vector> #include <vector>
using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::algorithm; using namespace boost::algorithm;
@ -43,25 +44,30 @@ using symbol_shorthand::L;
using symbol_shorthand::M; using symbol_shorthand::M;
using symbol_shorthand::X; using symbol_shorthand::X;
const size_t kMaxLoopCount = 3000; // Example default value
// 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5: 39s} No DT optimizations
// 3000: {1: 65s, 2: 20s, 3: 16s, 4: 21s, 5: 28s} With DT optimizations
// 3000: {1: 59s, 2: 19s, 3: 18s, 4: 26s, 5: 33s} With DT optimizations + merge
const size_t kUpdateFrequency = 3;
const size_t kMaxNrHypotheses = 10;
auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10);
const double kOpenLoopConstant = kOpenLoopModel->negLogConstant();
auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
(Vector(3) << 0.0001, 0.0001, 0.0001).finished()); (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas( auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas(
(Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant();
// Experiment Class // Experiment Class
class Experiment { class Experiment {
public:
// Parameters with default values
size_t maxLoopCount = 3000;
// 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5: 39s} No DT optimizations
// 3000: {1: 65s, 2: 20s, 3: 16s, 4: 21s, 5: 28s} With DT optimizations
// 3000: {1: 59s, 2: 19s, 3: 18s, 4: 26s, 5: 33s} With DT optimizations +
// merge
size_t updateFrequency = 3;
size_t maxNrHypotheses = 10;
private: private:
std::string filename_; std::string filename_;
HybridSmoother smoother_; HybridSmoother smoother_;
@ -78,7 +84,7 @@ class Experiment {
*/ */
void writeResult(const Values& result, size_t numPoses, void writeResult(const Values& result, size_t numPoses,
const std::string& filename = "Hybrid_city10000.txt") { const std::string& filename = "Hybrid_city10000.txt") {
ofstream outfile; std::ofstream outfile;
outfile.open(filename); outfile.open(filename);
for (size_t i = 0; i < numPoses; ++i) { for (size_t i = 0; i < numPoses; ++i) {
@ -104,9 +110,8 @@ class Experiment {
auto f1 = std::make_shared<BetweenFactor<Pose2>>( auto f1 = std::make_shared<BetweenFactor<Pose2>>(
X(keyS), X(keyT), measurement, kPoseNoiseModel); X(keyS), X(keyT), measurement, kPoseNoiseModel);
std::vector<NonlinearFactorValuePair> factors{ std::vector<NonlinearFactorValuePair> factors{{f0, kOpenLoopConstant},
{f0, kOpenLoopModel->negLogConstant()}, {f1, kPoseNoiseConstant}};
{f1, kPoseNoiseModel->negLogConstant()}};
HybridNonlinearFactor mixtureFactor(l, factors); HybridNonlinearFactor mixtureFactor(l, factors);
return mixtureFactor; return mixtureFactor;
} }
@ -114,14 +119,14 @@ class Experiment {
/// @brief Create hybrid odometry factor with discrete measurement choices. /// @brief Create hybrid odometry factor with discrete measurement choices.
HybridNonlinearFactor hybridOdometryFactor( HybridNonlinearFactor hybridOdometryFactor(
size_t numMeasurements, size_t keyS, size_t keyT, const DiscreteKey& m, size_t numMeasurements, size_t keyS, size_t keyT, const DiscreteKey& m,
const std::vector<Pose2>& poseArray, const std::vector<Pose2>& poseArray) {
const SharedNoiseModel& poseNoiseModel) {
auto f0 = std::make_shared<BetweenFactor<Pose2>>( auto f0 = std::make_shared<BetweenFactor<Pose2>>(
X(keyS), X(keyT), poseArray[0], poseNoiseModel); X(keyS), X(keyT), poseArray[0], kPoseNoiseModel);
auto f1 = std::make_shared<BetweenFactor<Pose2>>( auto f1 = std::make_shared<BetweenFactor<Pose2>>(
X(keyS), X(keyT), poseArray[1], poseNoiseModel); X(keyS), X(keyT), poseArray[1], kPoseNoiseModel);
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}}; std::vector<NonlinearFactorValuePair> factors{{f0, kPoseNoiseConstant},
{f1, kPoseNoiseConstant}};
HybridNonlinearFactor mixtureFactor(m, factors); HybridNonlinearFactor mixtureFactor(m, factors);
return mixtureFactor; return mixtureFactor;
} }
@ -129,9 +134,9 @@ class Experiment {
/// @brief Perform smoother update and optimize the graph. /// @brief Perform smoother update and optimize the graph.
void smootherUpdate(HybridSmoother& smoother, void smootherUpdate(HybridSmoother& smoother,
HybridNonlinearFactorGraph& graph, const Values& initial, HybridNonlinearFactorGraph& graph, const Values& initial,
size_t kMaxNrHypotheses, Values* result) { size_t maxNrHypotheses, Values* result) {
HybridGaussianFactorGraph linearized = *graph.linearize(initial); HybridGaussianFactorGraph linearized = *graph.linearize(initial);
smoother.update(linearized, kMaxNrHypotheses); smoother.update(linearized, maxNrHypotheses);
// throw if x0 not in hybridBayesNet_: // throw if x0 not in hybridBayesNet_:
const KeySet& keys = smoother.hybridBayesNet().keys(); const KeySet& keys = smoother.hybridBayesNet().keys();
if (keys.find(X(0)) == keys.end()) { if (keys.find(X(0)) == keys.end()) {
@ -148,11 +153,11 @@ class Experiment {
: filename_(filename), smoother_(0.99) {} : filename_(filename), smoother_(0.99) {}
/// @brief Run the main experiment with a given maxLoopCount. /// @brief Run the main experiment with a given maxLoopCount.
void run(size_t maxLoopCount) { void run() {
// Prepare reading // Prepare reading
ifstream in(filename_); std::ifstream in(filename_);
if (!in.is_open()) { if (!in.is_open()) {
cerr << "Failed to open file: " << filename_ << endl; std::cerr << "Failed to open file: " << filename_ << std::endl;
return; return;
} }
@ -173,7 +178,7 @@ class Experiment {
// Initial update // Initial update
clock_t beforeUpdate = clock(); clock_t beforeUpdate = clock();
smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
clock_t afterUpdate = clock(); clock_t afterUpdate = clock();
std::vector<std::pair<size_t, double>> smootherUpdateTimes; std::vector<std::pair<size_t, double>> smootherUpdateTimes;
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
@ -208,8 +213,8 @@ class Experiment {
if (numMeasurements > 1) { if (numMeasurements > 1) {
// Add hybrid factor // Add hybrid factor
DiscreteKey m(M(discreteCount), numMeasurements); DiscreteKey m(M(discreteCount), numMeasurements);
HybridNonlinearFactor mixtureFactor = hybridOdometryFactor( HybridNonlinearFactor mixtureFactor =
numMeasurements, keyS, keyT, m, poseArray, kPoseNoiseModel); hybridOdometryFactor(numMeasurements, keyS, keyT, m, poseArray);
graph_.push_back(mixtureFactor); graph_.push_back(mixtureFactor);
discreteCount++; discreteCount++;
numberOfHybridFactors += 1; numberOfHybridFactors += 1;
@ -231,12 +236,12 @@ class Experiment {
loopCount++; loopCount++;
} }
if (numberOfHybridFactors>=kUpdateFrequency) { if (numberOfHybridFactors >= updateFrequency) {
// print the keys involved in the smoother update // print the keys involved in the smoother update
std::cout << "Smoother update: " << graph_.size() << std::endl; std::cout << "Smoother update: " << graph_.size() << std::endl;
gttic_(SmootherUpdate); gttic_(SmootherUpdate);
beforeUpdate = clock(); beforeUpdate = clock();
smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
afterUpdate = clock(); afterUpdate = clock();
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
gttoc_(SmootherUpdate); gttoc_(SmootherUpdate);
@ -266,7 +271,7 @@ class Experiment {
// Final update // Final update
beforeUpdate = clock(); beforeUpdate = clock();
smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
afterUpdate = clock(); afterUpdate = clock();
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
@ -296,7 +301,7 @@ class Experiment {
// } // }
// Write timing info to file // Write timing info to file
ofstream outfileTime; std::ofstream outfileTime;
std::string timeFileName = "Hybrid_City10000_time.txt"; std::string timeFileName = "Hybrid_City10000_time.txt";
outfileTime.open(timeFileName); outfileTime.open(timeFileName);
for (auto accTime : timeList) { for (auto accTime : timeList) {
@ -308,15 +313,47 @@ class Experiment {
}; };
/* ************************************************************************* */ /* ************************************************************************* */
int main() { // Function to parse command-line arguments
void parseArguments(int argc, char* argv[], size_t& maxLoopCount,
size_t& updateFrequency, size_t& maxNrHypotheses) {
for (int i = 1; i < argc; ++i) {
std::string arg = argv[i];
if (arg == "--max-loop-count" && i + 1 < argc) {
maxLoopCount = std::stoul(argv[++i]);
} else if (arg == "--update-frequency" && i + 1 < argc) {
updateFrequency = std::stoul(argv[++i]);
} else if (arg == "--max-nr-hypotheses" && i + 1 < argc) {
maxNrHypotheses = std::stoul(argv[++i]);
} else if (arg == "--help") {
std::cout << "Usage: " << argv[0] << " [options]\n"
<< "Options:\n"
<< " --max-loop-count <value> Set the maximum loop "
"count (default: 3000)\n"
<< " --update-frequency <value> Set the update frequency "
"(default: 3)\n"
<< " --max-nr-hypotheses <value> Set the maximum number of "
"hypotheses (default: 10)\n"
<< " --help Show this help message\n";
std::exit(0);
}
}
}
/* ************************************************************************* */
// Main function
int main(int argc, char* argv[]) {
Experiment experiment(findExampleDataFile("T1_city10000_04.txt")); Experiment experiment(findExampleDataFile("T1_city10000_04.txt"));
// Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only // Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only
// Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only // Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only
// Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + // Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 +
// Type #3 // Type #3
// Parse command-line arguments
parseArguments(argc, argv, experiment.maxLoopCount,
experiment.updateFrequency, experiment.maxNrHypotheses);
// Run the experiment // Run the experiment
experiment.run(kMaxLoopCount); experiment.run();
return 0; return 0;
} }