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

change: add transform member class, and deprecate the static Transform class

release/4.3a0
lvzhaoyang 2015-06-25 02:11:28 -04:00
parent a740acfece
commit 13be5add6e
3 changed files with 72 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/geometry/OrientedPlane3.cpp
View File

@ -49,7 +49,7 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
}
if (Hp) {
Vector2 hpp = plane.n_.basis().transpose() * xr.translation().vector();
*Hp = zeros(3, 3);
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = n_hp;
Hp->block<1, 2>(2, 0) = hpp;
(*Hp)(2, 2) = 1;
@ -58,26 +58,48 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
Matrix23 n_hr;
Matrix22 n_hp;
Unit3 n_rotated = xr.rotation().unrotate(n_, n_hr, n_hp);
Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
if (Hr) {
*Hr = zeros(3, 6);
Hr->block<2, 3>(0, 0) = n_hr;
Hr->block<1, 3>(2, 3) = unit_vec;
}
if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = n_hp;
Hp->block<1, 2>(2, 0) = hpp;
(*Hp)(2, 2) = 1;
}
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector2 n_error = -n_.localCoordinates(plane.n_);
double d_error = d_ - plane.d_;
return Vector3(n_error(0), n_error(1), d_error);
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
Vector2 n_v(v(0), v(1));
Unit3 n_retracted = n_.retract(n_v);
double d_retracted = d_ + v(2);
return OrientedPlane3(n_retracted, d_retracted);
return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));
}
/* ************************************************************************* */
Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
Vector2 n_local = n_.localCoordinates(y.n_);
double d_local = d_ - y.d_;
Vector3 e;
return Vector3(n_local(0), n_local(1), -d_local);
}

17
gtsam/geometry/OrientedPlane3.h
View File

@ -79,16 +79,29 @@ public:
/// @}
/** Transforms a plane to the specified pose
* @param xr a transformation in current coordiante
* @param Hp optional Jacobian wrpt the destination plane
* @param Hr optional jacobian wrpt the pose transformation
* @return the transformed plane
*/
OrientedPlane3 transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/**
* @ deprecated the static method has wrong Jacobian order,
* please use the member method transform()
* @param The raw plane
* @param A transformation in current coordiante
* @param xr a transformation in current coordiante
* @param Hr optional jacobian wrpt the pose transformation
* @param Hp optional Jacobian wrpt the destination plane
* @return the transformed plane
*/
static OrientedPlane3 Transform(const OrientedPlane3& plane,
const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 3> Hp = boost::none);
/** Computes the error between two poses.
/** Computes the error between two planes.
* The error is a norm 1 difference in tangent space.
* @param the other plane
*/

44
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -48,36 +48,46 @@ TEST (OrientedPlane3, getMethods) {
EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector()));
}
//*******************************************************************************
OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) {
return OrientedPlane3::Transform(plane, xr);
}
OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) {
return plane.transform(xr);
}
TEST (OrientedPlane3, transform) {
// Test transforming a plane to a pose
gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0),
gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 expected_meas(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformed_plane = OrientedPlane3::Transform(plane, pose,
OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
none, none);
EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9));
OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9));
EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9));
// Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2;
{
expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(
boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
none);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
OrientedPlane3::Transform(plane, pose, actualH1, none);
// because the Transform class uses a wrong order of Jacobians in interface
expectedH1 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
OrientedPlane3::Transform(plane, pose, none, actualH2);
expectedH2 = numericalDerivative21(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}
{
expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3>(
boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
plane.transform(pose, actualH1, none);
expectedH1 = numericalDerivative21(transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
plane.transform(pose, none, actualH2);
expectedH2 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}
}
//*******************************************************************************