Go to the documentation of this file.
34 using namespace gtsam;
71 auto gPlus = initialize::buildPoseGraph<Pose2>(
g);
94 TEST( Lago, checkSTandChords ) {
96 auto gPlus = initialize::buildPoseGraph<Pose2>(
g);
100 vector<size_t> spanningTreeIds;
101 vector<size_t> chordsIds;
111 TEST(Lago, orientationsOverSpanningTree) {
113 auto gPlus = initialize::buildPoseGraph<Pose2>(
g);
148 TEST( Lago, regularizedMeasurements ) {
150 auto gPlus = initialize::buildPoseGraph<Pose2>(
g);
154 vector<size_t> spanningTreeIds;
155 vector<size_t> chordsIds;
161 std::pair<Matrix,Vector> actualAb = lagoGraph.
jacobian();
163 Vector actual = (
Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)).finished();
165 actual = 0.1 * actual;
173 TEST( Lago, smallGraphVectorValues ) {
174 bool useOdometricPath =
false;
185 TEST( Lago, smallGraphVectorValuesSP ) {
197 TEST( Lago, multiplePosePriors ) {
198 bool useOdometricPath =
false;
211 TEST( Lago, multiplePosePriorsSP ) {
224 TEST( Lago, multiplePoseAndRotPriors ) {
225 bool useOdometricPath =
false;
238 TEST( Lago, multiplePoseAndRotPriorsSP ) {
251 TEST( Lago, smallGraphValues ) {
290 TEST( Lago, largeGraphNoisy_orientations ) {
307 Pose2 poseLago =
Pose2(0.0,0.0,orientation(0));
315 const Key& k = key_pose.first;
322 TEST( Lago, largeGraphNoisy ) {
338 const Key& k = key_pose.first;
common code between lago.* (2D) and InitializePose3.* (3D)
static int runAllTests(TestResult &result)
Class between(const Class &g) const
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1))
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.)
#define EXPECT_LONGS_EQUAL(expected, actual)
#define EXPECT(condition)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
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...
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
key2doubleMap computeThetasToRoot(const key2doubleMap &deltaThetaMap, const PredecessorMap &tree)
void getSymbolicGraph(vector< size_t > &spanningTreeIds, vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap &tree, const NonlinearFactorGraph &g)
std::map< Key, Key > PredecessorMap
Values::const_iterator const_iterator
Const iterator over vector values.
NonlinearFactorGraph graph()
const ValueType at(Key j) const
utility functions for loading datasets
#define DOUBLES_EQUAL(expected, actual, threshold)
TEST(Lago, findMinimumSpanningTree)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Key symbol(unsigned char c, std::uint64_t j)
noiseModel::Base::shared_ptr SharedNoiseModel
VectorValues initializeOrientations(const NonlinearFactorGraph &graph, bool useOdometricPath)
void g(const string &key, int i)
const gtsam::Symbol key('X', 0)
std::shared_ptr< Diagonal > shared_ptr
Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization)....
void insert(Key j, const Value &val)
GaussianFactorGraph buildLinearOrientationGraph(const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap &tree)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static constexpr Key kAnchorKey
std::map< Key, double > key2doubleMap
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
std::uint64_t Key
Integer nonlinear key type.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
gtsam
Author(s):
autogenerated on Sun Nov 10 2024 04:06:11