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
const size_t max_loop_count = 3000; // 2000; // 200 //2000 //8000
noiseModel::Diagonal::shared_ptr prior_noise_model =
noiseModel::Diagonal::Sigmas(
auto 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(
auto pose_noise_model = noiseModel::Diagonal::Sigmas(
(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 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
* 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,
size_t key_t,
HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter,
size_t key_s, size_t key_t,
const Pose2& measurement) {
DiscreteKey l(L(loop_counter), 2);
@ -101,6 +94,7 @@ HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s,
return mixtureFactor;
}
// Create hybrid odometry factor with discrete measurement choices
HybridNonlinearFactor HybridOdometryFactor(
size_t num_measurements, size_t key_s, size_t key_t, const DiscreteKey& m,
const std::vector<Pose2>& pose_array,
@ -115,9 +109,10 @@ HybridNonlinearFactor HybridOdometryFactor(
return mixtureFactor;
}
void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph,
const Values& initial, size_t maxNrHypotheses,
Values* result) {
// Perform smoother update and optimize the graph
void SmootherUpdate(HybridSmoother& smoother,
HybridNonlinearFactorGraph& graph, const Values& initial,
size_t maxNrHypotheses, Values* result) {
HybridGaussianFactorGraph linearized = *graph.linearize(initial);
// std::cout << "index: " << index << std::endl;
smoother.update(linearized, maxNrHypotheses);
@ -126,15 +121,10 @@ void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph,
result->insert_or_assign(initial.retract(delta.continuous()));
}
/* ************************************************************************* */
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");
public:
/// Construct with filename of experiment to run
Experiment(const std::string& filename) {
ifstream in(filename);
size_t discrete_count = 0, index = 0;
size_t loop_count = 0;
size_t nrHybridFactors = 0;
@ -272,7 +262,8 @@ int main(int argc, char* argv[]) {
// TODO Write to file
// for (size_t i = 0; i < smoother_update_times.size(); 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;
std::string time_file_name = "Hybrid_City10000_time.txt";
@ -283,6 +274,16 @@ int main(int argc, char* argv[]) {
outfile_time.close();
cout << "output " << time_file_name << " file." << 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;
}