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

bearing test

release/4.3a0
Frank Dellaert 2015-07-11 18:32:10 -07:00
parent 5052eb2c64
commit 6b037ea492
2 changed files with 29 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/Pose3.cpp
View File

@ -315,16 +315,16 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
/* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {
Matrix36 D1;
Matrix3 D2;
Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
Matrix36 D_local_pose;
Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) {
return local.norm();
} else {
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
if (H1) *H1 = D_r_local * D1;
if (H2) *H2 = D_r_local * D2;
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;
}
}
@ -332,25 +332,25 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
/* ************************************************************************* */
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 6> H2) const {
Matrix13 D2;
double r = range(pose.translation(), H1, H2 ? &D2 : 0);
if (H2) *H2 << Matrix13::Zero(), D2 * pose.rotation().matrix();
Matrix13 D_local_point;
double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
return r;
}
/* ************************************************************************* */
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
OptionalJacobian<2, 3> H2) const {
Matrix36 D1;
Matrix3 D2;
Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
Matrix36 D_local_pose;
Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) {
return Unit3(local);
} else {
Matrix23 D_b_local;
Unit3 b = Unit3::FromPoint3(local, D_b_local);
if (H1)* H1 = D_b_local * D1;
if (H2) *H2 = D_b_local * D2;
if (H1) *H1 = D_b_local * D_local_pose;
if (H2) *H2 = D_b_local * D_local_point;
return b;
}
}

16
gtsam/geometry/tests/testPose3.cpp
View File

@ -633,6 +633,22 @@ TEST( Pose3, range_pose )
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
return pose.bearing(point);
}
TEST( Pose3, bearing )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
TEST( Pose3, unicycle )
{