46 #include <gtsam/config.h> 48 #include <boost/archive/binary_iarchive.hpp> 49 #include <boost/archive/binary_oarchive.hpp> 50 #include <boost/program_options.hpp> 51 #include <boost/range/algorithm/set_algorithm.hpp> 58 #include <tbb/task_arena.h> 59 #include <tbb/task_group.h> 63 using namespace gtsam;
65 namespace po = boost::program_options;
80 for(
const std::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
81 graph_dim += (
int)nlf->dim();
83 double dof = double(graph_dim) - double(config.
dim());
84 return 2. * graph.error(config) / dof;
115 int main(
int argc,
char *argv[]) {
117 po::options_description desc(
"Available options");
119 (
"help",
"Print help message")
120 (
"write-solution,o", po::value<string>(&
outputFile)->default_value(
""),
"Write graph and solution to the specified file")
121 (
"read-solution,i", po::value<string>(&
inputFile)->default_value(
""),
"Read graph and solution from the specified file")
122 (
"dataset,d", po::value<string>(&
datasetName)->default_value(
""),
"Read a dataset file (if and only if --incremental is used)")
123 (
"first-step,f", po::value<int>(&
firstStep)->default_value(0),
"First step to process from the dataset file")
124 (
"last-step,l", po::value<int>(&
lastStep)->default_value(-1),
"Last step to process, or -1 to process until the end of the dataset")
125 (
"threads", po::value<int>(&
nThreads)->default_value(-1),
"Number of threads, or -1 to use all processors")
126 (
"relinSkip", po::value<int>(&
relinSkip)->default_value(10),
"Fluid relinearization check every arg steps")
127 (
"incremental",
"Run in incremental mode using ISAM2 (default)")
128 (
"dogleg",
"When in incremental mode, solve with Dogleg instead of Gauss-Newton in iSAM2")
129 (
"batch",
"Run in batch mode, requires an initialization from --read-solution")
130 (
"compare", po::value<vector<string> >()->multitoken(),
"Compare two solution files")
131 (
"perturb", po::value<double>(&
perturbationNoise),
"Perturb a solution file with the specified noise")
132 (
"stats",
"Gather factorization statistics about the dataset, writes text-file histograms")
134 po::variables_map vm;
135 po::store(po::command_line_parser(argc, argv).
options(desc).
run(), vm);
138 batch = (vm.count(
"batch") > 0);
139 compare = (vm.count(
"compare") > 0);
140 perturb = (vm.count(
"perturb") > 0);
141 stats = (vm.count(
"stats") > 0);
143 incremental = (vm.count(
"incremental") > 0 || modesSpecified == 0);
144 dogleg = (vm.count(
"dogleg") > 0);
146 const vector<string>& compareFiles = vm[
"compare"].as<vector<string> >();
147 if(compareFiles.size() != 2) {
148 cout <<
"Must specify two files with --compare";
155 if(modesSpecified > 1) {
156 cout <<
"Only one of --incremental, --batch, --compare, --perturb, and --stats may be specified\n" << desc << endl;
160 cout <<
"In incremental and batch modes, a dataset must be specified\n" << desc << endl;
164 cout <<
"A dataset may only be specified in incremental or batch modes\n" << desc << endl;
168 cout <<
"In batch model, an input file must be specified\n" << desc << endl;
172 cout <<
"In perturb mode, specify input and output files\n" << desc << endl;
176 cout <<
"In stats mode, specify dataset and input file\n" << desc << endl;
183 cout <<
"Reading input file " <<
inputFile << endl;
184 std::ifstream readerStream(
inputFile.c_str(), ios::binary);
185 boost::archive::binary_iarchive
reader(readerStream);
195 std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>
data =
197 datasetMeasurements = *data.first;
198 }
catch(std::exception&
e) {
199 cout << e.what() << endl;
205 tbb::task_arena arena;
208 cout <<
"Using " <<
nThreads <<
" threads" << endl;
211 cout <<
"Using threads for all processors" << endl;
214 std::cout <<
"GTSAM is not compiled with TBB, so threading is disabled and the --threads option cannot be used." << endl;
253 cout <<
"Looking for first measurement from step " <<
firstStep << endl;
254 size_t nextMeasurement = 0;
255 bool havePreviousPose =
false;
257 while(nextMeasurement < datasetMeasurements.
size())
270 havePreviousPose =
true;
278 if(nextMeasurement == datasetMeasurements.
size()) {
279 cout <<
"The supplied first step is past the end of the dataset" << endl;
284 if(!havePreviousPose) {
285 cout <<
"Looks like " << firstPose <<
" is the first time step, so adding a prior on it" << endl;
289 newFactors.
addPrior(firstPose,
Pose(), noiseModel::Unit::Create(3));
292 isam2.
update(newFactors, newVariables);
295 cout <<
"Playing forward time steps..." << endl;
297 for (
size_t step = firstPose;
305 gttic_(Collect_measurements);
306 while(nextMeasurement < datasetMeasurements.
size()) {
308 NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
314 if(factor->key<1>() >
step || factor->key<2>() >
step)
318 if(factor->key<1>() !=
step && factor->key<2>() !=
step)
319 throw runtime_error(
"Problem in data file, out-of-sequence measurements");
323 const auto&
measured = factor->measured();
326 if(factor->key<1>() > factor->key<2>()) {
327 if(!newVariables.
exists(factor->key<1>())) {
332 newVariables.
insert(factor->key<1>(), prevPose *
measured.inverse());
336 if(!newVariables.
exists(factor->key<2>())) {
349 Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
353 throw runtime_error(
"Problem in data file, out-of-sequence measurements");
366 const auto&
measured = factor->measured();
368 double measuredRange = measured.
range();
369 newVariables.
insert(lmKey,
375 throw std::runtime_error(
"Unknown factor type read from data file");
379 gttoc_(Collect_measurements);
384 isam2.
update(newFactors, newVariables);
386 }
catch(std::exception&
e) {
387 cout << e.what() << endl;
391 if((
step - firstPose) % 1000 == 0) {
396 cout <<
"chi2 = " << chi2 << endl;
398 }
catch(std::exception& e) {
399 cout << e.what() << endl;
406 if((
step - firstPose) % 1000 == 0) {
407 cout <<
"Step " <<
step << endl;
415 cout <<
"Writing output file " <<
outputFile << endl;
416 std::ofstream writerStream(
outputFile.c_str(), ios::binary);
417 boost::archive::binary_oarchive writer(writerStream);
420 }
catch(std::exception&
e) {
421 cout << e.what() << endl;
469 cout <<
"Creating batch optimizer..." << endl;
472 measurements.
addPrior(0,
Pose(), noiseModel::Unit::Create(3));
481 lastError = optimizer.
error();
482 gttic_(Iterate_optimizer);
484 gttoc_(Iterate_optimizer);
485 cout <<
"Error: " << lastError <<
" -> " << optimizer.
error() << endl;
488 cout <<
"chi2 = " << chi2 << endl;
500 cout <<
"Writing output file " <<
outputFile << endl;
501 std::ofstream writerStream(
outputFile.c_str(), ios::binary);
502 boost::archive::binary_oarchive writer(writerStream);
503 writer << optimizer.
values();
504 }
catch(std::exception&
e) {
505 cout << e.what() << endl;
516 cout <<
"Reading solution file " <<
compareFile1 << endl;
518 std::ifstream readerStream(
compareFile1.c_str(), ios::binary);
519 boost::archive::binary_iarchive
reader(readerStream);
523 cout <<
"Reading solution file " <<
compareFile2 << endl;
525 std::ifstream readerStream(
compareFile2.c_str(), ios::binary);
526 boost::archive::binary_iarchive
reader(readerStream);
531 cout <<
"Comparing solutions..." << endl;
533 br::set_symmetric_difference(soln1.
keys(), soln2.
keys(), std::back_inserter(missingKeys));
534 if(!missingKeys.empty()) {
535 cout <<
" Keys unique to one solution file: ";
536 for(
size_t i = 0;
i < missingKeys.size(); ++
i) {
538 if(i != missingKeys.size() - 1)
544 br::set_intersection(soln1.
keys(), soln2.
keys(), std::back_inserter(commonKeys));
545 double maxDiff = 0.0;
546 for(
Key j: commonKeys)
547 maxDiff =
std::max(maxDiff, soln1.
at(
j).localCoordinates_(soln2.
at(
j)).norm());
548 cout <<
" Maximum solution difference (norm of logmap): " << maxDiff << endl;
560 for(
const auto& [
key, dim]: initial.
dims())
571 cout <<
"Writing output file " <<
outputFile << endl;
572 std::ofstream writerStream(
outputFile.c_str(), ios::binary);
573 boost::archive::binary_oarchive writer(writerStream);
575 }
catch(std::exception&
e) {
576 cout << e.what() << endl;
585 cout <<
"Gathering statistics..." << endl;
592 cout <<
"Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl;
593 file.open(
"SolverComparer_Stats_problemSizeHistogram.txt");
597 cout <<
"Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl;
598 file.open(
"SolverComparer_Stats_numberOfChildrenHistogram.txt");
602 cout <<
"Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl;
603 file.open(
"SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt");
const gtsam::Symbol key('X', 0)
def step(data, isam, result, truth, currPoseIndex)
Histogram problemSizeHistogram
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
const ValueType at(Key j) const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Struct to store gathered statistics about a forest.
iterator insert(const std::pair< Key, Vector > &key_value)
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
const Values & values() const
return values in current optimizer state
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
NonlinearFactorGraph graph
std::shared_ptr< This > shared_ptr
static const KeyFormatter DefaultKeyFormatter
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
const GaussNewtonParams & params() const
const Values & getLinearizationPoint() const
Access the current linearization point.
static const SmartProjectionParams params
Values retract(const VectorValues &delta) const
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Histogram problemSizeOfSecondLargestChildHistogram
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
NoiseModelFactorN< Pose, Pose > NM2
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
double errorTol
The maximum total error to stop iterating (default 0.0)
const NonlinearFactorGraph & getFactorsUnsafe() const
NoiseModelFactorN< Pose > NM1
std::shared_ptr< BetweenFactor > shared_ptr
BearingRangeFactor< Pose, Point2 > BR
const Symbol key1('v', 1)
Histogram numberOfChildrenHistogram
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t real_t idx_t * options
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Unit3_ normal(const OrientedPlane3_ &p)
OptimizationParams optimizationParams
std::map< Key, size_t > dims() const
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
GaussianFactorGraph::shared_ptr iterate() override
NonlinearFactorGraph datasetMeasurements
Double_ range(const Point2_ &p, const Point2_ &q)
bool enablePartialRelinearizationCheck
double chi2_red(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &config)
void insert(Key j, const Value &val)
A class for computing marginals in a NonlinearFactorGraph.
void tictoc_finishedIteration_()
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
double error() const
return error in current optimizer state
ForestStatistics GatherStatistics(const FOREST &forest)
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Values calculateEstimate() const
LinearSolverType linearSolverType
The type of linear solver to use in the nonlinear optimizer.
int main(int argc, char *argv[])
const Symbol key2('v', 2)