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

75
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 { const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
// NOTE(hayk): At some point it seemed like this reproducably resulted in // 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. // deadlock. However, I don't know why and I can no longer reproduce it.
// It may have been a red herring, or there is still a latent bug. // It either was a red herring or there is still a latent bug left to debug.
tbb::mutex::scoped_lock lock(B_mutex_); tbb::mutex::scoped_lock lock(B_mutex_);
#endif #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 // Get the unit vector
// NOTE(hayk): can't call point3(), because would recursively call basis(). // NOTE(hayk): We can't call point3(), due to the recursive call of basis().
n = Point3(p_); const Point3 n(p_);
// Get the axis of rotation with the minimum projected length of the point // 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()); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
if ((mx <= my) && (mx <= mz)) { if ((mx <= my) && (mx <= mz)) {
axis = Point3(1.0, 0.0, 0.0); axis = Point3(1.0, 0.0, 0.0);
} else if ((my <= mx) && (my <= mz)) { } else if ((my <= mx) && (my <= mz)) {
axis = Point3(0.0, 1.0, 0.0); axis = Point3(0.0, 1.0, 0.0);
} }
}
if (H) {
if (!H_B_) {
// Compute Jacobian. Possibly recomputes B_
// Choose the direction of the first basis vector b1 in the tangent plane // Choose the direction of the first basis vector b1 in the tangent plane
// by crossing n with the chosen axis. // by crossing n with the chosen axis.
Matrix33 H_B1_n; Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr);
const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);
// Normalize result to get a unit vector: b1 = B1 / |B1|. // Normalize result to get a unit vector: b1 = B1 / |B1|.
Matrix32 B; Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr);
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. // Get the second basis vector b2, through the cross-product of n and b1.
Matrix33 H_b2_n, H_b2_b1; // No need to normalize this, p and b1 are orthogonal unit vectors.
B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); Point3 b2 =
gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
// Chain rule tomfoolery to compute the jacobian. // Create the basis by stacking b1 and b2.
Matrix62 jacobian; Matrix32 stacked;
const Matrix32& H_n_p = B; stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; B_.reset(stacked);
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;
if (H) {
// Cache the result and jacobian if (!cachedBasis || !H_B_) {
B_.reset(B); // If Jacobian not cached or the basis was not cached, recompute it.
H_B_.reset(jacobian); // 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;
// 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_; *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_; return *B_;
} }