Go to the documentation of this file.
27 using namespace gtsam;
29 int main(
const int argc,
const char* argv[]) {
43 auto priorModel = noiseModel::Diagonal::Variances(
44 (
Vector(6) << 1
e-6, 1
e-6, 1
e-6, 1
e-4, 1
e-4, 1
e-4).finished());
47 std::cout <<
"Adding prior to g2o file " << std::endl;
53 std::cout <<
"Optimizing the factor graph" << std::endl;
55 params.setVerbosity(
"TERMINATION");
58 std::cout <<
"Optimization complete" << std::endl;
67 std::cout <<
"Writing results to file: " <<
outputFile << std::endl;
72 std::cout <<
"done! " << std::endl;
int main(const int argc, const char *argv[])
virtual const Values & optimize()
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
static const SmartProjectionParams params
Matrix marginalCovariance(Key variable) const
double error(const Values &values) const
utility functions for loading datasets
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
const gtsam::Symbol key('X', 0)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
A class for computing marginals in a NonlinearFactorGraph.
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
NonlinearFactorGraph graph
Marginals marginals(graph, result)
std::uint64_t Key
Integer nonlinear key type.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::shared_ptr< This > shared_ptr
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24