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  GTSAM_CONCEPT_ASSERT(IsGroup<double>);
38  GTSAM_CONCEPT_ASSERT(IsManifold<double>);
39  GTSAM_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  GTSAM_CONCEPT_ASSERT(IsGroup<Point2>);
52  GTSAM_CONCEPT_ASSERT(IsManifold<Point2>);
53  GTSAM_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 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
l3
Point3 l3(2, 2, 0)
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
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Testable.h
Concept check for values that can be used in unit tests.
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
TestHarness.h
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
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
main
int main()
Definition: testPoint2.cpp:241
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
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
gtsam::IsGroup
Definition: Group.h:42
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
arithmetic
Annotation to mark enums as an arithmetic type.
Definition: attr.h:119
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Point2.h
2D Point
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
numericalDerivative.h
Some functions to compute numerical derivatives.
x1
Pose3 x1
Definition: testPose3.cpp:663
norm_proxy
double norm_proxy(const Point3 &point)
Definition: testPoint3.cpp:189
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
l4
Point3 l4(1, 4,-4)
gtsam::Pose3::y
double y() const
get y
Definition: Pose3.h:297
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
TestResult
Definition: TestResult.h:26
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:64
gtsam::equality
bool equality(const Errors &actual, const Errors &expected, double tol)
Definition: Errors.cpp:52
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
constructor
Definition: init.h:200
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
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Pose3::x
double x() const
get x
Definition: Pose3.h:292
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
TEST
TEST(Point2, Constructor)
Definition: testPoint2.cpp:31
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
gtsam::circleCircleIntersection
std::optional< Point2 > circleCircleIntersection(double R_d, double r_d, double tol)
Definition: Point2.cpp:55
lieProxies.h
Provides convenient mappings of common member functions for testing.
gtsam::norm2
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point2.cpp:27
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
common
Definition: testImuFactor.cpp:172

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:06