Skip to content

Commit

Permalink
working example
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal committed Jan 23, 2025
1 parent 7c24448 commit ea82d51
Showing 1 changed file with 55 additions and 47 deletions.
102 changes: 55 additions & 47 deletions examples/Hybrid_City10000.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -37,21 +39,19 @@ 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(
(Vector(3) << 0.0001, 0.0001, 0.0001).finished());

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.
Expand Down Expand Up @@ -84,49 +84,42 @@ 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;

parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);

parameters.relinearizeThreshold = 0.01;

parameters.relinearizeSkip = 1;
ISAM2* isam2 = new ISAM2(parameters);
HybridNonlinearISAM isam;

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;

Pose2 prior_pose(x, y, rad);

init_values.insert(X(0), prior_pose);
pose_count++;
pose_count += 1;

graph.push_back(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model));

graph->add(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model));
isam.update(graph, init_values, maxNrHypotheses);

isam2->update(*graph, init_values);
graph->resize(0);
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(" "));

Expand All @@ -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++;
}

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));
}
graph->add(
BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, pose_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();
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();
Expand All @@ -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));
Expand All @@ -190,17 +196,19 @@ int main(int argc, char* argv[]) {
}
step_outfile.close();
}

index += 1;
}

clock_t end_time = clock();
clock_t total_time = end_time - start_time;
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;
Expand Down

0 comments on commit ea82d51

Please sign in to comment.