Go to the documentation of this file.
26 using namespace gtsam;
28 int main(
const int argc,
const char* argv[]) {
42 auto priorModel = noiseModel::Diagonal::Variances(
43 (
Vector(6) << 1
e-6, 1
e-6, 1
e-6, 1
e-4, 1
e-4, 1
e-4).finished());
46 std::cout <<
"Adding prior to g2o file " << std::endl;
52 std::cout <<
"Initializing Pose3 - Riemannian gradient" << std::endl;
53 bool useGradient =
true;
56 std::cout <<
"done!" << std::endl;
59 std::cout <<
"initialization error=" <<
graph->
error(initialization)
63 initialization.
print(
"initialization");
66 std::cout <<
"Writing results to file: " <<
outputFile << std::endl;
71 std::cout <<
"done! " << std::endl;
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...
Initialize Pose3 in a factor graph.
double error(const Values &values) const
utility functions for loading datasets
int main(const int argc, const char *argv[])
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
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
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