112 lines
3.3 KiB
C++
112 lines
3.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file timeFrobeniusFactor.cpp
|
|
* @brief time FrobeniusFactor with BAL file
|
|
* @author Frank Dellaert
|
|
* @date June 6, 2015
|
|
*/
|
|
|
|
#include <gtsam/base/timing.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/geometry/SO4.h>
|
|
#include <gtsam/linear/NoiseModel.h>
|
|
#include <gtsam/linear/PCGSolver.h>
|
|
#include <gtsam/linear/SubgraphPreconditioner.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/dataset.h>
|
|
#include <gtsam/slam/FrobeniusFactor.h>
|
|
|
|
#include <iostream>
|
|
#include <random>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
|
|
|
|
int main(int argc, char* argv[]) {
|
|
// primitive argument parsing:
|
|
if (argc > 3) {
|
|
throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
|
|
}
|
|
|
|
string g2oFile;
|
|
try {
|
|
if (argc > 1)
|
|
g2oFile = argv[argc - 1];
|
|
else
|
|
g2oFile =
|
|
"/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/"
|
|
"datasets/randomTorus3D.g2o";
|
|
// g2oFile = findExampleDataFile("sphere_smallnoise.graph");
|
|
} catch (const exception& e) {
|
|
cerr << e.what() << '\n';
|
|
exit(1);
|
|
}
|
|
|
|
// Read G2O file
|
|
const auto factors = parse3DFactors(g2oFile);
|
|
const auto poses = parse3DPoses(g2oFile);
|
|
|
|
// Build graph
|
|
NonlinearFactorGraph graph;
|
|
// graph.add(NonlinearEquality<SO4>(0, SO4()));
|
|
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
|
|
graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
|
|
for (const auto& factor : factors) {
|
|
const auto& keys = factor->keys();
|
|
const auto& Tij = factor->measured();
|
|
const auto& model = factor->noiseModel();
|
|
graph.emplace_shared<FrobeniusWormholeFactor>(
|
|
keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model);
|
|
}
|
|
|
|
std::mt19937 rng(42);
|
|
|
|
// Set parameters to be similar to ceres
|
|
LevenbergMarquardtParams params;
|
|
LevenbergMarquardtParams::SetCeresDefaults(¶ms);
|
|
params.setLinearSolverType("MULTIFRONTAL_QR");
|
|
// params.setVerbosityLM("SUMMARY");
|
|
// params.linearSolverType = LevenbergMarquardtParams::Iterative;
|
|
// auto pcg = boost::make_shared<PCGSolverParameters>();
|
|
// pcg->preconditioner_ =
|
|
// boost::make_shared<SubgraphPreconditionerParameters>();
|
|
// boost::make_shared<BlockJacobiPreconditionerParameters>();
|
|
// params.iterativeParams = pcg;
|
|
|
|
// Optimize
|
|
for (size_t i = 0; i < 100; i++) {
|
|
gttic_(optimize);
|
|
Values initial;
|
|
initial.insert(0, SO4());
|
|
for (size_t j = 1; j < poses.size(); j++) {
|
|
initial.insert(j, SO4::Random(rng));
|
|
}
|
|
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
|
Values result = lm.optimize();
|
|
cout << "cost = " << graph.error(result) << endl;
|
|
}
|
|
|
|
tictoc_finishedIteration_();
|
|
tictoc_print_();
|
|
|
|
return 0;
|
|
}
|