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, doubleCross) {
159  Matrix aH1, aH2;
160  std::function<Point3(const Point3&, const Point3&)> f =
161  [](const Point3& p, const Point3& q) { return doubleCross(p, q); };
162  const Point3 omega(1, 2, 3), theta(4, 5, 6);
163  doubleCross(omega, theta, aH1, aH2);
164  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
165  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
166 }
167 
168 //*************************************************************************
169 TEST (Point3, normalize) {
170  Matrix actualH;
171  Point3 point(1, -2, 3); // arbitrary point
172  Point3 expected(point / sqrt(14.0));
173  EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
174  std::function<Point3(const Point3&)> fn = [](const Point3& p) { return normalize(p); };
175  Matrix expectedH = numericalDerivative11<Point3, Point3>(fn, point);
176  EXPECT(assert_equal(expectedH, actualH, 1e-8));
177 }
178 
179 //*************************************************************************
180 TEST(Point3, mean) {
181  Point3 expected(2, 2, 2);
182  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
183  std::vector<Point3> a_points{a1, a2, a3};
184  Point3 actual = mean(a_points);
185  EXPECT(assert_equal(expected, actual));
186 }
187 
188 TEST(Point3, mean_pair) {
189  Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
190  Point3Pair expected = {a_mean, b_mean};
191  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
192  Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
193  std::vector<Point3Pair> point_pairs{{a1, b1}, {a2, b2}, {a3, b3}};
194  Point3Pair actual = means(point_pairs);
195  EXPECT(assert_equal(expected.first, actual.first));
196  EXPECT(assert_equal(expected.second, actual.second));
197 }
198 
199 //*************************************************************************
200 double norm_proxy(const Point3& point) {
201  return double(point.norm());
202 }
203 
204 TEST (Point3, norm) {
205  Matrix actualH;
206  Point3 point(3,4,5); // arbitrary point
207  double expected = sqrt(50);
208  EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8);
209  Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
210  EXPECT(assert_equal(expectedH, actualH, 1e-8));
211 }
212 
213 /* ************************************************************************* */
214 double testFunc(const Point3& P, const Point3& Q) {
215  return distance3(P, Q);
216 }
217 
218 TEST (Point3, distance) {
219  Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
220  Matrix H1, H2;
221  double d = distance3(P, Q, H1, H2);
222  double expectedDistance = 55.542686;
223  Matrix numH1 = numericalDerivative21(testFunc, P, Q);
224  Matrix numH2 = numericalDerivative22(testFunc, P, Q);
225  DOUBLES_EQUAL(expectedDistance, d, 1e-5);
226  EXPECT(assert_equal(numH1, H1, 1e-8));
227  EXPECT(assert_equal(numH2, H2, 1e-8));
228 }
229 
230 /* ************************************************************************* */
231 int main() {
232  TestResult tr;
233  return TestRegistry::runAllTests(tr);
234 }
235 /* ************************************************************************* */
236 
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:214
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:44
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
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
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:200
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:231
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:196
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: SFMdata.h:40
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:41
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
gtsam::doubleCross
Point3 doubleCross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
double cross product
Definition: Point3.cpp:72
ones
MatrixXcf ones
Definition: ComplexEigenSolver_eigenvalues.cpp:1

gtsam
Author(s):
autogenerated on Wed Jan 1 2025 04:06:54