Reordered functions to be in the same order in the header and cpp files
Richard Roberts
parent
fa4af2e211
commit
30508264d5
|
|
@ -199,6 +199,30 @@ namespace gtsam {
|
||||||
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
|
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
|
||||||
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
|
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
|
/// @name Manifold
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -294,30 +318,6 @@ namespace gtsam {
|
||||||
*/
|
*/
|
||||||
Quaternion toQuaternion() const;
|
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:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
|
|
|
||||||
|
|
@ -147,6 +147,92 @@ bool Rot3M::equals(const Rot3M & R, double tol) const {
|
||||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
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 Rot3M::matrix() const {
|
||||||
Matrix R(3,3);
|
Matrix R(3,3);
|
||||||
|
|
@ -207,79 +293,6 @@ Vector Rot3M::rpy() const {
|
||||||
return Vector_(3,q(0),q(1),q(2));
|
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 {
|
Quaternion Rot3M::toQuaternion() const {
|
||||||
return Quaternion((Eigen::Matrix3d() <<
|
return Quaternion((Eigen::Matrix3d() <<
|
||||||
|
|
@ -288,19 +301,6 @@ Quaternion Rot3M::toQuaternion() const {
|
||||||
r1_.z(), r2_.z(), r3_.z()).finished());
|
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) {
|
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -87,6 +87,71 @@ namespace gtsam {
|
||||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
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(); }
|
Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); }
|
||||||
|
|
||||||
|
|
@ -133,74 +198,9 @@ namespace gtsam {
|
||||||
return Vector_(3,q(0),q(1),q(2));
|
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_; }
|
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
|
} // namespace gtsam
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue