testPoint3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
17 #include <CppUnitLite/TestHarness.h>
18 #include <gtsam/base/Testable.h>
19 #include <gtsam/base/numericalDerivative.h>
20 #include <gtsam/geometry/Point3.h>
21 
22 using namespace std::placeholders;
23 using namespace gtsam;
24 
25 GTSAM_CONCEPT_TESTABLE_INST(Point3)
26 GTSAM_CONCEPT_LIE_INST(Point3)
27 
28 static Point3 P(0.2, 0.7, -2);
29 
30 //******************************************************************************
31 TEST(Point3 , Constructor) {
32  Point3 p;
33 }
34 
35 //******************************************************************************
36 TEST(Point3 , Concept) {
37  GTSAM_CONCEPT_ASSERT(IsGroup<Point3>);
38  GTSAM_CONCEPT_ASSERT(IsManifold<Point3>);
39  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point3>);
40 }
41 
42 //******************************************************************************
43 TEST(Point3 , Invariants) {
44  Point3 p1(1, 2, 3), p2(4, 5, 6);
45  EXPECT(check_group_invariants(p1, p2));
46  EXPECT(check_manifold_invariants(p1, p2));
47 }
48 
49 /* ************************************************************************* */
50 TEST(Point3, Lie) {
51  Point3 p1(1, 2, 3);
52  Point3 p2(4, 5, 6);
53  Matrix H1, H2;
54 
55  EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Compose(p1, p2, H1, H2)));
56  EXPECT(assert_equal(I_3x3, H1));
57  EXPECT(assert_equal(I_3x3, H2));
58 
59  EXPECT(assert_equal(Point3(3, 3, 3), traits<Point3>::Between(p1, p2, H1, H2)));
60  EXPECT(assert_equal(-I_3x3, H1));
61  EXPECT(assert_equal(I_3x3, H2));
62 
63  EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Retract(p1, Vector3(4,5,6))));
64  EXPECT(assert_equal(Vector3(3, 3, 3), traits<Point3>::Local(p1,p2)));
65 }
66 
67 /* ************************************************************************* */
68 TEST( Point3, arithmetic) {
69  CHECK(P * 3 == 3 * P);
70  CHECK(assert_equal<Point3>(Point3(-1, -5, -6), -Point3(1, 5, 6)));
71  CHECK(assert_equal<Point3>(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
72  CHECK(assert_equal<Point3>(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
73  CHECK(assert_equal<Point3>(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
74  CHECK(assert_equal<Point3>(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
75  CHECK(assert_equal<Point3>(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
76 }
77 
78 /* ************************************************************************* */
79 TEST( Point3, equals) {
80  CHECK(traits<Point3>::Equals(P,P));
81  Point3 Q(0,0,0);
82  CHECK(!traits<Point3>::Equals(P,Q));
83 }
84 
85 /* ************************************************************************* */
86 TEST( Point3, dot) {
87  Point3 origin(0,0,0), ones(1, 1, 1);
88  CHECK(origin.dot(Point3(1, 1, 0)) == 0);
89  CHECK(ones.dot(Point3(1, 1, 0)) == 2);
90 
91  Point3 p(1, 0.2, 0.3);
92  Point3 q = p + Point3(0.5, 0.2, -3.0);
93  Point3 r = p + Point3(0.8, 0, 0);
94  Point3 t = p + Point3(0, 0.3, -0.4);
95  EXPECT(assert_equal(1.130000, p.dot(p), 1e-8));
96  EXPECT(assert_equal(0.770000, p.dot(q), 1e-5));
97  EXPECT(assert_equal(1.930000, p.dot(r), 1e-5));
98  EXPECT(assert_equal(1.070000, p.dot(t), 1e-5));
99 
100  // Use numerical derivatives to calculate the expected Jacobians
101  Matrix H1, H2;
102  std::function<double(const Point3&, const Point3&)> f =
103  [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
104  {
105  gtsam::dot(p, q, H1, H2);
106  EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, q), H1, 1e-9));
107  EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, q), H2, 1e-9));
108  }
109  {
110  gtsam::dot(p, r, H1, H2);
111  EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, r), H1, 1e-9));
112  EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, r), H2, 1e-9));
113  }
114  {
115  gtsam::dot(p, t, H1, H2);
116  EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, t), H1, 1e-9));
117  EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, t), H2, 1e-9));
118  }
119 }
120 
121 /* ************************************************************************* */
122 TEST(Point3, cross) {
123  Matrix aH1, aH2;
124  std::function<Point3(const Point3&, const Point3&)> f =
125  [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); };
126  const Point3 omega(0, 1, 0), theta(4, 6, 8);
127  cross(omega, theta, aH1, aH2);
128  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
129  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
130 }
131 
132 /* ************************************************************************* */
133 TEST( Point3, cross2) {
134  Point3 p(1, 0.2, 0.3);
135  Point3 q = p + Point3(0.5, 0.2, -3.0);
136  Point3 r = p + Point3(0.8, 0, 0);
137  EXPECT(assert_equal(Point3(0, 0, 0), p.cross(p), 1e-8));
138  EXPECT(assert_equal(Point3(-0.66, 3.15, 0.1), p.cross(q), 1e-5));
139  EXPECT(assert_equal(Point3(0, 0.24, -0.16), p.cross(r), 1e-5));
140 
141  // Use numerical derivatives to calculate the expected Jacobians
142  Matrix H1, H2;
143  std::function<Point3(const Point3&, const Point3&)> f =
144  [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); };
145  {
146  gtsam::cross(p, q, H1, H2);
147  EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
148  EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, q), H2, 1e-9));
149  }
150  {
151  gtsam::cross(p, r, H1, H2);
152  EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, r), H1, 1e-9));
153  EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, r), H2, 1e-9));
154  }
155 }
156 
157 //*************************************************************************
158 TEST (Point3, normalize) {
159  Matrix actualH;
160  Point3 point(1, -2, 3); // arbitrary point
161  Point3 expected(point / sqrt(14.0));
162  EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
163  std::function<Point3(const Point3&)> fn = [](const Point3& p) { return normalize(p); };
164  Matrix expectedH = numericalDerivative11<Point3, Point3>(fn, point);
165  EXPECT(assert_equal(expectedH, actualH, 1e-8));
166 }
167 
168 //*************************************************************************
169 TEST(Point3, mean) {
170  Point3 expected(2, 2, 2);
171  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
172  std::vector<Point3> a_points{a1, a2, a3};
173  Point3 actual = mean(a_points);
174  EXPECT(assert_equal(expected, actual));
175 }
176 
177 TEST(Point3, mean_pair) {
178  Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
179  Point3Pair expected = {a_mean, b_mean};
180  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
181  Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
182  std::vector<Point3Pair> point_pairs{{a1, b1}, {a2, b2}, {a3, b3}};
183  Point3Pair actual = means(point_pairs);
184  EXPECT(assert_equal(expected.first, actual.first));
185  EXPECT(assert_equal(expected.second, actual.second));
186 }
187 
188 //*************************************************************************
189 double norm_proxy(const Point3& point) {
190  return double(point.norm());
191 }
192 
193 TEST (Point3, norm) {
194  Matrix actualH;
195  Point3 point(3,4,5); // arbitrary point
196  double expected = sqrt(50);
197  EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8);
198  Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
199  EXPECT(assert_equal(expectedH, actualH, 1e-8));
200 }
201 
202 /* ************************************************************************* */
203 double testFunc(const Point3& P, const Point3& Q) {
204  return distance3(P, Q);
205 }
206 
207 TEST (Point3, distance) {
208  Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
209  Matrix H1, H2;
210  double d = distance3(P, Q, H1, H2);
211  double expectedDistance = 55.542686;
212  Matrix numH1 = numericalDerivative21(testFunc, P, Q);
213  Matrix numH2 = numericalDerivative22(testFunc, P, Q);
214  DOUBLES_EQUAL(expectedDistance, d, 1e-5);
215  EXPECT(assert_equal(numH1, H1, 1e-8));
216  EXPECT(assert_equal(numH2, H2, 1e-8));
217 }
218 
219 /* ************************************************************************* */
220 int main() {
221  TestResult tr;
222  return TestRegistry::runAllTests(tr);
223 }
224 /* ************************************************************************* */
225 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Point3Pair
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42
simple_graph::b1
Vector2 b1(2, -1)
TEST
TEST(Point3, Constructor)
Definition: testPoint3.cpp:31
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
fn
static double fn[10]
Definition: fresnl.c:103
simple_graph::b2
Vector2 b2(4, -5)
testFunc
double testFunc(const Point3 &P, const Point3 &Q)
Definition: testPoint3.cpp:203
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::IsGroup
Definition: Group.h:42
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
Q
Quaternion Q
Definition: testQuaternion.cpp:27
arithmetic
Annotation to mark enums as an arithmetic type.
Definition: attr.h:119
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:49
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
align_3::a1
Point2 a1
Definition: testPose2.cpp:769
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
align_3::a3
Point2 a3
Definition: testPose2.cpp:771
gtsam::normalize
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
norm_proxy
double norm_proxy(const Point3 &point)
Definition: testPoint3.cpp:189
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
P
static Point3 P(0.2, 0.7, -2)
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
main
int main()
Definition: testPoint3.cpp:220
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
Eigen::internal::omega
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
simple_graph::b3
Vector2 b3(3, -6)
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:41
gtsam
traits
Definition: chartTesting.h:28
gtsam::traits
Definition: Group.h:36
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
align_3::a2
Point2 a2
Definition: testPose2.cpp:770
origin
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Definition: gnuplot_common_settings.hh:45
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
sampling::mean
static const Vector2 mean(20, 40)
align_3::t
Point2 t(10, 10)
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
ones
MatrixXcf ones
Definition: ComplexEigenSolver_eigenvalues.cpp:1

gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:06:24