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

Re-factor line parsing, clean up update

release/4.3a0
Frank Dellaert 2025-01-31 22:33:55 -05:00
parent c37eb49547
commit eb9c6d4356
1 changed files with 64 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
examples/Hybrid_City10000.cpp
View File

@ -68,12 +68,13 @@ class Experiment {
size_t maxNrHypotheses = 10; size_t maxNrHypotheses = 10;
size_t reLinearizationFrequency = 1;
private: private:
std::string filename_; std::string filename_;
HybridSmoother smoother_; HybridSmoother smoother_;
HybridNonlinearFactorGraph graph_; HybridNonlinearFactorGraph newFactors_;
Values initial_; Values initial_;
Values result_;
/** /**
* @brief Write the result of optimization to file. * @brief Write the result of optimization to file.
@ -83,7 +84,7 @@ class Experiment {
* @param filename The file name to save the result to. * @param filename The file name to save the result to.
*/ */
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") const {
std::ofstream outfile; std::ofstream outfile;
outfile.open(filename); outfile.open(filename);
@ -100,9 +101,9 @@ class Experiment {
* @brief Create a hybrid loop closure factor where * @brief Create a hybrid loop closure factor where
* 0 - loose noise model and 1 - loop noise model. * 0 - loose noise model and 1 - loop noise model.
*/ */
HybridNonlinearFactor hybridLoopClosureFactor(size_t loopCounter, size_t keyS, HybridNonlinearFactor hybridLoopClosureFactor(
size_t keyT, size_t loopCounter, size_t keyS, size_t keyT,
const Pose2& measurement) { const Pose2& measurement) const {
DiscreteKey l(L(loopCounter), 2); DiscreteKey l(L(loopCounter), 2);
auto f0 = std::make_shared<BetweenFactor<Pose2>>( auto f0 = std::make_shared<BetweenFactor<Pose2>>(
@ -119,7 +120,7 @@ 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 {
auto f0 = std::make_shared<BetweenFactor<Pose2>>( auto f0 = std::make_shared<BetweenFactor<Pose2>>(
X(keyS), X(keyT), poseArray[0], kPoseNoiseModel); X(keyS), X(keyT), poseArray[0], kPoseNoiseModel);
auto f1 = std::make_shared<BetweenFactor<Pose2>>( auto f1 = std::make_shared<BetweenFactor<Pose2>>(
@ -132,19 +133,34 @@ class Experiment {
} }
/// @brief Perform smoother update and optimize the graph. /// @brief Perform smoother update and optimize the graph.
void smootherUpdate(HybridSmoother& smoother, auto smootherUpdate(size_t maxNrHypotheses) {
HybridNonlinearFactorGraph& graph, const Values& initial, gttic_(SmootherUpdate);
size_t maxNrHypotheses, Values* result) { clock_t beforeUpdate = clock();
HybridGaussianFactorGraph linearized = *graph.linearize(initial); auto linearized = newFactors_.linearize(initial_);
smoother.update(linearized, maxNrHypotheses); smoother_.update(*linearized, maxNrHypotheses);
// throw if x0 not in hybridBayesNet_: newFactors_.resize(0);
const KeySet& keys = smoother.hybridBayesNet().keys(); clock_t afterUpdate = clock();
if (keys.find(X(0)) == keys.end()) { return afterUpdate - beforeUpdate;
throw std::runtime_error("x0 not in hybridBayesNet_"); }
// Parse line from file
std::pair<std::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
const std::string& line) const {
std::vector<std::string> parts;
split(parts, line, is_any_of(" "));
size_t keyS = stoi(parts[1]);
size_t keyT = stoi(parts[3]);
int numMeasurements = stoi(parts[5]);
std::vector<Pose2> poseArray(numMeasurements);
for (int i = 0; i < numMeasurements; ++i) {
double x = stod(parts[6 + 3 * i]);
double y = stod(parts[7 + 3 * i]);
double rad = stod(parts[8 + 3 * i]);
poseArray[i] = Pose2(x, y, rad);
} }
graph.resize(0); return {poseArray, {keyS, keyT}};
// HybridValues delta = smoother.hybridBayesNet().optimize();
// result->insert_or_assign(initial.retract(delta.continuous()));
} }
public: public:
@ -162,49 +178,34 @@ class Experiment {
} }
// Initialize local variables // Initialize local variables
size_t discreteCount = 0, index = 0; size_t discreteCount = 0, index = 0, loopCount = 0, updateCount = 0;
size_t loopCount = 0;
std::list<double> timeList; std::list<double> timeList;
// Set up initial prior // Set up initial prior
double x = 0.0; Pose2 priorPose(0, 0, 0);
double y = 0.0;
double rad = 0.0;
Pose2 priorPose(x, y, rad);
initial_.insert(X(0), priorPose); initial_.insert(X(0), priorPose);
graph_.push_back(PriorFactor<Pose2>(X(0), priorPose, kPriorNoiseModel)); newFactors_.push_back(
PriorFactor<Pose2>(X(0), priorPose, kPriorNoiseModel));
// Initial update // Initial update
clock_t beforeUpdate = clock(); auto time = smootherUpdate(maxNrHypotheses);
smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
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, time});
// Flag to decide whether to run smoother update // Flag to decide whether to run smoother update
size_t numberOfHybridFactors = 0; size_t numberOfHybridFactors = 0;
// Start main loop // Start main loop
Values result;
size_t keyS = 0, keyT = 0; size_t keyS = 0, keyT = 0;
clock_t startTime = clock(); clock_t startTime = clock();
std::string line; std::string line;
while (getline(in, line) && index < maxLoopCount) { while (getline(in, line) && index < maxLoopCount) {
std::vector<std::string> parts; auto [poseArray, keys] = parseLine(line);
split(parts, line, is_any_of(" ")); keyS = keys.first;
keyT = keys.second;
keyS = stoi(parts[1]); size_t numMeasurements = poseArray.size();
keyT = stoi(parts[3]);
int numMeasurements = stoi(parts[5]);
std::vector<Pose2> poseArray(numMeasurements);
for (int i = 0; i < numMeasurements; ++i) {
x = stod(parts[6 + 3 * i]);
y = stod(parts[7 + 3 * i]);
rad = stod(parts[8 + 3 * i]);
poseArray[i] = Pose2(x, y, rad);
}
// Take the first one as the initial estimate // Take the first one as the initial estimate
Pose2 odomPose = poseArray[0]; Pose2 odomPose = poseArray[0];
@ -215,13 +216,13 @@ class Experiment {
DiscreteKey m(M(discreteCount), numMeasurements); DiscreteKey m(M(discreteCount), numMeasurements);
HybridNonlinearFactor mixtureFactor = HybridNonlinearFactor mixtureFactor =
hybridOdometryFactor(numMeasurements, keyS, keyT, m, poseArray); hybridOdometryFactor(numMeasurements, keyS, keyT, m, poseArray);
graph_.push_back(mixtureFactor); newFactors_.push_back(mixtureFactor);
discreteCount++; discreteCount++;
numberOfHybridFactors += 1; numberOfHybridFactors += 1;
std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl; std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl;
} else { } else {
graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, newFactors_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
kPoseNoiseModel)); kPoseNoiseModel));
} }
// Insert next pose initial guess // Insert next pose initial guess
initial_.insert(X(keyT), initial_.at<Pose2>(X(keyS)) * odomPose); initial_.insert(X(keyT), initial_.at<Pose2>(X(keyS)) * odomPose);
@ -231,21 +232,24 @@ class Experiment {
hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose); hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose);
// print loop closure event keys: // print loop closure event keys:
std::cout << "Loop closure: " << keyS << " " << keyT << std::endl; std::cout << "Loop closure: " << keyS << " " << keyT << std::endl;
graph_.add(loopFactor); newFactors_.add(loopFactor);
numberOfHybridFactors += 1; numberOfHybridFactors += 1;
loopCount++; loopCount++;
} }
if (numberOfHybridFactors >= updateFrequency) { 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: " << newFactors_.size() << std::endl;
gttic_(SmootherUpdate); auto time = smootherUpdate(maxNrHypotheses);
beforeUpdate = clock(); smootherUpdateTimes.push_back({index, time});
smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
afterUpdate = clock();
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
gttoc_(SmootherUpdate);
numberOfHybridFactors = 0; numberOfHybridFactors = 0;
updateCount++;
if (updateCount % reLinearizationFrequency == 0) {
std::cout << "Re-linearizing: " << newFactors_.size() << std::endl;
HybridValues delta = smoother_.optimize();
result.insert_or_assign(initial_.retract(delta.continuous()));
}
} }
// Record timing for odometry edges only // Record timing for odometry edges only
@ -270,17 +274,15 @@ class Experiment {
} }
// Final update // Final update
beforeUpdate = clock(); time = smootherUpdate(maxNrHypotheses);
smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); smootherUpdateTimes.push_back({index, time});
afterUpdate = clock();
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
// Final optimize // Final optimize
gttic_(HybridSmootherOptimize); gttic_(HybridSmootherOptimize);
HybridValues delta = smoother_.optimize(); HybridValues delta = smoother_.optimize();
gttoc_(HybridSmootherOptimize); gttoc_(HybridSmootherOptimize);
result_.insert_or_assign(initial_.retract(delta.continuous())); result.insert_or_assign(initial_.retract(delta.continuous()));
std::cout << "Final error: " << smoother_.hybridBayesNet().error(delta) std::cout << "Final error: " << smoother_.hybridBayesNet().error(delta)
<< std::endl; << std::endl;
@ -291,7 +293,7 @@ class Experiment {
<< std::endl; << std::endl;
// Write results to file // Write results to file
writeResult(result_, keyT + 1, "Hybrid_City10000.txt"); writeResult(result, keyT + 1, "Hybrid_City10000.txt");
// TODO Write to file // TODO Write to file
// for (size_t i = 0; i < smoother_update_times.size(); i++) { // for (size_t i = 0; i < smoother_update_times.size(); i++) {