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

10
gtsam/geometry/Pose2.h
View File

@ -226,8 +226,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
Rot2 bearing(const Point2& point,
OptionalJacobian<2, 3> H1=boost::none,
OptionalJacobian<2, 2> H2=boost::none) const;
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate bearing to another pose
@ -235,8 +234,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
Rot2 bearing(const Pose2& point,
OptionalJacobian<2, 3> H1=boost::none,
OptionalJacobian<2, 2> H2=boost::none) const;
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to a landmark
@ -244,8 +242,8 @@ public:
* @return range (double)
*/
double range(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 2> H2=boost::none) const;
/**
* 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) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H)
*H << -y / d2, x / d2;
if (H) {
(*H).block(0,0,1,2) << -y / d2, x / d2;
}
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H)
(*H) << 0.0, 0.0;
if (H) {
(*H).block(0,0,1,2) << 0.0, 0.0;
}
return Rot2();
}
}

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

@ -519,7 +519,7 @@ TEST( Pose2, bearing )
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1));
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
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
@ -529,7 +529,7 @@ TEST( Pose2, bearing )
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */