Merged changes from develop
dellaert
parent
6be91a2df2
commit
0464b38ca0
|
|
@ -1,198 +1,209 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Rot3.cpp
|
||||
* @brief Rotation, common code between Rotation matrix and Quaternion
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <boost/random.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static const Matrix3 I3 = Matrix3::Identity();
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot3::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
|
||||
return rodriguez((Vector)w.vector(),theta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
|
||||
return rodriguez(w.point3(),theta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Random(boost::random::mt19937 & rng) {
|
||||
// TODO allow any engine without including all of boost :-(
|
||||
Sphere2 w = Sphere2::Random(rng);
|
||||
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
|
||||
double angle = randomAngle(rng);
|
||||
return rodriguez(w,angle);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::operator*(const Point3& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::rotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = rotate(p.point3(Hp));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
|
||||
if (HR)
|
||||
(*HR) = -q.basis().transpose() * matrix() * p.skew();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::operator*(const Sphere2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3::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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s1 = sin(vi)/vi;
|
||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpInvL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
||||
Matrix res = eye(3) + 0.5*x - s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3();
|
||||
else if(index == 2)
|
||||
return r2();
|
||||
else if(index == 1)
|
||||
return r1(); // default returns r1
|
||||
else
|
||||
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::rpy() const {
|
||||
return xyz();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::quaternion() const {
|
||||
Quaternion q = toQuaternion();
|
||||
Vector v(4);
|
||||
v(0) = q.w();
|
||||
v(1) = q.x();
|
||||
v(2) = q.y();
|
||||
v(3) = q.z();
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Rot3& R) {
|
||||
os << "\n";
|
||||
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
|
||||
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
|
||||
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Rot3.cpp
|
||||
* @brief Rotation, common code between Rotation matrix and Quaternion
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <boost/random.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static const Matrix3 I3 = Matrix3::Identity();
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot3::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
|
||||
return rodriguez((Vector)w.vector(),theta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
|
||||
return rodriguez(w.point3(),theta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Random(boost::random::mt19937 & rng) {
|
||||
// TODO allow any engine without including all of boost :-(
|
||||
Sphere2 w = Sphere2::Random(rng);
|
||||
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
|
||||
double angle = randomAngle(rng);
|
||||
return rodriguez(w,angle);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::operator*(const Point3& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::rotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = rotate(p.point3(Hp));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
|
||||
if (HR)
|
||||
(*HR) = -q.basis().transpose() * matrix() * p.skew();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::unrotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = unrotate(p.point3(Hp));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
|
||||
if (HR)
|
||||
(*HR) = q.basis().transpose() * q.skew();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::operator*(const Sphere2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3::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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s1 = sin(vi)/vi;
|
||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpInvL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
||||
Matrix res = eye(3) + 0.5*x - s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3();
|
||||
else if(index == 2)
|
||||
return r2();
|
||||
else if(index == 1)
|
||||
return r1(); // default returns r1
|
||||
else
|
||||
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::rpy() const {
|
||||
return xyz();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::quaternion() const {
|
||||
Quaternion q = toQuaternion();
|
||||
Vector v(4);
|
||||
v(0) = q.w();
|
||||
v(1) = q.x();
|
||||
v(2) = q.y();
|
||||
v(3) = q.z();
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Rot3& R) {
|
||||
os << "\n";
|
||||
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
|
||||
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
|
||||
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -331,6 +331,10 @@ namespace gtsam {
|
|||
Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
|
||||
boost::optional<Matrix&> Hp = boost::none) const;
|
||||
|
||||
/// unrotate 3D direction from world frame to rotated coordinate frame
|
||||
Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
|
||||
boost::optional<Matrix&> Hp = boost::none) const;
|
||||
|
||||
/// rotate 3D direction from rotated coordinate frame to world frame
|
||||
Sphere2 operator*(const Sphere2& p) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,308 +1,308 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Rot3M.cpp
|
||||
* @brief Rotation (internal: 3*3 matrix representation*)
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
|
||||
|
||||
#ifndef GTSAM_USE_QUATERNIONS
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static const Matrix3 I3 = 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();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(double R11, double R12, double R13,
|
||||
double R21, double R22, double R23,
|
||||
double R31, double R32, double R33) {
|
||||
rot_ << R11, R12, R13,
|
||||
R21, R22, R23,
|
||||
R31, R32, R33;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Matrix3& R) {
|
||||
rot_ = R;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Matrix& R) {
|
||||
if (R.rows()!=3 || R.cols()!=3)
|
||||
throw invalid_argument("Rot3 constructor expects 3*3 matrix");
|
||||
rot_ = R;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
#ifndef NDEBUG
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
#endif
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3(
|
||||
c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(Matrix3(rot_*R2.rot_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -rot_;
|
||||
return Rot3(Matrix3(rot_.transpose()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1 || H2) {
|
||||
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = rot_;
|
||||
}
|
||||
return Point3(rot_ * p.vector());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector3 Rot3::Logmap(const Rot3& R) {
|
||||
|
||||
static const double PI = boost::math::constants::pi<double>();
|
||||
|
||||
const Matrix3& rot = R.rot_;
|
||||
// Get trace(R)
|
||||
double tr = rot.trace();
|
||||
|
||||
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
// we do something special
|
||||
if (std::abs(tr+1.0) < 1e-10) {
|
||||
if(std::abs(rot(2,2)+1.0) > 1e-10)
|
||||
return (PI / sqrt(2.0+2.0*rot(2,2) )) *
|
||||
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
|
||||
else if(std::abs(rot(1,1)+1.0) > 1e-10)
|
||||
return (PI / sqrt(2.0+2.0*rot(1,1))) *
|
||||
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
|
||||
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
|
||||
return (PI / sqrt(2.0+2.0*rot(0,0))) *
|
||||
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
|
||||
} else {
|
||||
double magnitude;
|
||||
double tr_3 = tr-3.0; // always negative
|
||||
if (tr_3<-1e-7) {
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
magnitude = theta/(2.0*sin(theta));
|
||||
} else {
|
||||
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
|
||||
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
|
||||
magnitude = 0.5 - tr_3*tr_3/12.0;
|
||||
}
|
||||
return magnitude*Vector3(
|
||||
rot(2,1)-rot(1,2),
|
||||
rot(0,2)-rot(2,0),
|
||||
rot(1,0)-rot(0,1));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retractCayley(const Vector& omega) const {
|
||||
const double x = omega(0), y = omega(1), z = omega(2);
|
||||
const double x2 = x * x, y2 = y * y, z2 = z * z;
|
||||
const double xy = x * y, xz = x * z, yz = y * z;
|
||||
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
|
||||
return (*this)
|
||||
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
|
||||
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
|
||||
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return (*this)*Expmap(omega);
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
return retractCayley(omega);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
Matrix Omega = skewSymmetric(omega);
|
||||
return (*this)*Cayley<3>(-Omega/2);
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return Logmap(between(T));
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
// Create a fixed-size matrix
|
||||
Eigen::Matrix3d A(between(T).matrix());
|
||||
// Mathematica closed form optimization (procrastination?) gone wild:
|
||||
const double a=A(0,0),b=A(0,1),c=A(0,2);
|
||||
const double d=A(1,0),e=A(1,1),f=A(1,2);
|
||||
const double g=A(2,0),h=A(2,1),i=A(2,2);
|
||||
const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
|
||||
const double M = 1 + e - f*h + i + e*i;
|
||||
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
|
||||
const double x = (a * f - cd + f) * K;
|
||||
const double y = (b * f - ce - c) * K;
|
||||
const double z = (fg - di - d) * K;
|
||||
return -2 * Vector3(x, y, z);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
// Create a fixed-size matrix
|
||||
Eigen::Matrix3d A(between(T).matrix());
|
||||
// using templated version of Cayley
|
||||
Eigen::Matrix3d Omega = Cayley<3>(A);
|
||||
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::matrix() const {
|
||||
return rot_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::transpose() const {
|
||||
return rot_.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3::toQuaternion() const {
|
||||
return Quaternion(rot_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#endif
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Rot3M.cpp
|
||||
* @brief Rotation (internal: 3*3 matrix representation*)
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
|
||||
|
||||
#ifndef GTSAM_USE_QUATERNIONS
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static const Matrix3 I3 = 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();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(double R11, double R12, double R13,
|
||||
double R21, double R22, double R23,
|
||||
double R31, double R32, double R33) {
|
||||
rot_ << R11, R12, R13,
|
||||
R21, R22, R23,
|
||||
R31, R32, R33;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Matrix3& R) {
|
||||
rot_ = R;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Matrix& R) {
|
||||
if (R.rows()!=3 || R.cols()!=3)
|
||||
throw invalid_argument("Rot3 constructor expects 3*3 matrix");
|
||||
rot_ = R;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
#ifndef NDEBUG
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
#endif
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3(
|
||||
c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(Matrix3(rot_*R2.rot_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -rot_;
|
||||
return Rot3(Matrix3(rot_.transpose()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1 || H2) {
|
||||
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = rot_;
|
||||
}
|
||||
return Point3(rot_ * p.vector());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector3 Rot3::Logmap(const Rot3& R) {
|
||||
|
||||
static const double PI = boost::math::constants::pi<double>();
|
||||
|
||||
const Matrix3& rot = R.rot_;
|
||||
// Get trace(R)
|
||||
double tr = rot.trace();
|
||||
|
||||
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
// we do something special
|
||||
if (std::abs(tr+1.0) < 1e-10) {
|
||||
if(std::abs(rot(2,2)+1.0) > 1e-10)
|
||||
return (PI / sqrt(2.0+2.0*rot(2,2) )) *
|
||||
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
|
||||
else if(std::abs(rot(1,1)+1.0) > 1e-10)
|
||||
return (PI / sqrt(2.0+2.0*rot(1,1))) *
|
||||
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
|
||||
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
|
||||
return (PI / sqrt(2.0+2.0*rot(0,0))) *
|
||||
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
|
||||
} else {
|
||||
double magnitude;
|
||||
double tr_3 = tr-3.0; // always negative
|
||||
if (tr_3<-1e-7) {
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
magnitude = theta/(2.0*sin(theta));
|
||||
} else {
|
||||
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
|
||||
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
|
||||
magnitude = 0.5 - tr_3*tr_3/12.0;
|
||||
}
|
||||
return magnitude*Vector3(
|
||||
rot(2,1)-rot(1,2),
|
||||
rot(0,2)-rot(2,0),
|
||||
rot(1,0)-rot(0,1));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retractCayley(const Vector& omega) const {
|
||||
const double x = omega(0), y = omega(1), z = omega(2);
|
||||
const double x2 = x * x, y2 = y * y, z2 = z * z;
|
||||
const double xy = x * y, xz = x * z, yz = y * z;
|
||||
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
|
||||
return (*this)
|
||||
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
|
||||
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
|
||||
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return (*this)*Expmap(omega);
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
return retractCayley(omega);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
Matrix Omega = skewSymmetric(omega);
|
||||
return (*this)*Cayley<3>(-Omega/2);
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return Logmap(between(T));
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
// Create a fixed-size matrix
|
||||
Eigen::Matrix3d A(between(T).matrix());
|
||||
// Mathematica closed form optimization (procrastination?) gone wild:
|
||||
const double a=A(0,0),b=A(0,1),c=A(0,2);
|
||||
const double d=A(1,0),e=A(1,1),f=A(1,2);
|
||||
const double g=A(2,0),h=A(2,1),i=A(2,2);
|
||||
const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
|
||||
const double M = 1 + e - f*h + i + e*i;
|
||||
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
|
||||
const double x = (a * f - cd + f) * K;
|
||||
const double y = (b * f - ce - c) * K;
|
||||
const double z = (fg - di - d) * K;
|
||||
return -2 * Vector3(x, y, z);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
// Create a fixed-size matrix
|
||||
Eigen::Matrix3d A(between(T).matrix());
|
||||
// using templated version of Cayley
|
||||
Eigen::Matrix3d Omega = Cayley<3>(A);
|
||||
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::matrix() const {
|
||||
return rot_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::transpose() const {
|
||||
return rot_.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3::toQuaternion() const {
|
||||
return Quaternion(rot_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -62,11 +62,6 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot3::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
|
||||
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @date Feb 02, 2011
|
||||
* @author Can Erdogan
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Trevor
|
||||
* @brief The Sphere2 class - basically a point on a unit sphere
|
||||
*/
|
||||
|
||||
|
|
@ -113,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::retract(const Vector& v) const {
|
||||
Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
|
||||
|
||||
// Get the vector form of the point and the basis matrix
|
||||
Vector p = Point3::Logmap(p_);
|
||||
|
|
@ -121,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const {
|
|||
|
||||
// Compute the 3D xi_hat vector
|
||||
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
|
||||
Vector newPoint = p + xi_hat;
|
||||
|
||||
if (mode == Sphere2::EXPMAP) {
|
||||
double xi_hat_norm = xi_hat.norm();
|
||||
|
||||
// Project onto the manifold, i.e. the closest point on the circle to the new location;
|
||||
// same as putting it onto the unit circle
|
||||
Vector projected = newPoint / newPoint.norm();
|
||||
// Avoid nan
|
||||
if (xi_hat_norm == 0.0) {
|
||||
if (v.norm () == 0.0)
|
||||
return Sphere2 (point3 ());
|
||||
else
|
||||
return Sphere2 (-point3 ());
|
||||
}
|
||||
|
||||
Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
|
||||
return Sphere2(exp_p_xi_hat);
|
||||
} else if (mode == Sphere2::RENORM) {
|
||||
// Project onto the manifold, i.e. the closest point on the circle to the new location;
|
||||
// same as putting it onto the unit circle
|
||||
Vector newPoint = p + xi_hat;
|
||||
Vector projected = newPoint / newPoint.norm();
|
||||
|
||||
return Sphere2(Point3::Expmap(projected));
|
||||
return Sphere2(Point3::Expmap(projected));
|
||||
} else {
|
||||
assert (false);
|
||||
exit (1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sphere2::localCoordinates(const Sphere2& y) const {
|
||||
Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
|
||||
|
||||
// Make sure that the angle different between x and y is less than 90. Otherwise,
|
||||
// we can project x + xi_hat from the tangent space at x to y.
|
||||
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
|
||||
if (mode == Sphere2::EXPMAP) {
|
||||
Matrix B = basis();
|
||||
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
double theta = acos(p.transpose() * q);
|
||||
|
||||
// Get the basis matrix
|
||||
Matrix B = basis();
|
||||
|
||||
// Create the vector forms of p and q (the Point3 of y).
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
|
||||
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
|
||||
double alpha = p.transpose() * q;
|
||||
assert(alpha != 0.0);
|
||||
Matrix coeffs = (B.transpose() * q) / alpha;
|
||||
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
|
||||
return result;
|
||||
// the below will be nan if theta == 0.0
|
||||
if (p == q)
|
||||
return (Vector (2) << 0, 0);
|
||||
else if (p == (Vector)-q)
|
||||
return (Vector (2) << M_PI, 0);
|
||||
|
||||
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
|
||||
Vector result = B.transpose() * result_hat;
|
||||
|
||||
return result;
|
||||
} else if (mode == Sphere2::RENORM) {
|
||||
// Make sure that the angle different between x and y is less than 90. Otherwise,
|
||||
// we can project x + xi_hat from the tangent space at x to y.
|
||||
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
|
||||
|
||||
// Get the basis matrix
|
||||
Matrix B = basis();
|
||||
|
||||
// Create the vector forms of p and q (the Point3 of y).
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
|
||||
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
|
||||
double alpha = p.transpose() * q;
|
||||
assert(alpha != 0.0);
|
||||
Matrix coeffs = (B.transpose() * q) / alpha;
|
||||
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
|
||||
return result;
|
||||
} else {
|
||||
assert (false);
|
||||
exit (1);
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @date Feb 02, 2011
|
||||
* @author Can Erdogan
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Trevor
|
||||
* @brief Develop a Sphere2 class - basically a point on a unit sphere
|
||||
*/
|
||||
|
||||
|
|
@ -22,6 +23,10 @@
|
|||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
|
||||
#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
|
||||
#define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
|
||||
#endif
|
||||
|
||||
// (Cumbersome) forward declaration for random generator
|
||||
namespace boost {
|
||||
namespace random {
|
||||
|
|
@ -106,6 +111,13 @@ public:
|
|||
return p_;
|
||||
}
|
||||
|
||||
/// Return unit-norm Vector
|
||||
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return (p_.vector ());
|
||||
}
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
Vector error(const Sphere2& q,
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
|
|
@ -129,11 +141,16 @@ public:
|
|||
return 2;
|
||||
}
|
||||
|
||||
enum CoordinatesMode {
|
||||
EXPMAP, ///< Use the exponential map to retract
|
||||
RENORM ///< Retract with vector addtion and renormalize.
|
||||
};
|
||||
|
||||
/// The retract function
|
||||
Sphere2 retract(const Vector& v) const;
|
||||
Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// The local coordinates function
|
||||
Vector localCoordinates(const Sphere2& s) const;
|
||||
Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -89,10 +89,7 @@ TEST (EssentialMatrix, rotate) {
|
|||
|
||||
// Derivatives
|
||||
Matrix actH1, actH2;
|
||||
try {
|
||||
bodyE.rotate(cRb, actH1, actH2);
|
||||
} catch (exception e) {
|
||||
} // avoid exception
|
||||
try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
|
||||
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
|
||||
expH2 = numericalDerivative22(rotate_, bodyE, cRb);
|
||||
EXPECT(assert_equal(expH1, actH1, 1e-8));
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@
|
|||
* @file testSphere2.cpp
|
||||
* @date Feb 03, 2012
|
||||
* @author Can Erdogan
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Trevor
|
||||
* @brief Tests the Sphere2 class
|
||||
*/
|
||||
|
||||
|
|
@ -76,10 +78,35 @@ TEST(Sphere2, rotate) {
|
|||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
|
||||
return R.unrotate (p);
|
||||
}
|
||||
|
||||
TEST(Sphere2, unrotate) {
|
||||
Rot3 R = Rot3::yaw(-M_PI/4.0);
|
||||
Sphere2 p(1, 0, 0);
|
||||
Sphere2 expected = Sphere2(1, 1, 0);
|
||||
Sphere2 actual = R.unrotate (p);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expectedH = numericalDerivative21(unrotate_, R, p);
|
||||
R.unrotate(p, actualH, boost::none);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-9));
|
||||
}
|
||||
{
|
||||
expectedH = numericalDerivative22(unrotate_, R, p);
|
||||
R.unrotate(p, boost::none, actualH);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, error) {
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
r = p.retract((Vector(2) << 0.8, 0));
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
|
||||
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
|
||||
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
|
||||
EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
|
||||
EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
|
||||
|
|
@ -102,8 +129,8 @@ TEST(Sphere2, error) {
|
|||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, distance) {
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
r = p.retract((Vector(2) << 0.8, 0));
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
|
||||
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
|
||||
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
|
||||
|
|
@ -147,9 +174,20 @@ TEST(Sphere2, retract) {
|
|||
Vector v(2);
|
||||
v << 0.5, 0;
|
||||
Sphere2 expected(Point3(1, 0, 0.5));
|
||||
Sphere2 actual = p.retract(v);
|
||||
Sphere2 actual = p.retract(v, Sphere2::RENORM);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, retract_expmap) {
|
||||
Sphere2 p;
|
||||
Vector v(2);
|
||||
v << (M_PI/2.0), 0;
|
||||
Sphere2 expected(Point3(0, 0, 1));
|
||||
Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
|
|
@ -199,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) {
|
|||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Let x and y be two Sphere2's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
TEST(Sphere2, localCoordinates_retract_expmap) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
|
||||
Vector_(3, 1.0, 1.0, 1.0);
|
||||
Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI);
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
|
||||
// Sleep for the random number generator (TODO?: Better create all of them first).
|
||||
sleep(0);
|
||||
|
||||
// Create the two Sphere2s.
|
||||
// Unlike the above case, we can use any two sphers.
|
||||
Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
|
||||
// Magnitude of the rotation can be at most pi
|
||||
if (v12.norm () > M_PI)
|
||||
v12 = v12 / M_PI;
|
||||
Sphere2 s2 = s1.retract(v12);
|
||||
|
||||
// Check if the local coordinates and retract return the same results.
|
||||
Vector actual_v12 = s1.localCoordinates(s2);
|
||||
EXPECT(assert_equal(v12, actual_v12, 1e-3));
|
||||
Sphere2 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
//TEST( Pose2, between )
|
||||
//{
|
||||
|
|
@ -245,13 +316,13 @@ TEST(Sphere2, Random) {
|
|||
// Check that is deterministic given same random seed
|
||||
Point3 expected(-0.667578, 0.671447, 0.321713);
|
||||
Point3 actual = Sphere2::Random(rng).point3();
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
EXPECT(assert_equal(expected,actual,1e-5));
|
||||
// Check that means are all zero at least
|
||||
Point3 expectedMean, actualMean;
|
||||
for (size_t i = 0; i < 100; i++)
|
||||
actualMean = actualMean + Sphere2::Random(rng).point3();
|
||||
actualMean = actualMean / 100;
|
||||
EXPECT(assert_equal(expectedMean, actualMean, 0.1));
|
||||
actualMean = actualMean/100;
|
||||
EXPECT(assert_equal(expectedMean,actualMean,0.1));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -17,23 +17,23 @@
|
|||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
#define TEST(TITLE,STATEMENT) \
|
||||
gttic_(TITLE); \
|
||||
for(int i = 0; i < n; i++) \
|
||||
STATEMENT; \
|
||||
gttoc_(TITLE);
|
||||
gttic_(TITLE); \
|
||||
for(int i = 0; i < n; i++) \
|
||||
STATEMENT; \
|
||||
gttoc_(TITLE);
|
||||
|
||||
int main()
|
||||
{
|
||||
int n = 5000000;
|
||||
cout << "NOTE: Times are reported for " << n << " calls" << endl;
|
||||
int n = 5000000;
|
||||
cout << "NOTE: Times are reported for " << n << " calls" << endl;
|
||||
|
||||
double norm=sqrt(1.0+16.0+4.0);
|
||||
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
|
||||
|
|
@ -47,9 +47,9 @@ int main()
|
|||
TEST(between, T.between(T2))
|
||||
TEST(between_derivatives, T.between(T2,H1,H2))
|
||||
TEST(Logmap, Pose3::Logmap(T.between(T2)))
|
||||
|
||||
// Print timings
|
||||
tictoc_print_();
|
||||
|
||||
// Print timings
|
||||
tictoc_print_();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue