31 using namespace gtsam;
37 Values::shared_ptr posesInFile;
39 boost::tie(inputGraph, posesInFile) =
readG2o(g2oFile, is3D);
44 auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
48 for (
size_t i : {0, 1, 2, 3})
50 const Values poses = initialize::computePoses<Pose2>(orientations, &poseGraph);
60 Values::shared_ptr posesInFile;
62 boost::tie(inputGraph, posesInFile) =
readG2o(g2oFile, is3D);
67 auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
71 for (
size_t i : {0, 1, 2})
73 Values poses = initialize::computePoses<Pose3>(orientations, &poseGraph);
static int runAllTests(TestResult &result)
void insert(Key j, const Value &val)
common code between lago.* (2D) and InitializePose3.* (3D)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
string findExampleDataFile(const string &name)
boost::shared_ptr< This > shared_ptr
TEST(InitializePose3, computePoses2D)
#define EXPECT(condition)
const Rot2 & rotation() const
rotation
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GenericValue< T > genericValue(const T &v)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
utility functions for loading datasets
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation