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

Just always store transpose? Problem with optional was that the *empty* optional was copied from the Values, so we gained nothing. 

However, this is expensive space-wise (double), and optimizes for a particular usage of Rot3, so does not seem good practice (see stack overflow discussion, as well). But the alternative is cumbersome.
release/4.3a0
dellaert 2014-10-15 14:28:47 +02:00
parent c4bf680e96
commit 5bcc3d922c
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Rot3.h
View File

@ -74,7 +74,7 @@ namespace gtsam {
* transpose() is used millions of times in linearize, so cache it
* This also avoids multiple expensive conversions in the quaternion case
*/
mutable boost::optional<Matrix3> transpose_; ///< Cached R_.transpose()
Matrix3 transpose_; ///< Cached R_.transpose()
public:
@ -376,12 +376,10 @@ namespace gtsam {
/**
* Return 3*3 transpose (inverse) rotation matrix
* Actually returns cached transpose, or computes it if not yet done
* Actually returns cached transpose
*/
const Matrix3& transpose() const {
if (!transpose_)
transpose_.reset(inverse().matrix());
return *transpose_;
return transpose_;
}
/// @deprecated, this is base 1, and was just confusing

20
gtsam/geometry/Rot3M.cpp
View File

@ -33,13 +33,14 @@ namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
Rot3::Rot3() : rot_(Matrix3::Identity()), transpose_(Matrix3::Identity()) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1.vector();
rot_.col(1) = col2.vector();
rot_.col(2) = col3.vector();
transpose_ = rot_.transpose();
}
/* ************************************************************************* */
@ -49,11 +50,13 @@ Rot3::Rot3(double R11, double R12, double R13,
rot_ << R11, R12, R13,
R21, R22, R23,
R31, R32, R33;
transpose_ = rot_.transpose();
}
/* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) {
rot_ = R;
transpose_ = rot_.transpose();
}
/* ************************************************************************* */
@ -61,13 +64,13 @@ Rot3::Rot3(const Matrix& R) {
if (R.rows()!=3 || R.cols()!=3)
throw invalid_argument("Rot3 constructor expects 3*3 matrix");
rot_ = R;
transpose_ = rot_.transpose();
}
///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
transpose_ = rot_.transpose();
}
/* ************************************************************************* */
Rot3 Rot3::Rx(double t) {
@ -172,7 +175,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose()));
return Rot3(transpose());
}
/* ************************************************************************* */
@ -180,7 +183,8 @@ Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3;
return Rot3(Matrix3(rot_.transpose()*R2.rot_));
Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
}
/* ************************************************************************* */
@ -312,7 +316,7 @@ Quaternion Rot3::toQuaternion() const {
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(rot_.transpose()*p.vector()); // q = Rt*p
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */