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

Merge pull request #263 from kvmanohar22/isam2_imu_example 

adding functionality to use ISAM2 for imu preintegration example
release/4.3a0
Frank Dellaert 2020-07-15 18:03:43 -04:00 committed by GitHub
parent 87829cae40 954741093c
commit 18b705835b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
examples/ImuFactorsExample.cpp
View File

@ -35,22 +35,28 @@
* optional arguments: * optional arguments:
* data_csv_path path to the CSV file with the IMU data. * data_csv_path path to the CSV file with the IMU data.
* -c use CombinedImuFactor * -c use CombinedImuFactor
* Note: Define USE_LM to use Levenberg Marquardt Optimizer
* By default ISAM2 is used
*/ */
// GTSAM related includes. // GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <cstring> #include <cstring>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
// Uncomment the following to use Levenberg Marquardt Optimizer
// #define USE_LM
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
@ -64,6 +70,17 @@ static const char use_combined_imu_flag[3] = "-c";
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
string data_filename; string data_filename;
bool use_combined_imu = false; bool use_combined_imu = false;
#ifndef USE_LM
printf("Using ISAM2\n");
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2 isam2(parameters);
#else
printf("Using Levenberg Marquardt Optimizer\n");
#endif
if (argc < 2) { if (argc < 2) {
printf("using default CSV file\n"); printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv"); data_filename = findExampleDataFile("imuAndGPSdata.csv");
@ -252,9 +269,19 @@ int main(int argc, char* argv[]) {
initial_values.insert(V(correction_count), prop_state.v()); initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias); initial_values.insert(B(correction_count), prev_bias);
Values result;
#ifdef USE_LM
LevenbergMarquardtOptimizer optimizer(*graph, initial_values); LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
Values result = optimizer.optimize(); result = optimizer.optimize();
#else
isam2.update(*graph, initial_values);
isam2.update();
result = isam2.calculateEstimate();
// reset the graph
graph->resize(0);
initial_values.clear();
#endif
// Overwrite the beginning of the preintegration for the next step. // Overwrite the beginning of the preintegration for the next step.
prev_state = NavState(result.at<Pose3>(X(correction_count)), prev_state = NavState(result.at<Pose3>(X(correction_count)),
result.at<Vector3>(V(correction_count))); result.at<Vector3>(V(correction_count)));