Tests rotation priors on poses. More...
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PoseRotationPrior.h>
Go to the source code of this file.
Typedefs | |
typedef PoseRotationPrior< Pose2 > | Pose2RotationPrior |
typedef PoseRotationPrior< Pose3 > | Pose3RotationPrior |
Functions | |
Vector | evalFactorError2 (const Pose2RotationPrior &factor, const Pose2 &x) |
Vector | evalFactorError3 (const Pose3RotationPrior &factor, const Pose3 &x) |
int | main () |
const Point2 | point2A (1.0, 2.0) |
const Point2 | point2B (4.0, 6.0) |
const Point3 | point3A (1.0, 2.0, 3.0) |
const Point3 | point3B (4.0, 6.0, 8.0) |
TEST (testPoseRotationFactor, level2_error) | |
TEST (testPoseRotationFactor, level2_error_wrap) | |
TEST (testPoseRotationFactor, level2_zero_error) | |
TEST (testPoseRotationFactor, level3_error) | |
TEST (testPoseRotationFactor, level3_zero_error) | |
Variables | |
const SharedNoiseModel | model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1).finished()) |
const SharedNoiseModel | model3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)) |
const gtsam::Key | poseKey = 1 |
const Rot2 | rot2A |
const Rot2 | rot2B = Rot2::fromAngle(M_PI_2) |
const Rot2 | rot2C = Rot2::fromAngle(M_PI-0.01) |
const Rot2 | rot2D = Rot2::fromAngle(M_PI+0.01) |
const Rot3 | rot3A |
const Rot3 | rot3B = Rot3::Pitch(-M_PI_2) |
const Rot3 | rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)) |
const double | tol = 1e-5 |
typedef PoseRotationPrior<Pose2> Pose2RotationPrior |
Definition at line 24 of file testPoseRotationPrior.cpp.
typedef PoseRotationPrior<Pose3> Pose3RotationPrior |
Definition at line 25 of file testPoseRotationPrior.cpp.
Vector evalFactorError2 | ( | const Pose2RotationPrior & | factor, |
const Pose2 & | x | ||
) |
Definition at line 46 of file testPoseRotationPrior.cpp.
Vector evalFactorError3 | ( | const Pose3RotationPrior & | factor, |
const Pose3 & | x | ||
) |
Definition at line 41 of file testPoseRotationPrior.cpp.
int main | ( | ) |
Definition at line 107 of file testPoseRotationPrior.cpp.
const Point2 point2A | ( | 1. | 0, |
2. | 0 | ||
) |
const Point2 point2B | ( | 4. | 0, |
6. | 0 | ||
) |
const Point3 point3A | ( | 1. | 0, |
2. | 0, | ||
3. | 0 | ||
) |
const Point3 point3B | ( | 4. | 0, |
6. | 0, | ||
8. | 0 | ||
) |
TEST | ( | testPoseRotationFactor | , |
level2_error | |||
) |
Definition at line 87 of file testPoseRotationPrior.cpp.
TEST | ( | testPoseRotationFactor | , |
level2_error_wrap | |||
) |
Definition at line 97 of file testPoseRotationPrior.cpp.
TEST | ( | testPoseRotationFactor | , |
level2_zero_error | |||
) |
Definition at line 77 of file testPoseRotationPrior.cpp.
TEST | ( | testPoseRotationFactor | , |
level3_error | |||
) |
Definition at line 61 of file testPoseRotationPrior.cpp.
TEST | ( | testPoseRotationFactor | , |
level3_zero_error | |||
) |
Definition at line 51 of file testPoseRotationPrior.cpp.
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1).finished()) |
Definition at line 21 of file testPoseRotationPrior.cpp.
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)) |
Definition at line 22 of file testPoseRotationPrior.cpp.
const gtsam::Key poseKey = 1 |
Definition at line 29 of file testPoseRotationPrior.cpp.
const Rot2 rot2A |
Definition at line 37 of file testPoseRotationPrior.cpp.
Definition at line 37 of file testPoseRotationPrior.cpp.
Definition at line 38 of file testPoseRotationPrior.cpp.
Definition at line 38 of file testPoseRotationPrior.cpp.
const Rot3 rot3A |
Definition at line 33 of file testPoseRotationPrior.cpp.
Definition at line 33 of file testPoseRotationPrior.cpp.
const Rot3 rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)) |
Definition at line 33 of file testPoseRotationPrior.cpp.
const double tol = 1e-5 |
Definition at line 27 of file testPoseRotationPrior.cpp.