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

Removed transpose_. It did speed up things but was bad design. Will need to profile again and find different ways to cope with transpose() overhead. One way is to return a Eigen::Transpose<> object as hinted to in comments.

release/4.3a0
dellaert 2014-10-23 19:13:37 +02:00
parent 79efd2f3fc
commit cfe56a0aa8
4 changed files with 35 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Rot3.cpp
View File

@ -258,7 +258,7 @@ ostream &operator<<(ostream &os, const Rot3& R) {
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(transpose_*p.vector()); // q = Rt*p
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */

12
gtsam/geometry/Rot3.h
View File

@ -69,12 +69,6 @@ namespace gtsam {
Matrix3 rot_;
#endif
/**
* transpose() is used millions of times in linearize, so cache it
* This also avoids multiple expensive conversions in the quaternion case
*/
Matrix3 transpose_; ///< Cached R_.transpose()
public:
/// @name Constructors and named constructors
@ -375,11 +369,9 @@ namespace gtsam {
/**
* Return 3*3 transpose (inverse) rotation matrix
* Actually returns cached transpose
*/
const Matrix3& transpose() const {
return transpose_;
}
Matrix3 transpose() const;
// TODO: const Eigen::Transpose<const Matrix3> transpose() const;
/// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const;

15
gtsam/geometry/Rot3M.cpp
View File

@ -33,14 +33,13 @@ namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()), transpose_(Matrix3::Identity()) {}
Rot3::Rot3() : rot_(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();
}
/* ************************************************************************* */
@ -50,13 +49,11 @@ 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();
}
/* ************************************************************************* */
@ -64,12 +61,10 @@ 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 Quaternion& q) : rot_(q.toRotationMatrix()) {
transpose_ = rot_.transpose();
}
/* ************************************************************************* */
@ -172,10 +167,16 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_));
}
/* ************************************************************************* */
// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
Matrix3 Rot3::transpose() const {
return rot_.transpose();
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_;
return Rot3(transpose());
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */

41
gtsam/geometry/Rot3Q.cpp
View File

@ -30,36 +30,35 @@ namespace gtsam {
static const Matrix I3 = eye(3);
/* ************************************************************************* */
Rot3::Rot3() :
quaternion_(Quaternion::Identity()), transpose_(Matrix3::Identity()) {
}
Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
transpose_ << col1.x(), col1.y(), col1.z(), //
col2.x(), col2.y(), col2.z(), //
col3.x(), col3.y(), col3.z();
quaternion_ = Quaternion(transpose_.transpose());
}
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
quaternion_((Eigen::Matrix3d() <<
col1.x(), col2.x(), col3.x(),
col1.y(), col2.y(), col3.y(),
col1.z(), col2.z(), col3.z()).finished()) {}
/* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
double R23, double R31, double R32, double R33) {
transpose_ << R11, R21, R31, R12, R22, R32, R13, R23, R33;
quaternion_ = Quaternion(transpose_.transpose());
}
Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
quaternion_((Eigen::Matrix3d() <<
R11, R12, R13,
R21, R22, R23,
R31, R32, R33).finished()) {}
/* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) :
quaternion_(R), transpose_(R.transpose()) {}
quaternion_(R) {}
/* ************************************************************************* */
Rot3::Rot3(const Matrix& R) :
quaternion_(Matrix3(R)), transpose_(R.transpose()) {}
quaternion_(Matrix3(R)) {}
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) :
quaternion_(q), transpose_(q.toRotationMatrix().transpose()) {
quaternion_(q) {
}
/* ************************************************************************* */
@ -120,6 +119,14 @@ namespace gtsam {
return Rot3(quaternion_.inverse());
}
/* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because
// here we create an intermediate value by calling matrix()
// const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
Matrix3 Rot3::transpose() const {
return matrix().transpose();
}
/* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {