testPoint2.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 
18 #include <gtsam/geometry/Point2.h>
19 #include <gtsam/base/Testable.h>
20 #include <gtsam/base/numericalDerivative.h>
21 #include <gtsam/base/lieProxies.h>
22 #include <CppUnitLite/TestHarness.h>
23 
24 using namespace std;
25 using namespace gtsam;
26 
27 GTSAM_CONCEPT_TESTABLE_INST(Point2)
28 GTSAM_CONCEPT_LIE_INST(Point2)
29 
30 //******************************************************************************
31 TEST(Point2 , Constructor) {
32  Point2 p;
33 }
34 
35 //******************************************************************************
36 TEST(Double , Concept) {
37  BOOST_CONCEPT_ASSERT((IsGroup<double>));
38  BOOST_CONCEPT_ASSERT((IsManifold<double>));
39  BOOST_CONCEPT_ASSERT((IsVectorSpace<double>));
40 }
41 
42 //******************************************************************************
43 TEST(Double , Invariants) {
44  double p1(2), p2(5);
45  EXPECT(check_group_invariants(p1, p2));
46  EXPECT(check_manifold_invariants(p1, p2));
47 }
48 
49 //******************************************************************************
50 TEST(Point2 , Concept) {
51  BOOST_CONCEPT_ASSERT((IsGroup<Point2>));
52  BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
53  BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>));
54 }
55 
56 //******************************************************************************
57 TEST(Point2 , Invariants) {
58  Point2 p1(1, 2), p2(4, 5);
59  EXPECT(check_group_invariants(p1, p2));
60  EXPECT(check_manifold_invariants(p1, p2));
61 }
62 
63 /* ************************************************************************* */
64 TEST(Point2, constructor) {
65  Point2 p1(1, 2), p2 = p1;
66  EXPECT(assert_equal(p1, p2));
67 }
68 
69 /* ************************************************************************* */
70 TEST(Point2, equality) {
71  Point2 p1(1, 2), p2(1,3);
72  EXPECT(!(p1 == p2));
73 }
74 
75 /* ************************************************************************* */
76 TEST(Point2, Lie) {
77  Point2 p1(1, 2), p2(4, 5);
78  Matrix H1, H2;
79 
80  EXPECT(assert_equal(Point2(5,7), traits<Point2>::Compose(p1, p2, H1, H2)));
81  EXPECT(assert_equal(I_2x2, H1));
82  EXPECT(assert_equal(I_2x2, H2));
83 
84  EXPECT(assert_equal(Point2(3,3), traits<Point2>::Between(p1, p2, H1, H2)));
85  EXPECT(assert_equal(-I_2x2, H1));
86  EXPECT(assert_equal(I_2x2, H2));
87 
88  EXPECT(assert_equal(Point2(5,7), traits<Point2>::Retract(p1, Vector2(4., 5.))));
89  EXPECT(assert_equal(Vector2(3.,3.), traits<Point2>::Local(p1,p2)));
90 }
91 
92 /* ************************************************************************* */
93 TEST( Point2, expmap) {
94  Vector d(2);
95  d(0) = 1;
96  d(1) = -1;
97  Point2 a(4, 5), b = traits<Point2>::Retract(a,d), c(5, 4);
98  EXPECT(assert_equal(b,c));
99 }
100 
101 /* ************************************************************************* */
102 TEST( Point2, arithmetic) {
103  EXPECT(assert_equal<Point2>(Point2(-5, -6), -Point2(5, 6)));
104  EXPECT(assert_equal<Point2>(Point2(5, 6), Point2(4, 5) + Point2(1, 1)));
105  EXPECT(assert_equal<Point2>(Point2(3, 4), Point2(4, 5) - Point2(1, 1)));
106  EXPECT(assert_equal<Point2>(Point2(8, 6), Point2(4, 3) * 2));
107  EXPECT(assert_equal<Point2>(Point2(4, 6), 2.0 * Point2(2, 3)));
108  EXPECT(assert_equal<Point2>(Point2(2, 3), Point2(4, 6) / 2));
109 }
110 
111 /* ************************************************************************* */
112 TEST( Point2, unit) {
113  Point2 p0(10, 0), p1(0, -10), p2(10, 10);
114  EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6));
115  EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6));
116  EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6));
117 }
118 
119 namespace {
120  /* ************************************************************************* */
121  // some shared test values
122  Point2 x1(0,0), x2(1, 1), x3(1, 1);
123  Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
124 
125  /* ************************************************************************* */
126  double norm_proxy(const Point2& point) {
127  return point.norm();
128  }
129 }
130 TEST( Point2, norm ) {
131  Point2 p0(cos(5.0), sin(5.0));
132  DOUBLES_EQUAL(1, p0.norm(), 1e-6);
133  Point2 p1(4, 5), p2(1, 1);
134  DOUBLES_EQUAL( 5, distance2(p1, p2), 1e-6);
135  DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
136 
137  Matrix expectedH, actualH;
138  double actual;
139 
140  // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
141  actual = norm2(x1, actualH);
142  EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
143  expectedH = (Matrix(1, 2) << 1.0, 1.0).finished();
144  EXPECT(assert_equal(expectedH,actualH));
145 
146  actual = norm2(x2, actualH);
147  EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
148  expectedH = numericalDerivative11(norm_proxy, x2);
149  EXPECT(assert_equal(expectedH,actualH));
150 
151  // analytical
152  expectedH = (Matrix(1, 2) << x2.x()/actual, x2.y()/actual).finished();
153  EXPECT(assert_equal(expectedH,actualH));
154 }
155 
156 /* ************************************************************************* */
157 namespace {
158  double distance_proxy(const Point2& location, const Point2& point) {
159  return distance2(location, point);
160  }
161 }
162 TEST( Point2, distance ) {
163  Matrix expectedH1, actualH1, expectedH2, actualH2;
164 
165  // establish distance is indeed zero
166  EXPECT_DOUBLES_EQUAL(1, distance2(x1, l1), 1e-9);
167 
168  // establish distance is indeed 45 degrees
169  EXPECT_DOUBLES_EQUAL(sqrt(2.0), distance2(x1, l2), 1e-9);
170 
171  // Another pair
172  double actual23 = distance2(x2, l3, actualH1, actualH2);
173  EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
174 
175  // Check numerical derivatives
176  expectedH1 = numericalDerivative21(distance_proxy, x2, l3);
177  expectedH2 = numericalDerivative22(distance_proxy, x2, l3);
178  EXPECT(assert_equal(expectedH1,actualH1));
179  EXPECT(assert_equal(expectedH2,actualH2));
180 
181  // Another test
182  double actual34 = distance2(x3, l4, actualH1, actualH2);
183  EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
184 
185  // Check numerical derivatives
186  expectedH1 = numericalDerivative21(distance_proxy, x3, l4);
187  expectedH2 = numericalDerivative22(distance_proxy, x3, l4);
188  EXPECT(assert_equal(expectedH1,actualH1));
189  EXPECT(assert_equal(expectedH2,actualH2));
190 }
191 
192 /* ************************************************************************* */
193 TEST( Point2, circleCircleIntersection) {
194 
195  double offset = 0.994987;
196  // Test intersections of circle moving from inside to outside
197 
198  list<Point2> inside = circleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
199  EXPECT_LONGS_EQUAL(0,inside.size());
200 
201  list<Point2> touching1 = circleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
202  EXPECT_LONGS_EQUAL(1,touching1.size());
203  EXPECT(assert_equal(Point2(5,0), touching1.front()));
204 
205  list<Point2> common = circleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
206  EXPECT_LONGS_EQUAL(2,common.size());
207  EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6));
208  EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
209 
210  list<Point2> touching2 = circleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
211  EXPECT_LONGS_EQUAL(1,touching2.size());
212  EXPECT(assert_equal(Point2(5,0), touching2.front()));
213 
214  // test rotated case
215  list<Point2> rotated = circleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
216  EXPECT_LONGS_EQUAL(2,rotated.size());
217  EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
218  EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
219 
220  // test r1<r2
221  list<Point2> smaller = circleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
222  EXPECT_LONGS_EQUAL(2,smaller.size());
223  EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
224  EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
225 
226  // test offset case, r1>r2
227  list<Point2> offset1 = circleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
228  EXPECT_LONGS_EQUAL(2,offset1.size());
229  EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
230  EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
231 
232  // test offset case, r1<r2
233  list<Point2> offset2 = circleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
234  EXPECT_LONGS_EQUAL(2,offset2.size());
235  EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
236  EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
237 
238 }
239 
240 /* ************************************************************************* */
241 int main () {
242  TestResult tr;
243  return TestRegistry::runAllTests(tr);
244 }
245 /* ************************************************************************* */
246 
test_docs.d
d
Definition: test_docs.py:28
b
Scalar * b
Definition: benchVecAdd.cpp:17
Testable.h
Concept check for values that can be used in unit tests.
lieProxies.h
Provides convenient mappings of common member functions for testing.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
TEST
TEST(Point2, Constructor)
Definition: testPoint2.cpp:31
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::BOOST_CONCEPT_ASSERT
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
gtsam::distance2
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:25
offset
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 set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
Definition: gnuplot_common_settings.hh:12
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
std
Definition: Half.h:150
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
numericalDerivative.h
Some functions to compute numerical derivatives.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
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
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
a
Array33i a
Definition: Cwise_product.cpp:1
gtsam::Pose3::x
double x() const
get x
Definition: Pose3.h:269
example::point
Point3 point(10, 0,-5)
cos
EIGEN_DEVICE_FUNC const CosReturnType cos() const
Definition: ArrayCwiseUnaryOps.h:202
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
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
gtsam::norm2
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point2.cpp:27
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
gtsam::Pose3::y
double y() const
get y
Definition: Pose3.h:274
l3
static const Symbol l3('l', 3)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
main
int main()
Definition: testPoint2.cpp:241
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
l4
Point3 l4(1, 4,-4)
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:25
gtsam
traits
Definition: chartTesting.h:28
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
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
gtsam::circleCircleIntersection
boost::optional< Point2 > circleCircleIntersection(double R_d, double r_d, double tol)
Definition: Point2.cpp:55
x1
Pose3 x1
Definition: testPose3.cpp:588
p
float * p
Definition: Tutorial_Map_using.cpp:9
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:52
sin
EIGEN_DEVICE_FUNC const SinReturnType sin() const
Definition: ArrayCwiseUnaryOps.h:220
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:118
Point2.h
2D Point
norm_proxy
double norm_proxy(const Point3 &point)
Definition: testPoint3.cpp:188
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:174

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