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

working example

release/4.3a0
Varun Agrawal 2025-01-23 12:52:45 -05:00
parent 7c24448a2b
commit ea82d51d10
1 changed files with 55 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
examples/Hybrid_City10000.cpp
View File

@ -18,9 +18,11 @@
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearISAM.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
@ -37,13 +39,11 @@ using namespace std;
using namespace gtsam;
using namespace boost::algorithm;
using symbol_shorthand::M;
using symbol_shorthand::X;
// Testing params
const size_t max_loop_count = 2000; // 200 //2000 //8000
const bool is_with_ambiguity = false; // run original iSAM2 without ambiguities
// const bool is_with_ambiguity = true; // run original iSAM2 with ambiguities
const size_t max_loop_count = 1000; // 2000; // 200 //2000 //8000
noiseModel::Diagonal::shared_ptr prior_noise_model =
noiseModel::Diagonal::Sigmas(
@ -51,7 +51,7 @@ noiseModel::Diagonal::shared_ptr prior_noise_model =
noiseModel::Diagonal::shared_ptr pose_noise_model =
noiseModel::Diagonal::Sigmas(
(Vector(3) << 1.0 / 50.0, 1.0 / 50.0, 1.0 / 100.0).finished());
(Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
/**
* @brief Write the results of optimization to filename.
@ -84,25 +84,20 @@ int main(int argc, char* argv[]) {
// ifstream in("../data/mh_All_city10000_groundtruth.txt");
size_t pose_count = 0;
size_t pose_count = 0, discrete_count = 0;
size_t index = 0;
std::list<double> time_list;
ISAM2Params parameters;
HybridNonlinearISAM isam;
parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2* isam2 = new ISAM2(parameters);
NonlinearFactorGraph* graph = new NonlinearFactorGraph();
HybridNonlinearFactorGraph graph;
Values init_values;
Values results;
size_t maxNrHypotheses = 3;
double x = 0.0;
double y = 0.0;
double rad = 0.0;
@ -110,23 +105,21 @@ int main(int argc, char* argv[]) {
Pose2 prior_pose(x, y, rad);
init_values.insert(X(0), prior_pose);
pose_count++;
pose_count += 1;
graph->add(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model));
graph.push_back(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model));
isam2->update(*graph, init_values);
graph->resize(0);
isam.update(graph, init_values, maxNrHypotheses);
graph.resize(0);
init_values.clear();
results = isam2->calculateBestEstimate();
results = isam.estimate();
//*
size_t key_s = 0;
size_t key_t = 0;
size_t key_s, key_t;
clock_t start_time = clock();
string str;
std::string str;
while (getline(in, str) && index < max_loop_count) {
// cout << str << endl;
vector<string> parts;
split(parts, str, is_any_of(" "));
@ -142,35 +135,47 @@ int main(int argc, char* argv[]) {
pose_array[i] = Pose2(x, y, rad);
}
Pose2 odom_pose;
if (is_with_ambiguity) {
// Get wrong intentionally
int id = index % num_measurements;
odom_pose = Pose2(pose_array[id]);
} else {
odom_pose = pose_array[0];
}
// Take the first one as the initial estimate
Pose2 odom_pose = pose_array[0];
if (key_s == key_t - 1) { // new X(key)
init_values.insert(X(key_t), results.at<Pose2>(X(key_s)) * odom_pose);
pose_count++;
} else { // loop
index++;
// index++;
}
graph->add(
BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, pose_noise_model));
isam2->update(*graph, init_values);
graph->resize(0);
if (num_measurements == 2) {
// Add hybrid factor which considers both measurements
DiscreteKey m(M(discrete_count), num_measurements);
discrete_count++;
graph.push_back(DecisionTreeFactor(m, "0.6 0.4"));
auto f0 = std::make_shared<BetweenFactor<Pose2>>(
X(key_s), X(key_t), pose_array[0], pose_noise_model);
auto f1 = std::make_shared<BetweenFactor<Pose2>>(
X(key_s), X(key_t), pose_array[1], pose_noise_model);
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
// HybridNonlinearFactor mixtureFactor(m, factors);
HybridNonlinearFactor mixtureFactor(m, {f0, f1});
graph.push_back(mixtureFactor);
} else {
graph.add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose,
pose_noise_model));
}
isam.update(graph, init_values, maxNrHypotheses);
graph.resize(0);
init_values.clear();
results = isam2->calculateBestEstimate();
results = isam.estimate();
isam.assignment().print("The Discrete Assignment");
//*
// Print loop index and time taken in processor clock ticks
if (index % 50 == 0 && key_s != key_t - 1) {
std::cout << "index: " << index << std::endl;
std::cout << "acc_time: " << time_list.back() << std::endl;
}
// */
if (key_s == key_t - 1) {
clock_t cur_time = clock();
@ -181,7 +186,8 @@ int main(int argc, char* argv[]) {
string step_file_idx = std::to_string(100000 + time_list.size());
ofstream step_outfile;
string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx;
string step_file_name =
"step_files/HybridISAM_city10000_S" + step_file_idx;
step_outfile.open(step_file_name + ".txt");
for (size_t i = 0; i < (key_t + 1); ++i) {
Pose2 out_pose = results.at<Pose2>(X(i));
@ -190,6 +196,8 @@ int main(int argc, char* argv[]) {
}
step_outfile.close();
}
index += 1;
}
clock_t end_time = clock();
@ -197,10 +205,10 @@ int main(int argc, char* argv[]) {
cout << "total_time: " << total_time << endl;
/// Write results to file
write_results(results, (key_t + 1));
write_results(results, (key_t + 1), "HybridISAM_city10000.txt");
ofstream outfile_time;
std::string time_file_name = "ISAM2_city10000_time.txt";
std::string time_file_name = "HybridISAM_city10000_time.txt";
outfile_time.open(time_file_name);
for (auto acc_time : time_list) {
outfile_time << acc_time << std::endl;