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

Reordered functions to be in the same order in the header and cpp files

release/4.3a0
Richard Roberts 2012-01-02 16:17:27 +00:00
parent fa4af2e211
commit 30508264d5
3 changed files with 175 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/geometry/Rot3.h
View File

@ -199,6 +199,30 @@ namespace gtsam {
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// @}
/// @name Manifold
/// @{
@ -294,30 +318,6 @@ namespace gtsam {
*/
Quaternion toQuaternion() const;
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;

172
gtsam/geometry/Rot3M.cpp
View File

@ -147,6 +147,92 @@ bool Rot3M::equals(const Rot3M & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Rot3M Rot3M::compose (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}
/* ************************************************************************* */
Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); }
/* ************************************************************************* */
Rot3M Rot3M::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -matrix();
return Rot3M(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/* ************************************************************************* */
Rot3M Rot3M::between (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
}
/* ************************************************************************* */
Rot3M Rot3M::operator*(const Rot3M& R2) const {
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/* ************************************************************************* */
Point3 Rot3M::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = matrix();
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3M::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
return q;
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3M::Logmap(const Rot3M& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
// FIXME should tr in statement below be absolute value?
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3);
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
double theta = acos((tr-1.0)/2.0);
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
return (0.5 + theta*theta/12)*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
// FIXME: in statement below, is this the right comparision?
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(fabs(R.r3().z() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
else if(fabs(R.r2().y() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
} else {
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
}
}
/* ************************************************************************* */
Matrix Rot3M::matrix() const {
Matrix R(3,3);
@ -207,79 +293,6 @@ Vector Rot3M::rpy() const {
return Vector_(3,q(0),q(1),q(2));
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3M::Logmap(const Rot3M& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
// FIXME should tr in statement below be absolute value?
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3);
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
double theta = acos((tr-1.0)/2.0);
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
return (0.5 + theta*theta/12)*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
// FIXME: in statement below, is this the right comparision?
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(fabs(R.r3().z() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
else if(fabs(R.r2().y() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
} else {
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
}
}
/* ************************************************************************* */
Point3 Rot3M::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = matrix();
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3M::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
return q;
}
/* ************************************************************************* */
Rot3M Rot3M::compose (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}
/* ************************************************************************* */
Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); }
/* ************************************************************************* */
Rot3M Rot3M::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -matrix();
return Rot3M(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/* ************************************************************************* */
Quaternion Rot3M::toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
@ -288,19 +301,6 @@ Quaternion Rot3M::toQuaternion() const {
r1_.z(), r2_.z(), r3_.z()).finished());
}
/* ************************************************************************* */
Rot3M Rot3M::between (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
}
/* ************************************************************************* */
Rot3M Rot3M::operator*(const Rot3M& R2) const {
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/* ************************************************************************* */
pair<Matrix, Vector> RQ(const Matrix& A) {

130
gtsam/geometry/Rot3Q.cpp
View File

@ -87,6 +87,71 @@ namespace gtsam {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Rot3Q Rot3Q::compose(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Point3 Rot3Q::operator*(const Point3& p) const {
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
return Point3(r(0), r(1), r(2));
}
/* ************************************************************************* */
Rot3Q Rot3Q::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -matrix();
return Rot3Q(quaternion_.inverse());
}
/* ************************************************************************* */
Rot3Q Rot3Q::between(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
}
/* ************************************************************************* */
Rot3Q Rot3Q::operator*(const Rot3Q& R2) const {
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Point3 Rot3Q::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
Eigen::Vector3d r = R * p.vector();
return Point3(r.x(), r.y(), r.z());
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3Q::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
return q;
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3Q::Logmap(const Rot3Q& R) {
Eigen::AngleAxisd angleAxis(R.quaternion_);
if(angleAxis.angle() > M_PI) // Important: use the smallest possible
angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep
if(angleAxis.angle() < -M_PI) // error continuous.
angleAxis.angle() += 2.0*M_PI;
return angleAxis.axis() * angleAxis.angle();
}
/* ************************************************************************* */
Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); }
@ -133,74 +198,9 @@ namespace gtsam {
return Vector_(3,q(0),q(1),q(2));
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3Q::Logmap(const Rot3Q& R) {
Eigen::AngleAxisd angleAxis(R.quaternion_);
if(angleAxis.angle() > M_PI) // Important: use the smallest possible
angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep
if(angleAxis.angle() < -M_PI) // error continuous.
angleAxis.angle() += 2.0*M_PI;
return angleAxis.axis() * angleAxis.angle();
}
/* ************************************************************************* */
Point3 Rot3Q::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
Eigen::Vector3d r = R * p.vector();
return Point3(r.x(), r.y(), r.z());
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3Q::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
return q;
}
/* ************************************************************************* */
Rot3Q Rot3Q::compose(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Point3 Rot3Q::operator*(const Point3& p) const {
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
return Point3(r(0), r(1), r(2));
}
/* ************************************************************************* */
Rot3Q Rot3Q::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -matrix();
return Rot3Q(quaternion_.inverse());
}
/* ************************************************************************* */
Quaternion Rot3Q::toQuaternion() const { return quaternion_; }
/* ************************************************************************* */
Rot3Q Rot3Q::between(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
}
/* ************************************************************************* */
Rot3Q Rot3Q::operator*(const Rot3Q& R2) const {
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
} // namespace gtsam