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

additional jacobian for retract and local and unit tests

release/4.3a0
akrishnan86 2020-05-09 08:28:44 -04:00
parent 479d619573
commit ad2c9b6683
3 changed files with 72 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/geometry/Line3.cpp
View File

@ -2,15 +2,21 @@
namespace gtsam {
Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const {
Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> Dp, OptionalJacobian<4, 4> Dv) const {
Vector3 w;
w << v[0], v[1], 0;
Rot3 incR;
if (H) {
if (Dp) {
Dp->setIdentity();
incR = Rot3::Expmap(w);
Dp->block<2, 2>(0, 0) = ((incR.matrix()).transpose()).block<2, 2>(0, 0);
}
if (Dv) {
Matrix3 Dw;
incR = Rot3::Expmap(w, Dw);
H->setIdentity();
H->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
Dv->setIdentity();
Dv->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
} else {
incR = Rot3::Expmap(w);
}
@ -18,18 +24,25 @@ Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const {
return Line3(Rt, a_ + v[2], b_ + v[3]);
}
Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H) const {
Vector3 localR;
Vector4 local;
if (H) {
Matrix3 Dw;
localR = Rot3::Logmap(R_.inverse() * q.R_, Dw);
H->setIdentity();
H->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
} else {
localR = Rot3::Logmap(R_.inverse() * q.R_);
Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> Dp,
OptionalJacobian<4, 4> Dq) const {
Vector3 omega;
Matrix3 D_log;
omega = Rot3::Logmap(R_.inverse() * q.R_, D_log);
if (Dp) {
Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix();
Matrix3 Dwp = D_log * D_log_wp;
Dp->setIdentity();
Dp->block<2, 2>(0, 0) = Dwp.block<2, 2>(0, 0);
(*Dp)(2, 2) = -1;
(*Dp)(3, 3) = -1;
}
local << localR[0], localR[1], q.a_ - a_, q.b_ - b_;
if (Dq) {
Dq->setIdentity();
Dq->block<2, 2>(0, 0) = D_log.block<2, 2>(0, 0);
}
Vector4 local;
local << omega[0], omega[1], q.a_ - a_, q.b_ - b_;
return local;
}
@ -92,9 +105,9 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
Matrix3 lRc = (cRl.matrix()).transpose();
Dpose->setZero();
// rotation
Dpose->block<2, 3>(0, 0) = -lRc.block<2,3>(0, 0);
Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0);
// translation
Dpose->block<2, 3>(2, 3) = -lRc.block<2,3>(0, 0);
Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0);
}
if (Dline) {
Dline->setIdentity();

25
gtsam/geometry/Line3.h
View File

@ -44,22 +44,31 @@ class Line3 {
/**
* The retract method maps from the tangent space back to the manifold.
* The method q = p + v, where p is this line, v is an increment along
* the tangent space and q is the resulting line.
* The tangent space for the rotation of a line is only two dimensional -
* rotation about x and y
* @param v: increment in tangent space
* @param H: Jacobian of retraction with respect to the increment
* @return: resulting line after adding the increment and mapping to the manifold
* @param Dp: increment of retraction with respect to this line
* @param Dv: Jacobian of retraction with respect to the increment
* @return q: resulting line after adding the increment and mapping to the manifold
*/
Line3 retract(const Vector4 &v, OptionalJacobian<4, 4> H = boost::none) const;
Line3 retract(const Vector4 &v,
OptionalJacobian<4, 4> Dp = boost::none,
OptionalJacobian<4, 4> Dv = boost::none) const;
/**
* The localCoordinates method is the inverse of retract and finds the difference
* between two lines in the tangent space.
* @param q Line3 on manifold
* @param H OptionalJacobian of localCoordinates with respect to line
* @return difference in the tangent space
* between two lines in the tangent space. It computes v = q - p where q is an input
* line, p is this line and v is their difference in the tangent space.
* @param q: Line3 on manifold
* @param Dp: OptionalJacobian of localCoordinates with respect to this line
* @param Dq: OptionalJacobian of localCoordinates with respect to this line
* @return v: difference in the tangent space
*/
Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H = boost::none) const;
Vector4 localCoordinates(const Line3 &q,
OptionalJacobian<4, 4> Dp = boost::none,
OptionalJacobian<4, 4> Dq = boost::none) const;
/**
* Print R, a, b

27
gtsam/geometry/tests/testLine3.cpp
View File

@ -41,11 +41,22 @@ TEST(Line3, localCoordinates) {
CHECK(assert_equal(l3.localCoordinates(l), v3));
// l4 and l differ in R_y
Rot3 r4;
r4 = r4.Expmap(Vector3(0, M_PI / 4, 0));
Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
Line3 l4(r4, 1, 1);
Vector4 v4(0, -M_PI / 4, 0, 0);
CHECK(assert_equal(l4.localCoordinates(l), v4));
// Jacobians
Line3 l5(Rot3::Expmap(Vector3(M_PI / 3, -M_PI / 4, 0)), -0.8, 2);
Values val;
val.insert(1, l);
val.insert(2, l5);
Line3_ l_(1);
Line3_ l5_(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
Vector4_ local_(l5_, &Line3::localCoordinates, l_);
ExpressionFactor<Vector4> f(model, l5.localCoordinates(l), local_);
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
}
// testing retract along 4 dimensions
@ -72,6 +83,18 @@ TEST(Line3, retract) {
Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
Line3 l4(r4, 1, 1);
EXPECT(l4.equals(l.retract(v4)));
// Jacobians
Vector4 v5(M_PI / 3, -M_PI / 4, -0.4, 1.2); // arbitrary vector
Values val;
val.insert(1, l);
val.insert(2, v5);
Line3_ l_(1);
Vector4_ v5_(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
Line3_ retract_(l_, &Line3::retract, v5_);
ExpressionFactor<Line3> f(model, l.retract(v5), retract_);
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
}
// testing manifold property - Retract(p, Local(p,q)) == q