Merge branch 'trunk'
						commit
						31a9319fca
					
				|  | @ -77,6 +77,7 @@ TEST( Point2, unit) { | |||
|   EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| namespace { | ||||
|   /* ************************************************************************* */ | ||||
|   // some shared test values
 | ||||
|   Point2 x1, x2(1, 1), x3(1, 1); | ||||
|  | @ -86,6 +87,7 @@ Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); | |||
|   LieVector norm_proxy(const Point2& point) { | ||||
|     return LieVector(point.norm()); | ||||
|   } | ||||
| } | ||||
| TEST( Point2, norm ) { | ||||
|   Point2 p0(cos(5.0), sin(5.0)); | ||||
|   DOUBLES_EQUAL(1, p0.norm(), 1e-6); | ||||
|  | @ -109,9 +111,11 @@ TEST( Point2, norm ) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| namespace { | ||||
|   LieVector distance_proxy(const Point2& location, const Point2& point) { | ||||
|     return LieVector(location.distance(point)); | ||||
|   } | ||||
| } | ||||
| TEST( Point2, distance ) { | ||||
|   Matrix expectedH1, actualH1, expectedH2, actualH2; | ||||
| 
 | ||||
|  |  | |||
|  | @ -345,6 +345,7 @@ TEST(Pose2, inverse ) | |||
|   EXPECT(assert_equal(numericalH,actualDinverse)); | ||||
| } | ||||
| 
 | ||||
| namespace { | ||||
|   /* ************************************************************************* */ | ||||
|   Vector homogeneous(const Point2& p) { | ||||
|     return Vector_(3, p.x(), p.y(), 1.0); | ||||
|  | @ -359,6 +360,7 @@ Matrix matrix(const Pose2& gTl) { | |||
|       gRl(1, 0), gRl(1, 1), gt.y(), | ||||
|       0.0,       0.0,   1.0); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( Pose2, matrix ) | ||||
|  | @ -487,6 +489,7 @@ TEST(Pose2, members) | |||
|   EXPECT(pose.dim() == 3); | ||||
| } | ||||
| 
 | ||||
| namespace { | ||||
|   /* ************************************************************************* */ | ||||
|   // some shared test values
 | ||||
|   Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0); | ||||
|  | @ -496,6 +499,7 @@ Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); | |||
|   Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) { | ||||
|     return pose.bearing(pt); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| TEST( Pose2, bearing ) | ||||
| { | ||||
|  | @ -529,9 +533,11 @@ TEST( Pose2, bearing ) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| namespace { | ||||
|   Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) { | ||||
|     return pose.bearing(pt); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| TEST( Pose2, bearing_pose ) | ||||
| { | ||||
|  | @ -567,9 +573,11 @@ TEST( Pose2, bearing_pose ) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| namespace { | ||||
|   LieVector range_proxy(const Pose2& pose, const Point2& point) { | ||||
|     return LieVector(pose.range(point)); | ||||
|   } | ||||
| } | ||||
| TEST( Pose2, range ) | ||||
| { | ||||
|   Matrix expectedH1, actualH1, expectedH2, actualH2; | ||||
|  | @ -602,9 +610,11 @@ TEST( Pose2, range ) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| namespace { | ||||
|   LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { | ||||
|     return LieVector(pose.range(point)); | ||||
|   } | ||||
| } | ||||
| TEST( Pose2, range_pose ) | ||||
| { | ||||
|   Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0); | ||||
|  | @ -690,6 +700,7 @@ TEST(Pose2, align_3) { | |||
|   EXPECT(assert_equal(expected, *actual)); | ||||
| } | ||||
| 
 | ||||
| namespace { | ||||
|   /* ************************************************************************* */ | ||||
|   // Prototype code to align two triangles using a rigid transform
 | ||||
|   /* ************************************************************************* */ | ||||
|  | @ -702,6 +713,7 @@ boost::optional<Pose2> align(const vector<Point2>& ps, const vector<Point2>& qs, | |||
|       correspondences += make_pair(ps[t1.i_],qs[t2.i_]), make_pair(ps[t1.j_],qs[t2.j_]), make_pair(ps[t1.k_],qs[t2.k_]); | ||||
|       return align(correspondences); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| TEST(Pose2, align_4) { | ||||
|   using namespace align_3; | ||||
|  |  | |||
|  | @ -22,7 +22,7 @@ namespace gtsam { | |||
|  * Smart factor for range SLAM | ||||
|  * @addtogroup SLAM | ||||
|  */ | ||||
| class GTSAM_UNSTABLE_EXPORT SmartRangeFactor: public NoiseModelFactor { | ||||
| class SmartRangeFactor: public NoiseModelFactor { | ||||
| protected: | ||||
| 
 | ||||
|   struct Circle2 { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue