diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index f9f30970a..95290497d 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -31,33 +31,6 @@ void OrientedPlane3::print(const string& s) const { cout << s << " : " << coeffs << endl; } -/* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, - const Pose3& xr, OptionalJacobian<3, 6> Hr, - OptionalJacobian<3, 3> Hp) { - Matrix23 D_rotated_plane; - Matrix22 D_rotated_pose; - Unit3 n_rotated = xr.rotation().unrotate(plane.n_, D_rotated_plane, D_rotated_pose); - - Vector3 unit_vec = n_rotated.unitVector(); - double pred_d = plane.n_.unitVector().dot(xr.translation().vector()) + plane.d_; - - if (Hr) { - *Hr = zeros(3, 6); - Hr->block<2, 3>(0, 0) = D_rotated_plane; - Hr->block<1, 3>(2, 3) = unit_vec; - } - if (Hp) { - Vector2 hpp = plane.n_.basis().transpose() * xr.translation().vector(); - *Hp = Z_3x3; - Hp->block<2, 2>(0, 0) = D_rotated_pose; - Hp->block<1, 2>(2, 0) = hpp; - (*Hp)(2, 2) = 1; - } - - 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 { diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 2d2527a25..949e4a285 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -99,7 +99,9 @@ public: */ static OrientedPlane3 Transform(const OrientedPlane3& plane, const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, - OptionalJacobian<3, 3> Hp = boost::none); + OptionalJacobian<3, 3> Hp = boost::none) { + return plane.transform(xr, Hp, Hr); + } /** Computes the error between two planes. * The error is a norm 1 difference in tangent space.