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> H2) const {
// Numerically calculate the derivative since this function doesn't provide
// one.
Vector2 n_error = -n_.localCoordinates(other.n_);
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_);
;
Matrix2 H_n_error_this = -numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, other.n_);
*H1 << H_n_error_this, Z_2x1, 0, 0, 1;
}
if (H2) {
*H2 = -I_3x3;
H2->block<2, 2>(0, 0) =
-numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, plane.n_);
;
Matrix H_n_error_other = -numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, other.n_);
*H2 << H_n_error_other, Z_2x1, 0, 0, -1;
}
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.
* 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> H2 = boost::none) const;
@ -106,7 +106,7 @@ public:
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives.
* 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, //
OptionalJacobian<3, 3> H2 = boost::none) const;