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

major refactor to add Experiment class

release/4.3a0
Varun Agrawal 2025-02-06 16:32:24 -05:00
parent fa371e1415
commit a18857a117
1 changed files with 130 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
examples/ISAM2_City10000.cpp
View File

@ -38,97 +38,65 @@ using namespace boost::algorithm;
using symbol_shorthand::X; using symbol_shorthand::X;
// Testing params // Experiment Class
const size_t max_loop_count = 2000; // 200 //2000 //8000 class Experiment {
/// The City10000 dataset
City10000Dataset dataset_;
const bool is_with_ambiguity = false; // run original iSAM2 without ambiguities public:
// const bool is_with_ambiguity = true; // run original iSAM2 with ambiguities // Parameters with default values
size_t maxLoopCount = 2000; // 200 //2000 //8000
noiseModel::Diagonal::shared_ptr prior_noise_model = // false: run original iSAM2 without ambiguities
noiseModel::Diagonal::Sigmas( // true: run original iSAM2 with ambiguities
(Vector(3) << 0.0001, 0.0001, 0.0001).finished()); const bool is_with_ambiguity = false;
noiseModel::Diagonal::shared_ptr pose_noise_model =
noiseModel::Diagonal::Sigmas(
(Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
/**
* @brief Write the results of optimization to filename.
*
* @param results The Values object with the final results.
* @param num_poses The number of poses to write to the file.
* @param filename The file name to save the results to.
*/
void write_results(const Values& results, size_t num_poses,
const std::string& filename = "ISAM2_city10000.txt") {
ofstream outfile;
outfile.open(filename);
for (size_t i = 0; i < num_poses; ++i) {
Pose2 out_pose = results.at<Pose2>(X(i));
outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta()
<< std::endl;
}
outfile.close();
std::cout << "output written to " << filename << std::endl;
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
ifstream in(findExampleDataFile("T1_city10000_04.txt"));
// ifstream in("../data/mh_T1_city10000_04.txt"); //Type #1 only
// ifstream in("../data/mh_T3b_city10000_10.txt"); //Type #3 only
// ifstream in("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3
// ifstream in("../data/mh_All_city10000_groundtruth.txt");
size_t pose_count = 0;
size_t index = 0;
std::list<double> time_list;
private:
ISAM2Params parameters; ISAM2Params parameters;
parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
parameters.relinearizeThreshold = 0.01; parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
ISAM2* isam2 = new ISAM2(parameters); ISAM2 isam2(parameters);
NonlinearFactorGraph graph;
NonlinearFactorGraph* graph = new NonlinearFactorGraph(); Values initial_;
Values init_values;
Values results; Values results;
double x = 0.0; public:
double y = 0.0; /// Construct with filename of experiment to run
double rad = 0.0; explicit Experiment(const std::string& filename) : dataset_(filename) {}
Pose2 prior_pose(x, y, rad); /// @brief Run the main experiment with a given maxLoopCount.
void run() {
// Initialize local variables
size_t pose_count = 0, index = 0;
init_values.insert(X(0), prior_pose); std::list<double> timeList;
// Set up initial prior
Pose2 priorPose(0, 0, 0);
initial_.insert(X(0), priorPose);
graph.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
pose_count++; pose_count++;
graph->addPrior<Pose2>(X(0), prior_pose, prior_noise_model); // Initial update
isam2.update(*graph, initial_);
isam2->update(*graph, init_values); graph.resize(0);
graph->resize(0); initial_.clear();
init_values.clear(); results = isam2.calculateBestEstimate();
results = isam2->calculateBestEstimate();
//*
size_t key_s = 0;
size_t key_t = 0;
// Start main loop
size_t keyS = 0;
size_t keyT = 0;
clock_t start_time = clock(); clock_t start_time = clock();
string str; string str;
while (getline(in, str) && index < max_loop_count) { while (getline(in, str) && index < max_loop_count) {
// cout << str << endl;
vector<string> parts; vector<string> parts;
split(parts, str, is_any_of(" ")); split(parts, str, is_any_of(" "));
key_s = stoi(parts[1]); keyS = stoi(parts[1]);
key_t = stoi(parts[3]); keyT = stoi(parts[3]);
int num_measurements = stoi(parts[5]); int num_measurements = stoi(parts[5]);
vector<Pose2> pose_array(num_measurements); vector<Pose2> pose_array(num_measurements);
@ -148,48 +116,48 @@ int main(int argc, char* argv[]) {
odom_pose = pose_array[0]; odom_pose = pose_array[0];
} }
if (key_s == key_t - 1) { // new X(key) if (keyS == keyT - 1) { // new X(key)
init_values.insert(X(key_t), results.at<Pose2>(X(key_s)) * odom_pose); initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odom_pose);
graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, graph->add(
pose_noise_model)); BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel));
pose_count++; pose_count++;
} else { // loop } else { // loop
int id = index % num_measurements; int id = index % num_measurements;
if (is_with_ambiguity && id % 2 == 0) { if (is_with_ambiguity && id % 2 == 0) {
graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, graph->add(BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose,
pose_noise_model)); kPoseNoiseModel));
} else { } else {
graph->add(BetweenFactor<Pose2>( graph->add(BetweenFactor<Pose2>(
X(key_s), X(key_t), odom_pose, X(keyS), X(keyT), odom_pose,
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0))); noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
} }
index++; index++;
} }
isam2->update(*graph, init_values); isam2->update(*graph, initial_);
graph->resize(0); graph->resize(0);
init_values.clear(); initial_.clear();
results = isam2->calculateBestEstimate(); results = isam2->calculateBestEstimate();
// Print loop index and time taken in processor clock ticks // Print loop index and time taken in processor clock ticks
if (index % 50 == 0 && key_s != key_t - 1) { if (index % 50 == 0 && keyS != keyT - 1) {
std::cout << "index: " << index << std::endl; std::cout << "index: " << index << std::endl;
std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC std::cout << "acc_time: " << timeList.back() / CLOCKS_PER_SEC
<< std::endl; << std::endl;
} }
if (key_s == key_t - 1) { if (keyS == keyT - 1) {
clock_t cur_time = clock(); clock_t cur_time = clock();
time_list.push_back(cur_time - start_time); timeList.push_back(cur_time - start_time);
} }
if (time_list.size() % 100 == 0 && (key_s == key_t - 1)) { if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) {
string step_file_idx = std::to_string(100000 + time_list.size()); string step_file_idx = std::to_string(100000 + timeList.size());
ofstream step_outfile; ofstream step_outfile;
string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx; string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx;
step_outfile.open(step_file_name + ".txt"); step_outfile.open(step_file_name + ".txt");
for (size_t i = 0; i < (key_t + 1); ++i) { for (size_t i = 0; i < (keyT + 1); ++i) {
Pose2 out_pose = results.at<Pose2>(X(i)); Pose2 out_pose = results.at<Pose2>(X(i));
step_outfile << out_pose.x() << " " << out_pose.y() << " " step_outfile << out_pose.x() << " " << out_pose.y() << " "
<< out_pose.theta() << endl; << out_pose.theta() << endl;
@ -203,16 +171,30 @@ int main(int argc, char* argv[]) {
cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl;
/// Write results to file /// Write results to file
write_results(results, (key_t + 1)); writeResult(results, (keyT + 1), "ISAM2_city10000.txt");
ofstream outfile_time; ofstream outfile_time;
std::string time_file_name = "ISAM2_city10000_time.txt"; std::string time_file_name = "ISAM2_city10000_time.txt";
outfile_time.open(time_file_name); outfile_time.open(time_file_name);
for (auto acc_time : time_list) { for (auto acc_time : timeList) {
outfile_time << acc_time << std::endl; outfile_time << acc_time << std::endl;
} }
outfile_time.close(); outfile_time.close();
cout << "Written cumulative time to: " << time_file_name << " file." << endl; cout << "Written cumulative time to: " << time_file_name << " file."
<< endl;
}
};
/* ************************************************************************* */
int main(int argc, char* argv[]) {
Experiment experiment(findExampleDataFile("T1_city10000_04.txt"));
// 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_T1_T3_city10000_04.txt"); //Type #1 +
// Type #3
// Run the experiment
experiment.run();
return 0; return 0;
} }