Go to the documentation of this file.
18 using namespace std::placeholders;
19 using namespace gtsam;
31 static const Unit3 p1(1, 0, 0),
p2(0, 1, 0),
p3(0, 0, 1);
57 Model model = noiseModel::Isotropic::Sigma(3, 0.01);
62 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
72 expected = numericalDerivative11<Vector3, Rot3>(
73 [&
f](
const Rot3& r) {
return f.evaluateError(r); },
iRc);
74 f.evaluateError(
iRc, actual);
78 expected = numericalDerivative11<Vector3, Rot3>(
79 [&
f](
const Rot3& r) {
return f.evaluateError(r); },
R);
80 f.evaluateError(
R, actual);
89 Model model = noiseModel::Isotropic::Sigma(3, 0.01);
104 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
126 Model model = noiseModel::Isotropic::Sigma(2, 0.01);
132 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
143 expected = numericalDerivative11<Vector,Rot3>(
144 [&
f](
const Rot3& r) {
return f.evaluateError(r);},
iRc);
145 f.evaluateError(
iRc, actual);
149 expected = numericalDerivative11<Vector,Rot3>(
150 [&
f](
const Rot3& r) {
return f.evaluateError(r);},
R);
151 f.evaluateError(
R, actual);
160 Model model = noiseModel::Isotropic::Sigma(2, 0.01);
175 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
201 std::vector<double> angles = {0, 45, 85, 135, 180, 225, 275, 315};
202 for (
double yaw : angles) {
204 for (
double pitch : angles) {
206 for (
double roll : angles) {
210 const Rot3 nRb = nRy * yRp * pRb;
215 const Unit3 b_z(b_acc);
218 const Rot3 actual_nRb = RotateDirectionsFactor::Initialize(n_p, b_z);
static int runAllTests(TestResult &result)
virtual const Values & optimize()
static const Rot3 iRc(cameraX, cameraY, cameraZ)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
noiseModel::Isotropic::shared_ptr Model
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
TEST(RotateFactor, checkMath)
static const Unit3 p2(0, 1, 0)
static const Point3 cameraX(0, 1, 0)
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
static const Unit3 p3(0, 0, 1)
double error(const Values &values) const
const ValueType at(Key j) const
static const double kDegree
Some functions to compute numerical derivatives.
static const Point3 cameraY(0, 0, 1)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static ConjugateGradientParameters parameters
noiseModel::Diagonal::shared_ptr model
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Rot3 inverse() const
inverse of a rotation
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
static const Unit3 p1(1, 0, 0)
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
std::shared_ptr< Isotropic > shared_ptr
Represents a 3D point on a unit sphere.
NonlinearFactorGraph graph
Rot2 R(Rot2::fromAngle(0.1))
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
static const Point3 cameraZ(1, 0, 0)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:19