12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'trunk'

release/4.3a0
Richard Roberts 2013-06-24 19:30:29 +00:00
parent bad9373aec 26df712592
commit 31a9319fca
3 changed files with 58 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/tests/testPoint2.cpp
View File

@ -77,14 +77,16 @@ TEST( Point2, unit) {
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); 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); // some shared test values
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Point2 x1, x2(1, 1), x3(1, 1);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector norm_proxy(const Point2& point) { LieVector norm_proxy(const Point2& point) {
return LieVector(point.norm()); return LieVector(point.norm());
}
} }
TEST( Point2, norm ) { TEST( Point2, norm ) {
Point2 p0(cos(5.0), sin(5.0)); Point2 p0(cos(5.0), sin(5.0));
@ -109,8 +111,10 @@ TEST( Point2, norm ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector distance_proxy(const Point2& location, const Point2& point) { namespace {
return LieVector(location.distance(point)); LieVector distance_proxy(const Point2& location, const Point2& point) {
return LieVector(location.distance(point));
}
} }
TEST( Point2, distance ) { TEST( Point2, distance ) {
Matrix expectedH1, actualH1, expectedH2, actualH2; Matrix expectedH1, actualH1, expectedH2, actualH2;

76
gtsam/geometry/tests/testPose2.cpp
View File

@ -345,19 +345,21 @@ TEST(Pose2, inverse )
EXPECT(assert_equal(numericalH,actualDinverse)); EXPECT(assert_equal(numericalH,actualDinverse));
} }
/* ************************************************************************* */ namespace {
Vector homogeneous(const Point2& p) { /* ************************************************************************* */
return Vector_(3, p.x(), p.y(), 1.0); Vector homogeneous(const Point2& p) {
} return Vector_(3, p.x(), p.y(), 1.0);
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix matrix(const Pose2& gTl) { Matrix matrix(const Pose2& gTl) {
Matrix gRl = gTl.r().matrix(); Matrix gRl = gTl.r().matrix();
Point2 gt = gTl.t(); Point2 gt = gTl.t();
return Matrix_(3, 3, return Matrix_(3, 3,
gRl(0, 0), gRl(0, 1), gt.x(), gRl(0, 0), gRl(0, 1), gt.x(),
gRl(1, 0), gRl(1, 1), gt.y(), gRl(1, 0), gRl(1, 1), gt.y(),
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -487,14 +489,16 @@ TEST(Pose2, members)
EXPECT(pose.dim() == 3); EXPECT(pose.dim() == 3);
} }
/* ************************************************************************* */ namespace {
// some shared test values /* ************************************************************************* */
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0); // some shared test values
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) { Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) {
return pose.bearing(pt); return pose.bearing(pt);
}
} }
TEST( Pose2, bearing ) TEST( Pose2, bearing )
@ -529,8 +533,10 @@ TEST( Pose2, bearing )
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) { namespace {
return pose.bearing(pt); Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
return pose.bearing(pt);
}
} }
TEST( Pose2, bearing_pose ) TEST( Pose2, bearing_pose )
@ -567,8 +573,10 @@ TEST( Pose2, bearing_pose )
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector range_proxy(const Pose2& pose, const Point2& point) { namespace {
return LieVector(pose.range(point)); LieVector range_proxy(const Pose2& pose, const Point2& point) {
return LieVector(pose.range(point));
}
} }
TEST( Pose2, range ) TEST( Pose2, range )
{ {
@ -602,8 +610,10 @@ TEST( Pose2, range )
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { namespace {
return LieVector(pose.range(point)); LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
return LieVector(pose.range(point));
}
} }
TEST( Pose2, range_pose ) TEST( Pose2, range_pose )
{ {
@ -690,17 +700,19 @@ TEST(Pose2, align_3) {
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
} }
/* ************************************************************************* */ namespace {
// Prototype code to align two triangles using a rigid transform /* ************************************************************************* */
/* ************************************************************************* */ // Prototype code to align two triangles using a rigid transform
struct Triangle { size_t i_,j_,k_;}; /* ************************************************************************* */
struct Triangle { size_t i_,j_,k_;};
boost::optional<Pose2> align(const vector<Point2>& ps, const vector<Point2>& qs, boost::optional<Pose2> align(const vector<Point2>& ps, const vector<Point2>& qs,
const pair<Triangle, Triangle>& trianglePair) { const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
vector<Point2Pair> correspondences; vector<Point2Pair> correspondences;
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_]); 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); return align(correspondences);
}
} }
TEST(Pose2, align_4) { TEST(Pose2, align_4) {

2
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
* Smart factor for range SLAM * Smart factor for range SLAM
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class GTSAM_UNSTABLE_EXPORT SmartRangeFactor: public NoiseModelFactor { class SmartRangeFactor: public NoiseModelFactor {
protected: protected:
struct Circle2 { struct Circle2 {