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

Cleaned up basis a bit

release/4.3a0
dellaert 2015-07-03 11:34:18 -07:00
parent 440a955710
commit d6ffe54fd2
1 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/Unit3.cpp
View File

@ -15,12 +15,12 @@
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert * @author Frank Dellaert
* @author Alex Trevor * @author Alex Trevor
* @author Zhaoyang Lv
* @brief The Unit3 class - basically a point on a unit sphere * @brief The Unit3 class - basically a point on a unit sphere
*/ */
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#include <gtsam/config.h> // for GTSAM_USE_TBB #include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef __clang__ #ifdef __clang__
@ -85,26 +85,24 @@ const Matrix32& Unit3::basis() const {
return *B_; return *B_;
// 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
Point3 axis; Vector3 axis;
double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
if ((mx <= my) && (mx <= mz)) if ((mx <= my) && (mx <= mz))
axis = Point3(1.0, 0.0, 0.0); axis = Vector3(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 = Vector3(0.0, 1.0, 0.0);
else if ((mz <= mx) && (mz <= my)) else if ((mz <= mx) && (mz <= my))
axis = Point3(0.0, 0.0, 1.0); axis = Vector3(0.0, 0.0, 1.0);
else else
assert(false); assert(false);
// Create the two basis vectors // Create the two basis vectors
Point3 b1 = p_.cross(axis); Vector3 b1 = p_.vector().cross(axis).normalized();
b1 = b1 / b1.norm(); Vector3 b2 = p_.vector().cross(b1).normalized();
Point3 b2 = p_.cross(b1);
b2 = b2 / b2.norm();
// Create the basis matrix // Create the basis matrix
B_.reset(Matrix32()); B_.reset(Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); (*B_) << b1, b2;
return *B_; return *B_;
} }