testLine3.cpp
Go to the documentation of this file.
2 #include <gtsam/base/Testable.h>
4 #include <gtsam/geometry/Line3.h>
9 
10 using namespace gtsam;
11 
14 
15 static const Line3 l(Rot3(), 1, 1);
16 
17 // Testing getters
18 TEST(Line3, getMethods) {
19  const double a = 5, b = 10;
20  const Rot3 R = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
21  const Line3 line(R, a, b);
22  EXPECT_DOUBLES_EQUAL(a, line.a(), 1e-8);
23  EXPECT_DOUBLES_EQUAL(b, line.b(), 1e-8);
24  EXPECT(assert_equal(R, line.R(), 1e-8));
25 }
26 
27 // Testing equals function of Line3
29  Line3 l_same = l;
30  EXPECT(l.equals(l_same));
31  Line3 l2(Rot3(), 1, 2);
32  EXPECT(!l.equals(l2));
33 }
34 
35 // testing localCoordinates along 4 dimensions
36 TEST(Line3, localCoordinates) {
37  // l1 and l differ only in a_
38  Line3 l1(Rot3(), 2, 1);
39  Vector4 v1(0, 0, -1, 0);
41 
42  // l2 and l differ only in b_
43  Line3 l2(Rot3(), 1, 2);
44  Vector4 v2(0, 0, 0, -1);
46 
47  // l3 and l differ in R_x
48  Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0));
49  Line3 l3(r3, 1, 1);
50  Vector4 v3(-M_PI / 4, 0, 0, 0);
52 
53  // l4 and l differ in R_y
54  Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
55  Line3 l4(r4, 1, 1);
56  Vector4 v4(0, -M_PI / 4, 0, 0);
58 
59  // Jacobians
60  Line3 l5(Rot3::Expmap(Vector3(M_PI / 3, -M_PI / 4, 0)), -0.8, 2);
61  Values val;
62  val.insert(1, l);
63  val.insert(2, l5);
64  Line3_ l_(1);
65  Line3_ l5_(2);
67  Vector4_ local_(l5_, &Line3::localCoordinates, l_);
68  ExpressionFactor<Vector4> f(model, l5.localCoordinates(l), local_);
69  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
70 }
71 
72 // testing retract along 4 dimensions
73 TEST(Line3, retract) {
74  // l1 and l differ only in a_
75  Vector4 v1(0, 0, 0, 1);
76  Line3 l1(Rot3(), 1, 2);
77  EXPECT(l1.equals(l.retract(v1)));
78 
79  // l2 and l differ only in b_
80  Vector4 v2(0, 0, 1, 0);
81  Line3 l2(Rot3(), 2, 1);
82  EXPECT(l2.equals(l.retract(v2)));
83 
84  // l3 and l differ in R_x
85  Vector4 v3(M_PI / 4, 0, 0, 0);
86  Rot3 r3;
87  r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0));
88  Line3 l3(r3, 1, 1);
89  EXPECT(l3.equals(l.retract(v3)));
90 
91  // l4 and l differ in R_y
92  Vector4 v4(0, M_PI / 4, 0, 0);
93  Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
94  Line3 l4(r4, 1, 1);
95  EXPECT(l4.equals(l.retract(v4)));
96 
97  // Jacobians
98  Vector4 v5(M_PI / 3, -M_PI / 4, -0.4, 1.2); // arbitrary vector
99  Values val;
100  val.insert(1, l);
101  val.insert(2, v5);
102  Line3_ l_(1);
103  Vector4_ v5_(2);
105  Line3_ retract_(l_, &Line3::retract, v5_);
106  ExpressionFactor<Line3> f(model, l.retract(v5), retract_);
107  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
108 }
109 
110 // testing manifold property - Retract(p, Local(p,q)) == q
111 TEST(Line3, retractOfLocalCoordinates) {
112  Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0));
113  Line3 l2(r2, 5, 9);
115 }
116 
117 // testing manifold property - Local(p, Retract(p, v)) == v
118 TEST(Line3, localCoordinatesOfRetract) {
119  Vector4 r2(2.3, 0.987, -3, 4);
121 }
122 
123 // transform from world to camera test
124 TEST(Line3, transformToExpressionJacobians) {
125  Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0));
126  Vector3 t(-2.0, 2.0, 3.0);
127  Pose3 p(r, t);
128 
129  Line3 l_c(r.inverse(), 3, -1);
130  Line3 l_w(Rot3(), 1, 1);
131  EXPECT(l_c.equals(transformTo(p, l_w)));
132 
133  Line3_ l_(1);
134  Pose3_ p_(2);
135  Values val;
136  val.insert(1, l_w);
137  val.insert(2, p);
138 
140  ExpressionFactor<Line3> f(model, transformTo(p, l_w), transformTo(p_, l_));
141  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
142 }
143 
144 // projection in camera frame test
146  Rot3 r = Rot3::Expmap(Vector3(0, 0, 0));
147  Line3 wL(r, 1, 1);
148 
149  Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0));
150  EXPECT(expected.equals(wL.project()));
151 
152  Values val;
153  val.insert(1, wL);
154  Line3_ wL_(1);
155 
157  Unit3_ projected_(wL_, &Line3::project);
158  ExpressionFactor<Unit3> f(model, expected, projected_);
159  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
160 }
161 
162 int main() {
163  TestResult tr;
164  return TestRegistry::runAllTests(tr);
165 }
#define CHECK(condition)
Definition: Test.h:108
int main()
Definition: testLine3.cpp:162
Vector v2
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector v1
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:971
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:114
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
#define M_PI
Definition: main.h:106
Rot2 R(Rot2::fromAngle(0.1))
gtsam::Key l2
Some functions to compute numerical derivatives.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
Rot3 R() const
Definition: Line3.h:123
Point3 l3(2, 2, 0)
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
double a() const
Definition: Line3.h:130
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Vector v3
static const Line3 l(Rot3(), 1, 1)
Vector4 localCoordinates(const Line3 &q, OptionalJacobian< 4, 4 > Dp={}, OptionalJacobian< 4, 4 > Dq={}) const
Definition: Line3.cpp:27
bool equals(const Line3 &l2, double tol=10e-9) const
Definition: Line3.cpp:55
Test harness methods for expressions.
#define EXPECT(condition)
Definition: Test.h:150
double b() const
Definition: Line3.h:137
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double r2
static const double r3
const G & b
Definition: Group.h:86
4 dimensional manifold of 3D lines
Point3 l4(1, 4,-4)
gtsam::Key l1
traits
Definition: chartTesting.h:28
static Unit3 FromPoint3(const Point3 &point, OptionalJacobian< 2, 3 > H={})
Named constructor from Point3 with optional Jacobian.
Definition: Unit3.cpp:44
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Definition: Manifold.h:177
Expression< Point2 > projection(f, p_cam)
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
float * p
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
Line3 retract(const Vector4 &v, OptionalJacobian< 4, 4 > Dp={}, OptionalJacobian< 4, 4 > Dv={}) const
Definition: Line3.cpp:5
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
Unit3 project(OptionalJacobian< 2, 4 > Dline={}) const
Definition: Line3.cpp:61
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Point2 t(10, 10)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:36