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

Second attempt at a logical refactor of Unit3::basis method

release/4.3a0
Andrei Costinescu 2018-10-13 06:56:04 -04:00
parent 658ec8c17b
commit 88c4bd0a33
1 changed files with 38 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/geometry/Unit3.cpp Normal file → Executable file
View File

@ -69,70 +69,63 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
#ifdef GTSAM_USE_TBB
// NOTE(hayk): At some point it seemed like this reproducably resulted in
// deadlock. However, I can't see reason why and I can no longer reproduce it.
// It may have been a red herring, or there is still a latent bug.
// deadlock. However, I don't know why and I can no longer reproduce it.
// It either was a red herring or there is still a latent bug left to debug.
tbb::mutex::scoped_lock lock(B_mutex_);
#endif
Point3 n, axis;
if (!B_ || (H && !H_B_)) {
bool cachedBasis = static_cast<bool>(B_);
Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1;
if (!cachedBasis) {
// Get the unit vector
// NOTE(hayk): can't call point3(), because would recursively call basis().
n = Point3(p_);
// NOTE(hayk): We can't call point3(), due to the recursive call of basis().
const Point3 n(p_);
// Get the axis of rotation with the minimum projected length of the point
axis = Point3(0, 0, 1);
Point3 axis(0, 0, 1);
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
if ((mx <= my) && (mx <= mz)) {
axis = Point3(1.0, 0.0, 0.0);
} else if ((my <= mx) && (my <= mz)) {
axis = Point3(0.0, 1.0, 0.0);
}
// Choose the direction of the first basis vector b1 in the tangent plane
// by crossing n with the chosen axis.
Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr);
// Normalize result to get a unit vector: b1 = B1 / |B1|.
Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr);
// Get the second basis vector b2, through the cross-product of n and b1.
// No need to normalize this, p and b1 are orthogonal unit vectors.
Point3 b2 =
gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
// Create the basis by stacking b1 and b2.
Matrix32 stacked;
stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
B_.reset(stacked);
}
if (H) {
if (!H_B_) {
// Compute Jacobian. Possibly recomputes B_
if (!cachedBasis || !H_B_) {
// If Jacobian not cached or the basis was not cached, recompute it.
// Chain rule tomfoolery to compute the derivative.
const Matrix32& H_n_p = *B_;
const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
// Choose the direction of the first basis vector b1 in the tangent plane
// by crossing n with the chosen axis.
Matrix33 H_B1_n;
const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);
// Normalize result to get a unit vector: b1 = B1 / |B1|.
Matrix32 B;
Matrix33 H_b1_B1;
B.col(0) = normalize(B1, &H_b1_B1);
// Get the second basis vector b2, which is orthogonal to n and b1.
Matrix33 H_b2_n, H_b2_b1;
B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1);
// Chain rule tomfoolery to compute the jacobian.
Matrix62 jacobian;
const Matrix32& H_n_p = B;
jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p;
auto H_b1_p = jacobian.block<3, 2>(0, 0);
jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
// Cache the result and jacobian
B_.reset(B);
H_B_.reset(jacobian);
// Cache the derivative and fill the result.
Matrix62 derivative;
derivative << H_b1_p, H_b2_p;
H_B_.reset(derivative);
}
// Return cached jacobian, possibly computed just above
*H = *H_B_;
}
if (!B_) {
// Same calculation as above, without derivatives.
// Done after H block, as that possibly computes B_ for the first time
Matrix32 B;
const Point3 B1 = gtsam::cross(n, axis);
B.col(0) = normalize(B1);
B.col(1) = gtsam::cross(n, B.col(0));
B_.reset(B);
}
return *B_;
}
@ -262,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const {
std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
return Unit3(exp_p_xi_hat);
}
/* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& other) const {
const double x = p_.dot(other.p_);