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

Cleaned up derivative code

release/4.3a0
Frank Dellaert 2021-01-21 15:02:54 -05:00
parent 6c39730872
commit 1b36c7497e
2 changed files with 11 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/geometry/OrientedPlane3.cpp
View File

@ -60,28 +60,21 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane, Vector3 OrientedPlane3::error(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H2) const {
// Numerically calculate the derivative since this function doesn't provide Vector2 n_error = -n_.localCoordinates(other.n_);
// one.
const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); const auto f = boost::bind(&Unit3::localCoordinates, _1, _2);
Vector2 n_error = -n_.localCoordinates(plane.n_);
if (H1) { if (H1) {
*H1 = I_3x3; Matrix2 H_n_error_this = -numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, other.n_);
H1->block<2, 2>(0, 0) = *H1 << H_n_error_this, Z_2x1, 0, 0, 1;
-numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, plane.n_);
;
} }
if (H2) { if (H2) {
*H2 = -I_3x3; Matrix H_n_error_other = -numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, other.n_);
H2->block<2, 2>(0, 0) = *H2 << H_n_error_other, Z_2x1, 0, 0, -1;
-numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, plane.n_);
;
} }
return Vector3(n_error(0), n_error(1), d_ - plane.d_); return Vector3(n_error(0), n_error(1), d_ - other.d_);
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/geometry/OrientedPlane3.h
View File

@ -96,9 +96,9 @@ public:
/** Computes the error between two planes. /** Computes the error between two planes.
* The error is a norm 1 difference in tangent space. * The error is a norm 1 difference in tangent space.
* @param plane The other plane * @param other The other plane
*/ */
Vector3 error(const OrientedPlane3& plane, Vector3 error(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = boost::none) const;
@ -106,7 +106,7 @@ public:
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives. * Unit3::localCoordinates. This one has correct derivatives.
* NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. * NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
* @param the other plane * @param other the other plane
*/ */
Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, // Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = boost::none) const;