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 
align_3::a1
Point2 a1
Definition: testPose2.cpp:769
test_docs.d
d
Definition: test_docs.py:28
CHECK
#define CHECK(condition)
Definition: Test.h:108
Q
Quaternion Q
Definition: testQuaternion.cpp:27
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
align_3::a3
Point2 a3
Definition: testPose2.cpp:771
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
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:12
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
simple_graph::b2
Vector2 b2(4, -5)
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
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
gtsam::normalize
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
sampling::mean
static const Vector2 mean(20, 40)
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:49
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
ones
MatrixXcf ones
Definition: ComplexEigenSolver_eigenvalues.cpp:1
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
TEST
TEST(Point3, Constructor)
Definition: testPoint3.cpp:31
Eigen::internal::omega
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam
traits
Definition: chartTesting.h:28
align_3::a2
Point2 a2
Definition: testPose2.cpp:770
simple_graph::b3
Vector2 b3(3, -6)
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
arithmetic
Annotation to mark enums as an arithmetic type.
Definition: attr.h:116
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
Point3.h
3D Point
p
float * p
Definition: Tutorial_Map_using.cpp:9
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
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
testFunc
double testFunc(const Point3 &P, const Point3 &Q)
Definition: testPoint3.cpp:203
P
static Point3 P(0.2, 0.7, -2)
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:41
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
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
norm_proxy
double norm_proxy(const Point3 &point)
Definition: testPoint3.cpp:189
main
int main()
Definition: testPoint3.cpp:220
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
align_3::t
Point2 t(10, 10)
gtsam::Point3Pair
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42
simple_graph::b1
Vector2 b1(2, -1)

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