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

Make class

release/4.3a0
Frank Dellaert 2025-01-28 21:31:32 -05:00
parent 90d3f2e84a
commit 18be91b917
1 changed files with 213 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
examples/Hybrid_City10000.cpp
View File

@ -46,16 +46,15 @@ using symbol_shorthand::X;
// Testing params // Testing params
const size_t max_loop_count = 3000; // 2000; // 200 //2000 //8000 const size_t max_loop_count = 3000; // 2000; // 200 //2000 //8000
noiseModel::Diagonal::shared_ptr prior_noise_model = auto prior_noise_model = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::Sigmas(
(Vector(3) << 0.0001, 0.0001, 0.0001).finished()); (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
noiseModel::Diagonal::shared_ptr pose_noise_model = auto pose_noise_model = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::Sigmas(
(Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
class Experiment {
/** /**
* @brief Write the result of optimization to filename. * @brief Write the result of optimization to file.
* *
* @param result The Values object with the final result. * @param result The Values object with the final result.
* @param num_poses The number of poses to write to the file. * @param num_poses The number of poses to write to the file.
@ -79,15 +78,9 @@ void write_result(const Values& result, size_t num_poses,
/** /**
* @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.
*
* @param loop_counter
* @param key_s
* @param key_t
* @param measurement
* @return HybridNonlinearFactor
*/ */
HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s, HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter,
size_t key_t, size_t key_s, size_t key_t,
const Pose2& measurement) { const Pose2& measurement) {
DiscreteKey l(L(loop_counter), 2); DiscreteKey l(L(loop_counter), 2);
@ -101,6 +94,7 @@ HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s,
return mixtureFactor; return mixtureFactor;
} }
// Create hybrid odometry factor with discrete measurement choices
HybridNonlinearFactor HybridOdometryFactor( HybridNonlinearFactor HybridOdometryFactor(
size_t num_measurements, size_t key_s, size_t key_t, const DiscreteKey& m, size_t num_measurements, size_t key_s, size_t key_t, const DiscreteKey& m,
const std::vector<Pose2>& pose_array, const std::vector<Pose2>& pose_array,
@ -115,9 +109,10 @@ HybridNonlinearFactor HybridOdometryFactor(
return mixtureFactor; return mixtureFactor;
} }
void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, // Perform smoother update and optimize the graph
const Values& initial, size_t maxNrHypotheses, void SmootherUpdate(HybridSmoother& smoother,
Values* result) { HybridNonlinearFactorGraph& graph, const Values& initial,
size_t maxNrHypotheses, Values* result) {
HybridGaussianFactorGraph linearized = *graph.linearize(initial); HybridGaussianFactorGraph linearized = *graph.linearize(initial);
// std::cout << "index: " << index << std::endl; // std::cout << "index: " << index << std::endl;
smoother.update(linearized, maxNrHypotheses); smoother.update(linearized, maxNrHypotheses);
@ -126,15 +121,10 @@ void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph,
result->insert_or_assign(initial.retract(delta.continuous())); result->insert_or_assign(initial.retract(delta.continuous()));
} }
/* ************************************************************************* */ public:
int main(int argc, char* argv[]) { /// Construct with filename of experiment to run
ifstream in(findExampleDataFile("T1_city10000_04.txt")); Experiment(const std::string& filename) {
// ifstream in("../data/mh_T1_city10000_04.txt"); //Type #1 only ifstream in(filename);
// 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 discrete_count = 0, index = 0; size_t discrete_count = 0, index = 0;
size_t loop_count = 0; size_t loop_count = 0;
size_t nrHybridFactors = 0; size_t nrHybridFactors = 0;
@ -272,7 +262,8 @@ int main(int argc, char* argv[]) {
// 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++) {
// auto p = smoother_update_times.at(i); // auto p = smoother_update_times.at(i);
// std::cout << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl; // std::cout << p.first << ", " << p.second / CLOCKS_PER_SEC <<
// std::endl;
// } // }
ofstream outfile_time; ofstream outfile_time;
std::string time_file_name = "Hybrid_City10000_time.txt"; std::string time_file_name = "Hybrid_City10000_time.txt";
@ -283,6 +274,16 @@ int main(int argc, char* argv[]) {
outfile_time.close(); outfile_time.close();
cout << "output " << time_file_name << " file." << endl; cout << "output " << time_file_name << " file." << endl;
std::cout << nrHybridFactors << std::endl; std::cout << nrHybridFactors << std::endl;
}
};
/* ************************************************************************* */
int main(int argc, char* argv[]) {
Experiment(findExampleDataFile("T1_city10000_04.txt"));
// Experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only
// Experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only
// Experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3
// Experiment("../data/mh_All_city10000_groundtruth.txt");
return 0; return 0;
} }