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 <gtsam/geometry/Point3.h>
18 #include <gtsam/base/Testable.h>
19 #include <gtsam/base/numericalDerivative.h>
20 #include <CppUnitLite/TestHarness.h>
21 
22 using namespace gtsam;
23 
24 GTSAM_CONCEPT_TESTABLE_INST(Point3)
25 GTSAM_CONCEPT_LIE_INST(Point3)
26 
27 static Point3 P(0.2, 0.7, -2);
28 
29 //******************************************************************************
30 TEST(Point3 , Constructor) {
31  Point3 p;
32 }
33 
34 //******************************************************************************
35 TEST(Point3 , Concept) {
36  BOOST_CONCEPT_ASSERT((IsGroup<Point3>));
37  BOOST_CONCEPT_ASSERT((IsManifold<Point3>));
38  BOOST_CONCEPT_ASSERT((IsVectorSpace<Point3>));
39 }
40 
41 //******************************************************************************
42 TEST(Point3 , Invariants) {
43  Point3 p1(1, 2, 3), p2(4, 5, 6);
44  EXPECT(check_group_invariants(p1, p2));
45  EXPECT(check_manifold_invariants(p1, p2));
46 }
47 
48 /* ************************************************************************* */
49 TEST(Point3, Lie) {
50  Point3 p1(1, 2, 3);
51  Point3 p2(4, 5, 6);
52  Matrix H1, H2;
53 
54  EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Compose(p1, p2, H1, H2)));
55  EXPECT(assert_equal(I_3x3, H1));
56  EXPECT(assert_equal(I_3x3, H2));
57 
58  EXPECT(assert_equal(Point3(3, 3, 3), traits<Point3>::Between(p1, p2, H1, H2)));
59  EXPECT(assert_equal(-I_3x3, H1));
60  EXPECT(assert_equal(I_3x3, H2));
61 
62  EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Retract(p1, Vector3(4,5,6))));
63  EXPECT(assert_equal(Vector3(3, 3, 3), traits<Point3>::Local(p1,p2)));
64 }
65 
66 /* ************************************************************************* */
67 TEST( Point3, arithmetic) {
68  CHECK(P * 3 == 3 * P);
69  CHECK(assert_equal<Point3>(Point3(-1, -5, -6), -Point3(1, 5, 6)));
70  CHECK(assert_equal<Point3>(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
71  CHECK(assert_equal<Point3>(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
72  CHECK(assert_equal<Point3>(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
73  CHECK(assert_equal<Point3>(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
74  CHECK(assert_equal<Point3>(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
75 }
76 
77 /* ************************************************************************* */
78 TEST( Point3, equals) {
79  CHECK(traits<Point3>::Equals(P,P));
80  Point3 Q(0,0,0);
81  CHECK(!traits<Point3>::Equals(P,Q));
82 }
83 
84 /* ************************************************************************* */
85 TEST( Point3, dot) {
86  Point3 origin(0,0,0), ones(1, 1, 1);
87  CHECK(origin.dot(Point3(1, 1, 0)) == 0);
88  CHECK(ones.dot(Point3(1, 1, 0)) == 2);
89 
90  Point3 p(1, 0.2, 0.3);
91  Point3 q = p + Point3(0.5, 0.2, -3.0);
92  Point3 r = p + Point3(0.8, 0, 0);
93  Point3 t = p + Point3(0, 0.3, -0.4);
94  EXPECT(assert_equal(1.130000, p.dot(p), 1e-8));
95  EXPECT(assert_equal(0.770000, p.dot(q), 1e-5));
96  EXPECT(assert_equal(1.930000, p.dot(r), 1e-5));
97  EXPECT(assert_equal(1.070000, p.dot(t), 1e-5));
98 
99  // Use numerical derivatives to calculate the expected Jacobians
100  Matrix H1, H2;
101  boost::function<double(const Point3&, const Point3&)> f =
102  [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
103  {
104  gtsam::dot(p, q, H1, H2);
105  EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, q), H1, 1e-9));
106  EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, q), H2, 1e-9));
107  }
108  {
109  gtsam::dot(p, r, H1, H2);
110  EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, r), H1, 1e-9));
111  EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, r), H2, 1e-9));
112  }
113  {
114  gtsam::dot(p, t, H1, H2);
115  EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, t), H1, 1e-9));
116  EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, t), H2, 1e-9));
117  }
118 }
119 
120 /* ************************************************************************* */
121 TEST(Point3, cross) {
122  Matrix aH1, aH2;
123  boost::function<Point3(const Point3&, const Point3&)> f =
124  boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
125  const Point3 omega(0, 1, 0), theta(4, 6, 8);
126  cross(omega, theta, aH1, aH2);
127  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
128  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
129 }
130 
131 /* ************************************************************************* */
132 TEST( Point3, cross2) {
133  Point3 p(1, 0.2, 0.3);
134  Point3 q = p + Point3(0.5, 0.2, -3.0);
135  Point3 r = p + Point3(0.8, 0, 0);
136  EXPECT(assert_equal(Point3(0, 0, 0), p.cross(p), 1e-8));
137  EXPECT(assert_equal(Point3(-0.66, 3.15, 0.1), p.cross(q), 1e-5));
138  EXPECT(assert_equal(Point3(0, 0.24, -0.16), p.cross(r), 1e-5));
139 
140  // Use numerical derivatives to calculate the expected Jacobians
141  Matrix H1, H2;
142  boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
143  boost::none, boost::none);
144  {
145  gtsam::cross(p, q, H1, H2);
146  EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
147  EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, q), H2, 1e-9));
148  }
149  {
150  gtsam::cross(p, r, H1, H2);
151  EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, r), H1, 1e-9));
152  EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, r), H2, 1e-9));
153  }
154 }
155 
156 //*************************************************************************
157 TEST (Point3, normalize) {
158  Matrix actualH;
159  Point3 point(1, -2, 3); // arbitrary point
160  Point3 expected(point / sqrt(14.0));
161  EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
162  Matrix expectedH = numericalDerivative11<Point3, Point3>(
163  boost::bind(gtsam::normalize, _1, boost::none), point);
164  EXPECT(assert_equal(expectedH, actualH, 1e-8));
165 }
166 
167 //*************************************************************************
168 TEST(Point3, mean) {
169  Point3 expected(2, 2, 2);
170  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
171  std::vector<Point3> a_points{a1, a2, a3};
172  Point3 actual = mean(a_points);
173  EXPECT(assert_equal(expected, actual));
174 }
175 
176 TEST(Point3, mean_pair) {
177  Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
178  Point3Pair expected = std::make_pair(a_mean, b_mean);
179  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
180  Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
181  std::vector<Point3Pair> point_pairs{{a1, b1}, {a2, b2}, {a3, b3}};
182  Point3Pair actual = means(point_pairs);
183  EXPECT(assert_equal(expected.first, actual.first));
184  EXPECT(assert_equal(expected.second, actual.second));
185 }
186 
187 //*************************************************************************
188 double norm_proxy(const Point3& point) {
189  return double(point.norm());
190 }
191 
192 TEST (Point3, norm) {
193  Matrix actualH;
194  Point3 point(3,4,5); // arbitrary point
195  double expected = sqrt(50);
196  EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8);
197  Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
198  EXPECT(assert_equal(expectedH, actualH, 1e-8));
199 }
200 
201 /* ************************************************************************* */
202 double testFunc(const Point3& P, const Point3& Q) {
203  return distance3(P, Q);
204 }
205 
206 TEST (Point3, distance) {
207  Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
208  Matrix H1, H2;
209  double d = distance3(P, Q, H1, H2);
210  double expectedDistance = 55.542686;
211  Matrix numH1 = numericalDerivative21(testFunc, P, Q);
212  Matrix numH2 = numericalDerivative22(testFunc, P, Q);
213  DOUBLES_EQUAL(expectedDistance, d, 1e-5);
214  EXPECT(assert_equal(numH1, H1, 1e-8));
215  EXPECT(assert_equal(numH2, H2, 1e-8));
216 }
217 
218 /* ************************************************************************* */
219 int main() {
220  TestResult tr;
221  return TestRegistry::runAllTests(tr);
222 }
223 /* ************************************************************************* */
224 
test_docs.d
d
Definition: test_docs.py:28
CHECK
#define CHECK(condition)
Definition: Test.h:109
Q
Quaternion Q
Definition: testQuaternion.cpp:27
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:194
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
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:974
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::BOOST_CONCEPT_ASSERT
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
simple_graph::b3
Vector2 b3(3,-6)
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:173
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
theta
Rot2 theta
Definition: testMagFactor.cpp:44
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:355
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
gtsam::mean
Point3 mean(const CONTAINER &points)
mean
Definition: Point3.h:66
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:26
gtsam::normalize
static void normalize(Signature::Row &row)
Definition: Signature.cpp:158
example::point
Point3 point(10, 0,-5)
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:199
P
static Point3 P(0.2, 0.7,-2)
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:63
ones
MatrixXcf ones
Definition: ComplexEigenSolver_eigenvalues.cpp:1
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
simple_graph::b2
Vector2 b2(4,-5)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1520
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:40
gtsam
traits
Definition: chartTesting.h:28
simple_graph::b1
Vector2 b1(2,-1)
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
arithmetic
Annotation to mark enums as an arithmetic type.
Definition: attr.h:75
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:273
Point3.h
3D Point
gtsam::means
Point3Pair means(const std::vector< Point3Pair > &abPointPairs)
Calculate the two means of a set of Point3 pairs.
Definition: Point3.cpp:78
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::TEST
TEST(LPInitSolver, InfiniteLoopSingleVar)
Definition: testLPSolver.cpp:64
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:52
testFunc
double testFunc(const Point3 &P, const Point3 &Q)
Definition: testPoint3.cpp:202
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
norm_proxy
double norm_proxy(const Point3 &point)
Definition: testPoint3.cpp:188
main
int main()
Definition: testPoint3.cpp:219
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:174
align_3::t
Point2 t(10, 10)
gtsam::Point3Pair
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:38

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:35