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

Got rid of static versions of methods

release/4.3a0
Frank Dellaert 2021-01-21 14:53:13 -05:00
parent 68887f2da6
commit 6c39730872
3 changed files with 35 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/geometry/OrientedPlane3.cpp
View File

@ -17,11 +17,11 @@
* @brief A plane, represented by a normal direction and perpendicular distance
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
#include <gtsam/base/numericalDerivative.h>
using namespace std;
@ -34,8 +34,9 @@ void OrientedPlane3::print(const string& s) const {
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
Matrix23 D_rotated_plane;
Matrix22 D_rotated_pose;
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
@ -60,38 +61,44 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane,
OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
// Numerically calculate the derivative since this function doesn't provide one.
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
// Numerically calculate the derivative since this function doesn't provide
// one.
const auto f = boost::bind(&Unit3::localCoordinates, _1, _2);
Vector2 n_error = -n_.localCoordinates(plane.n_);
if (H1) {
*H1 = I_3x3;
H1->block<2,2>(0,0) = -numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, plane.n_);;
H1->block<2, 2>(0, 0) =
-numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, plane.n_);
;
}
if (H2) {
*H2 = -I_3x3;
H2->block<2,2>(0,0) = -numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, plane.n_);;
H2->block<2, 2>(0, 0) =
-numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, plane.n_);
;
}
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix22 H_n_error_this, H_n_error_other;
Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0,
H2 ? &H_n_error_other : 0);
H2 ? &H_n_error_other : 0);
double d_error = d_ - other.d_;
if (H1) {
*H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1;
*H1 << H_n_error_this, Z_2x1, 0, 0, 1;
}
if (H2) {
*H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1;
*H2 << H_n_error_other, Z_2x1, 0, 0, -1;
}
return Vector3(n_error(0), n_error(1), d_error);
@ -103,7 +110,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector3& v,
Matrix22 H_n;
Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
if (H) {
*H << H_n, Vector2::Zero(), 0, 0, 1;
*H << H_n, Z_2x1, 0, 0, 1;
}
return OrientedPlane3(n_retract, d_ + v(2));
}

27
gtsam/geometry/OrientedPlane3.h
View File

@ -91,35 +91,16 @@ public:
* @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 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) {
return plane.transform(xr, Hp, Hr);
}
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/** Computes the error between two planes.
* The error is a norm 1 difference in tangent space.
* @param plane The other plane
*/
Vector3 error(const OrientedPlane3& plane,
OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) {
return plane1.error(plane2);
}
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses

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

@ -50,44 +50,27 @@ TEST (OrientedPlane3, getMethods) {
//*******************************************************************************
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(OrientedPlane3, transform) {
gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0),
gtsam::Point3(2.0, 3.0, 4.0));
gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
none, none);
OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5));
EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5));
OrientedPlane3 transformedPlane = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5));
// Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2;
{
// because the Transform class uses a wrong order of Jacobians in interface
OrientedPlane3::Transform(plane, pose, actualH1, none);
expectedH1 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
OrientedPlane3::Transform(plane, pose, none, actualH2);
expectedH2 = numericalDerivative21(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
{
plane.transform(pose, actualH1, none);
expectedH1 = numericalDerivative21(transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
plane.transform(pose, none, actualH2);
expectedH2 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
expectedH1 = numericalDerivative21(transform_, plane, pose);
plane.transform(pose, actualH1, none);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
expectedH2 = numericalDerivative22(transform_, plane, pose);
plane.transform(pose, none, actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
//*******************************************************************************