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

278
examples/ISAM2_City10000.cpp
View File

@ -38,181 +38,163 @@ 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;
pose_count++;
graph->addPrior<Pose2>(X(0), prior_pose, prior_noise_model); // Set up initial prior
Pose2 priorPose(0, 0, 0);
initial_.insert(X(0), priorPose);
graph.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
pose_count++;
isam2->update(*graph, init_values); // Initial update
graph->resize(0); isam2.update(*graph, initial_);
init_values.clear(); graph.resize(0);
results = isam2->calculateBestEstimate(); initial_.clear();
results = isam2.calculateBestEstimate();
//* // Start main loop
size_t key_s = 0; size_t keyS = 0;
size_t key_t = 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) { vector<string> parts;
// cout << str << endl; split(parts, str, is_any_of(" "));
vector<string> parts;
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);
for (int i = 0; i < num_measurements; ++i) { for (int i = 0; i < num_measurements; ++i) {
x = stod(parts[6 + 3 * i]); x = stod(parts[6 + 3 * i]);
y = stod(parts[7 + 3 * i]); y = stod(parts[7 + 3 * i]);
rad = stod(parts[8 + 3 * i]); rad = stod(parts[8 + 3 * i]);
pose_array[i] = Pose2(x, y, rad); pose_array[i] = Pose2(x, y, rad);
} }
Pose2 odom_pose; Pose2 odom_pose;
if (is_with_ambiguity) { if (is_with_ambiguity) {
// Get wrong intentionally // Get wrong intentionally
int id = index % num_measurements; int id = index % num_measurements;
odom_pose = Pose2(pose_array[id]); odom_pose = Pose2(pose_array[id]);
} else {
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);
graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose,
pose_noise_model));
pose_count++;
} else { // loop
int id = index % num_measurements;
if (is_with_ambiguity && id % 2 == 0) {
graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose,
pose_noise_model));
} else { } else {
graph->add(BetweenFactor<Pose2>( odom_pose = pose_array[0];
X(key_s), X(key_t), odom_pose,
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
} }
index++;
}
isam2->update(*graph, init_values); if (keyS == keyT - 1) { // new X(key)
graph->resize(0); initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odom_pose);
init_values.clear(); graph->add(
results = isam2->calculateBestEstimate(); BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel));
pose_count++;
// Print loop index and time taken in processor clock ticks } else { // loop
if (index % 50 == 0 && key_s != key_t - 1) { int id = index % num_measurements;
std::cout << "index: " << index << std::endl; if (is_with_ambiguity && id % 2 == 0) {
std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC graph->add(BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose,
<< std::endl; kPoseNoiseModel));
} } else {
graph->add(BetweenFactor<Pose2>(
if (key_s == key_t - 1) { X(keyS), X(keyT), odom_pose,
clock_t cur_time = clock(); noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
time_list.push_back(cur_time - start_time); }
} index++;
}
if (time_list.size() % 100 == 0 && (key_s == key_t - 1)) {
string step_file_idx = std::to_string(100000 + time_list.size()); isam2->update(*graph, initial_);
graph->resize(0);
ofstream step_outfile; initial_.clear();
string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx; results = isam2->calculateBestEstimate();
step_outfile.open(step_file_name + ".txt");
for (size_t i = 0; i < (key_t + 1); ++i) { // Print loop index and time taken in processor clock ticks
Pose2 out_pose = results.at<Pose2>(X(i)); if (index % 50 == 0 && keyS != keyT - 1) {
step_outfile << out_pose.x() << " " << out_pose.y() << " " std::cout << "index: " << index << std::endl;
<< out_pose.theta() << endl; std::cout << "acc_time: " << timeList.back() / CLOCKS_PER_SEC
<< std::endl;
}
if (keyS == keyT - 1) {
clock_t cur_time = clock();
timeList.push_back(cur_time - start_time);
}
if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) {
string step_file_idx = std::to_string(100000 + timeList.size());
ofstream step_outfile;
string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx;
step_outfile.open(step_file_name + ".txt");
for (size_t i = 0; i < (keyT + 1); ++i) {
Pose2 out_pose = results.at<Pose2>(X(i));
step_outfile << out_pose.x() << " " << out_pose.y() << " "
<< out_pose.theta() << endl;
}
step_outfile.close();
} }
step_outfile.close();
} }
clock_t end_time = clock();
clock_t total_time = end_time - start_time;
cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl;
/// Write results to file
writeResult(results, (keyT + 1), "ISAM2_city10000.txt");
ofstream outfile_time;
std::string time_file_name = "ISAM2_city10000_time.txt";
outfile_time.open(time_file_name);
for (auto acc_time : timeList) {
outfile_time << acc_time << std::endl;
}
outfile_time.close();
cout << "Written cumulative time to: " << time_file_name << " file."
<< endl;
} }
};
clock_t end_time = clock(); /* ************************************************************************* */
clock_t total_time = end_time - start_time; int main(int argc, char* argv[]) {
cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; 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
/// Write results to file // Run the experiment
write_results(results, (key_t + 1)); experiment.run();
ofstream outfile_time;
std::string time_file_name = "ISAM2_city10000_time.txt";
outfile_time.open(time_file_name);
for (auto acc_time : time_list) {
outfile_time << acc_time << std::endl;
}
outfile_time.close();
cout << "Written cumulative time to: " << time_file_name << " file." << endl;
return 0; return 0;
} }