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

remove deprecated Unit3::error() which is replaced by Unit3::errorVector()

release/4.3a0
David Wisth 2021-02-15 14:43:28 +00:00
parent 0ad488c567
commit 5087d36ab1
6 changed files with 7 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Unit3.cpp
View File

@ -187,16 +187,6 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
return d;
}
/* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
const Vector2 xi = basis().transpose() * q.p_;
if (H_q) {
*H_q = basis().transpose() * q.basis();
}
return xi;
}
/* ************************************************************************* */
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
OptionalJacobian<2, 2> H_q) const {
@ -236,7 +226,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
/* ************************************************************************* */
double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
Matrix2 H_xi_q;
const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
const Vector2 xi = errorVector(q, boost::none, H ? &H_xi_q : nullptr);
const double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * H_xi_q;

4
gtsam/geometry/Unit3.h
View File

@ -152,10 +152,6 @@ public:
GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
OptionalJacobian<1,2> H2 = boost::none) const;
/// Signed, vector-valued error between two directions
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
/// Signed, vector-valued error between two directions
/// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //

28
gtsam/geometry/tests/testUnit3.cpp
View File

@ -145,30 +145,6 @@ TEST(Unit3, dot) {
}
}
//*******************************************************************************
TEST(Unit3, error) {
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
r = p.retract(Vector2(0.8, 0));
EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
}
//*******************************************************************************
TEST(Unit3, error2) {
Unit3 p(0.1, -0.2, 0.8);
@ -487,10 +463,10 @@ TEST(Unit3, ErrorBetweenFactor) {
TEST(Unit3, CopyAssign) {
Unit3 p{1, 0.2, 0.3};
EXPECT(p.error(p).isZero());
EXPECT(p.errorVector(p).isZero());
p = Unit3{-1, 2, 8};
EXPECT(p.error(p).isZero());
EXPECT(p.errorVector(p).isZero());
}
/* ************************************************************************* */

4
gtsam/navigation/AttitudeFactor.cpp
View File

@ -29,13 +29,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb,
Matrix23 D_nRef_R;
Matrix22 D_e_nRef;
Unit3 nRef = nRb.rotate(bRef_, D_nRef_R);
Vector e = nZ_.error(nRef, D_e_nRef);
Vector e = nZ_.errorVector(nRef, boost::none, D_e_nRef);
(*H) = D_e_nRef * D_nRef_R;
return e;
} else {
Unit3 nRef = nRb * bRef_;
return nZ_.error(nRef);
return nZ_.errorVector(nRef);
}
}

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -67,7 +67,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Matrix2 H_p;
Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr);
Vector e = n_hat_p.errorVector(n_hat_q, boost::none, H ? &H_p : nullptr);
if (H) {
H->resize(2, 3);
*H << H_p, Z_2x1;

2
gtsam/slam/RotateFactor.h
View File

@ -104,7 +104,7 @@ public:
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
Unit3 i_q = iRc * c_z_;
Vector error = i_p_.error(i_q, H);
Vector error = i_p_.errorVector(i_q, boost::none, H);
if (H) {
Matrix DR;
iRc.rotate(c_z_, DR);