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

some improvements

release/4.3a0
Varun Agrawal 2025-01-23 12:53:08 -05:00
parent ea82d51d10
commit b2503ca443
1 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/ISAM2_City10000.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <time.h>
@ -51,7 +50,7 @@ noiseModel::Diagonal::shared_ptr prior_noise_model =
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.
@ -90,12 +89,10 @@ int main(int argc, char* argv[]) {
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);
NonlinearFactorGraph* graph = new NonlinearFactorGraph();
@ -112,7 +109,7 @@ int main(int argc, char* argv[]) {
init_values.insert(X(0), prior_pose);
pose_count++;
graph->add(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model));
graph->addPrior<Pose2>(X(0), prior_pose, prior_noise_model);
isam2->update(*graph, init_values);
graph->resize(0);
@ -165,12 +162,11 @@ int main(int argc, char* argv[]) {
init_values.clear();
results = isam2->calculateBestEstimate();
//*
// 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();