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

make check works in this commit

release/4.3a0
nsrinivasan7 2014-11-30 15:42:50 -05:00
parent 9137123f5f
commit 8457ef4182
4 changed files with 26 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/Pose2.cpp
View File

@ -198,7 +198,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point, Rot2 Pose2::bearing(const Point2& point,
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = transform_to(point, H1, H2); Point2 d = transform_to(point, H1, H2);
if (!H1 && !H2) return Rot2::relativeBearing(d); if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix D_result_d; Matrix D_result_d;
@ -209,27 +209,29 @@ Rot2 Pose2::bearing(const Point2& point,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point, Rot2 Pose2::bearing(const Pose2& pose,
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 result = bearing(point.t(), H1, H2); Rot2 result = bearing(pose.t(), H1, H2);
if (H2) { if (H2) {
Matrix2 H2_ = *H2 * point.r().matrix(); Matrix H2_ = *H2 * pose.r().matrix();
*H2 = zeros(1, 3); *H2 = zeros(1, 3);
(*H2).block(0, 0, H2_.rows(), H2_.cols()) = H2_; insertSub(*H2, H2_, 0, 0);
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose2::range(const Point2& point, double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
Point2 d = point - t_; Point2 d = point - t_;
if (!H1 && !H2) return d.norm(); if (!H1 && !H2) return d.norm();
Matrix H; Matrix12 H;
double r = d.norm(H); double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) << if (H1) {
-r_.c(), r_.s(), 0.0, Matrix23 mvalue; // is this the correct name ?
-r_.s(), -r_.c(), 0.0).finished(); mvalue << -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 = H * mvalue;
}
if (H2) *H2 = H; if (H2) *H2 = H;
return r; return r;
} }

10
gtsam/geometry/Pose2.h
View File

@ -226,8 +226,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
Rot2 bearing(const Point2& point, Rot2 bearing(const Point2& point,
OptionalJacobian<2, 3> H1=boost::none, boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<2, 2> H2=boost::none) const;
/** /**
* Calculate bearing to another pose * Calculate bearing to another pose
@ -235,8 +234,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
Rot2 bearing(const Pose2& point, Rot2 bearing(const Pose2& point,
OptionalJacobian<2, 3> H1=boost::none, boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<2, 2> H2=boost::none) const;
/** /**
* Calculate range to a landmark * Calculate range to a landmark
@ -244,8 +242,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Point2& point, double range(const Point2& point,
boost::optional<Matrix&> H1=boost::none, OptionalJacobian<1, 3> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const; OptionalJacobian<1, 2> H2=boost::none) const;
/** /**
* Calculate range to another pose * Calculate range to another pose

10
gtsam/geometry/Rot2.cpp
View File

@ -99,12 +99,14 @@ Point2 Rot2::unrotate(const Point2& p,
Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) { if(fabs(n) > 1e-5) {
if (H) if (H) {
*H << -y / d2, x / d2; (*H).block(0,0,1,2) << -y / d2, x / d2;
}
return Rot2::fromCosSin(x / n, y / n); return Rot2::fromCosSin(x / n, y / n);
} else { } else {
if (H) if (H) {
(*H) << 0.0, 0.0; (*H).block(0,0,1,2) << 0.0, 0.0;
}
return Rot2(); return Rot2();
} }
} }

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

@ -519,7 +519,7 @@ TEST( Pose2, bearing )
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(bearing_proxy, x2, l3); expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2));
// establish bearing is indeed 45 degrees even if rotated // establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
@ -529,7 +529,7 @@ TEST( Pose2, bearing )
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
expectedH2 = numericalDerivative22(bearing_proxy, x3, l4); expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH2,actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */