Rename Sphere2 -> Unit3
dellaert
parent
83105f08a2
commit
926c27c732
18
gtsam.h
18
gtsam.h
|
|
@ -563,15 +563,15 @@ virtual class Pose3 : gtsam::Value {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
virtual class Sphere2 : gtsam::Value {
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
virtual class Unit3 : gtsam::Value {
|
||||
// Standard Constructors
|
||||
Sphere2();
|
||||
Sphere2(const gtsam::Point3& pose);
|
||||
Unit3();
|
||||
Unit3(const gtsam::Point3& pose);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::Sphere2& pose, double tol) const;
|
||||
bool equals(const gtsam::Unit3& pose, double tol) const;
|
||||
|
||||
// Other functionality
|
||||
Matrix basis() const;
|
||||
|
|
@ -580,14 +580,14 @@ virtual class Sphere2 : gtsam::Value {
|
|||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Sphere2 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Sphere2& s) const;
|
||||
gtsam::Unit3 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Unit3& s) const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
virtual class EssentialMatrix : gtsam::Value {
|
||||
// Standard Constructors
|
||||
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Sphere2& aTb);
|
||||
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
|
@ -601,7 +601,7 @@ virtual class EssentialMatrix : gtsam::Value {
|
|||
|
||||
// Other methods:
|
||||
gtsam::Rot3 rotation() const;
|
||||
gtsam::Sphere2 direction() const;
|
||||
gtsam::Unit3 direction() const;
|
||||
Matrix matrix() const;
|
||||
double error(Vector vA, Vector vB);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -19,14 +19,14 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
|
|||
const Point3& _1T2_ = _1P2_.translation();
|
||||
if (!H) {
|
||||
// just make a direction out of translation and create E
|
||||
Sphere2 direction(_1T2_);
|
||||
Unit3 direction(_1T2_);
|
||||
return EssentialMatrix(_1R2_, direction);
|
||||
} else {
|
||||
// Calculate the 5*6 Jacobian H = D_E_1P2
|
||||
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
|
||||
// First get 2*3 derivative from Sphere2::FromPoint3
|
||||
// First get 2*3 derivative from Unit3::FromPoint3
|
||||
Matrix D_direction_1T2;
|
||||
Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2);
|
||||
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
|
||||
H->resize(5, 6);
|
||||
H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
|
||||
H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
|
||||
|
|
@ -49,7 +49,7 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
|
|||
Vector3 omega(sub(xi, 0, 3));
|
||||
Vector2 z(sub(xi, 3, 5));
|
||||
Rot3 R = aRb_.retract(omega);
|
||||
Sphere2 t = aTb_.retract(z);
|
||||
Unit3 t = aTb_.retract(z);
|
||||
return EssentialMatrix(R, t);
|
||||
}
|
||||
|
||||
|
|
@ -85,12 +85,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
|
|||
|
||||
if (!HE && !HR) {
|
||||
// Rotate translation direction and return
|
||||
Sphere2 c1Tc2 = cRb * aTb_;
|
||||
Unit3 c1Tc2 = cRb * aTb_;
|
||||
return EssentialMatrix(c1Rc2, c1Tc2);
|
||||
} else {
|
||||
// Calculate derivatives
|
||||
Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
|
||||
Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
|
||||
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
|
||||
if (HE) {
|
||||
*HE = zeros(5, 5);
|
||||
HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
|
||||
|
|
@ -126,7 +126,7 @@ double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
|
|||
/* ************************************************************************* */
|
||||
ostream& operator <<(ostream& os, const EssentialMatrix& E) {
|
||||
Rot3 R = E.rotation();
|
||||
Sphere2 d = E.direction();
|
||||
Unit3 d = E.direction();
|
||||
os.precision(10);
|
||||
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
|
||||
return os;
|
||||
|
|
@ -140,7 +140,7 @@ istream& operator >>(istream& is, EssentialMatrix& E) {
|
|||
|
||||
// Create EssentialMatrix from rotation and translation
|
||||
Rot3 rot = Rot3::RzRyRx(rx, ry, rz);
|
||||
Sphere2 dt = Sphere2(dx, dy, dz);
|
||||
Unit3 dt = Unit3(dx, dy, dz);
|
||||
E = EssentialMatrix(rot, dt);
|
||||
|
||||
return is;
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -17,7 +17,7 @@ namespace gtsam {
|
|||
/**
|
||||
* An essential matrix is like a Pose3, except with translation up to scale
|
||||
* It is named after the 3*3 matrix aEb = [aTb]x aRb from computer vision,
|
||||
* but here we choose instead to parameterize it as a (Rot3,Sphere2) pair.
|
||||
* but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
|
||||
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
|
||||
*/
|
||||
class EssentialMatrix: public DerivedValue<EssentialMatrix> {
|
||||
|
|
@ -25,7 +25,7 @@ class EssentialMatrix: public DerivedValue<EssentialMatrix> {
|
|||
private:
|
||||
|
||||
Rot3 aRb_; ///< Rotation between a and b
|
||||
Sphere2 aTb_; ///< translation direction from a to b
|
||||
Unit3 aTb_; ///< translation direction from a to b
|
||||
Matrix3 E_; ///< Essential matrix
|
||||
|
||||
public:
|
||||
|
|
@ -44,7 +44,7 @@ public:
|
|||
}
|
||||
|
||||
/// Construct from rotation and translation
|
||||
EssentialMatrix(const Rot3& aRb, const Sphere2& aTb) :
|
||||
EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
|
||||
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
|
||||
}
|
||||
|
||||
|
|
@ -52,10 +52,10 @@ public:
|
|||
static EssentialMatrix FromPose3(const Pose3& _1P2_,
|
||||
boost::optional<Matrix&> H = boost::none);
|
||||
|
||||
/// Random, using Rot3::Random and Sphere2::Random
|
||||
/// Random, using Rot3::Random and Unit3::Random
|
||||
template<typename Engine>
|
||||
static EssentialMatrix Random(Engine & rng) {
|
||||
return EssentialMatrix(Rot3::Random(rng), Sphere2::Random(rng));
|
||||
return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -103,7 +103,7 @@ public:
|
|||
}
|
||||
|
||||
/// Direction
|
||||
inline const Sphere2& direction() const {
|
||||
inline const Unit3& direction() const {
|
||||
return aTb_;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -40,14 +40,14 @@ Rot3 Rot3::rodriguez(const Point3& w, double theta) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
|
||||
Rot3 Rot3::rodriguez(const Unit3& 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);
|
||||
Unit3 w = Unit3::Random(rng);
|
||||
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
|
||||
double angle = randomAngle(rng);
|
||||
return rodriguez(w,angle);
|
||||
|
|
@ -71,9 +71,9 @@ Point3 Rot3::operator*(const Point3& p) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::rotate(const Sphere2& p,
|
||||
Unit3 Rot3::rotate(const Unit3& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = rotate(p.point3(Hp));
|
||||
Unit3 q = rotate(p.point3(Hp));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
|
||||
if (HR)
|
||||
|
|
@ -82,9 +82,9 @@ Sphere2 Rot3::rotate(const Sphere2& p,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::unrotate(const Sphere2& p,
|
||||
Unit3 Rot3::unrotate(const Unit3& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = unrotate(p.point3(Hp));
|
||||
Unit3 q = unrotate(p.point3(Hp));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
|
||||
if (HR)
|
||||
|
|
@ -93,7 +93,7 @@ Sphere2 Rot3::unrotate(const Sphere2& p,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::operator*(const Sphere2& p) const {
|
||||
Unit3 Rot3::operator*(const Unit3& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@
|
|||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -177,7 +177,7 @@ namespace gtsam {
|
|||
* @param theta rotation angle
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
static Rot3 rodriguez(const Sphere2& w, double theta);
|
||||
static Rot3 rodriguez(const Unit3& w, double theta);
|
||||
|
||||
/**
|
||||
* Rodriguez' formula to compute an incremental rotation matrix
|
||||
|
|
@ -324,19 +324,19 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Sphere2
|
||||
/// @name Group Action on Unit3
|
||||
/// @{
|
||||
|
||||
/// rotate 3D direction from rotated coordinate frame to world frame
|
||||
Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
|
||||
Unit3 rotate(const Unit3& 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,
|
||||
Unit3 unrotate(const Unit3& 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;
|
||||
Unit3 operator*(const Unit3& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
|
|||
|
|
@ -10,15 +10,15 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file Sphere2.h
|
||||
* @file Unit3.h
|
||||
* @date Feb 02, 2011
|
||||
* @author Can Erdogan
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Trevor
|
||||
* @brief The Sphere2 class - basically a point on a unit sphere
|
||||
* @brief The Unit3 class - basically a point on a unit sphere
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/random/uniform_on_sphere.hpp>
|
||||
|
|
@ -29,8 +29,8 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
|
||||
Sphere2 direction(point);
|
||||
Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
|
||||
Unit3 direction(point);
|
||||
if (H) {
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix D_p_point;
|
||||
|
|
@ -43,17 +43,17 @@ Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
|
||||
Unit3 Unit3::Random(boost::random::mt19937 & rng) {
|
||||
// TODO allow any engine without including all of boost :-(
|
||||
boost::random::uniform_on_sphere<double> randomDirection(3);
|
||||
vector<double> d = randomDirection(rng);
|
||||
Sphere2 result;
|
||||
Unit3 result;
|
||||
result.p_ = Point3(d[0], d[1], d[2]);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Sphere2::basis() const {
|
||||
Matrix Unit3::basis() const {
|
||||
|
||||
// Return cached version if exists
|
||||
if (B_.rows() == 3)
|
||||
|
|
@ -85,17 +85,17 @@ Matrix Sphere2::basis() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
/// The print fuction
|
||||
void Sphere2::print(const std::string& s) const {
|
||||
void Unit3::print(const std::string& s) const {
|
||||
cout << s << ":" << p_ << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Sphere2::skew() const {
|
||||
Matrix Unit3::skew() const {
|
||||
return skewSymmetric(p_.x(), p_.y(), p_.z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
|
||||
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
|
||||
Matrix Bt = basis().transpose();
|
||||
Vector xi = Bt * q.p_.vector();
|
||||
if (H)
|
||||
|
|
@ -104,7 +104,7 @@ Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
|
||||
double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
|
||||
Vector xi = error(q, H);
|
||||
double theta = xi.norm();
|
||||
if (H)
|
||||
|
|
@ -113,7 +113,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::retract(const Vector& v) const {
|
||||
Unit3 Unit3::retract(const Vector& v) const {
|
||||
|
||||
// Get the vector form of the point and the basis matrix
|
||||
Vector p = Point3::Logmap(p_);
|
||||
|
|
@ -127,19 +127,19 @@ Sphere2 Sphere2::retract(const Vector& v) const {
|
|||
// Avoid nan
|
||||
if (xi_hat_norm == 0.0) {
|
||||
if (v.norm() == 0.0)
|
||||
return Sphere2(point3());
|
||||
return Unit3(point3());
|
||||
else
|
||||
return Sphere2(-point3());
|
||||
return Unit3(-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);
|
||||
return Unit3(exp_p_xi_hat);
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sphere2::localCoordinates(const Sphere2& y) const {
|
||||
Vector Unit3::localCoordinates(const Unit3& y) const {
|
||||
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
|
|
@ -10,12 +10,12 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file Sphere2.h
|
||||
* @file Unit3.h
|
||||
* @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
|
||||
* @brief Develop a Unit3 class - basically a point on a unit sphere
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -38,7 +38,7 @@ typedef mersenne_twister_engine<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11,
|
|||
namespace gtsam {
|
||||
|
||||
/// Represents a 3D point on a unit sphere.
|
||||
class Sphere2: public DerivedValue<Sphere2> {
|
||||
class Unit3: public DerivedValue<Unit3> {
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -51,27 +51,27 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
Sphere2() :
|
||||
Unit3() :
|
||||
p_(1.0, 0.0, 0.0) {
|
||||
}
|
||||
|
||||
/// Construct from point
|
||||
Sphere2(const Point3& p) :
|
||||
Unit3(const Point3& p) :
|
||||
p_(p / p.norm()) {
|
||||
}
|
||||
|
||||
/// Construct from x,y,z
|
||||
Sphere2(double x, double y, double z) :
|
||||
Unit3(double x, double y, double z) :
|
||||
p_(x, y, z) {
|
||||
p_ = p_ / p_.norm();
|
||||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
static Sphere2 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
|
||||
static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
|
||||
boost::none);
|
||||
|
||||
/// Random direction, using boost::uniform_on_sphere
|
||||
static Sphere2 Random(boost::random::mt19937 & rng);
|
||||
static Unit3 Random(boost::random::mt19937 & rng);
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -82,7 +82,7 @@ public:
|
|||
void print(const std::string& s = std::string()) const;
|
||||
|
||||
/// The equals function with tolerance
|
||||
bool equals(const Sphere2& s, double tol = 1e-9) const {
|
||||
bool equals(const Unit3& s, double tol = 1e-9) const {
|
||||
return p_.equals(s.p_, tol);
|
||||
}
|
||||
/// @}
|
||||
|
|
@ -108,16 +108,16 @@ public:
|
|||
}
|
||||
|
||||
/// Return scaled direction as Point3
|
||||
friend Point3 operator*(double s, const Sphere2& d) {
|
||||
friend Point3 operator*(double s, const Unit3& d) {
|
||||
return s * d.p_;
|
||||
}
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
Vector error(const Sphere2& q,
|
||||
Vector error(const Unit3& q,
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
|
||||
/// Distance between two directions
|
||||
double distance(const Sphere2& q,
|
||||
double distance(const Unit3& q,
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
|
||||
/// @}
|
||||
|
|
@ -141,10 +141,10 @@ public:
|
|||
};
|
||||
|
||||
/// The retract function
|
||||
Sphere2 retract(const Vector& v) const;
|
||||
Unit3 retract(const Vector& v) const;
|
||||
|
||||
/// The local coordinates function
|
||||
Vector localCoordinates(const Sphere2& s) const;
|
||||
Vector localCoordinates(const Unit3& s) const;
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
|
@ -70,7 +70,7 @@ TEST (EssentialMatrix, retract1) {
|
|||
//*************************************************************************
|
||||
TEST (EssentialMatrix, retract2) {
|
||||
EssentialMatrix expected(c1Rc2,
|
||||
Sphere2(c1Tc2).retract((Vector(2) << 0.1, 0)));
|
||||
Unit3(c1Tc2).retract((Vector(2) << 0.1, 0)));
|
||||
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
|
@ -85,7 +85,7 @@ TEST (EssentialMatrix, transform_to) {
|
|||
* Rot3::roll(M_PI / 6.0);
|
||||
Point3 aTb2(19.2, 3.7, 5.9);
|
||||
EssentialMatrix E(aRb2, aTb2);
|
||||
//EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
|
||||
//EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0)));
|
||||
static Point3 P(0.2, 0.7, -2);
|
||||
Matrix actH1, actH2;
|
||||
E.transform_to(P, actH1, actH2);
|
||||
|
|
|
|||
|
|
@ -10,15 +10,15 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testSphere2.cpp
|
||||
* @file testUnit3.cpp
|
||||
* @date Feb 03, 2012
|
||||
* @author Can Erdogan
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Trevor
|
||||
* @brief Tests the Sphere2 class
|
||||
* @brief Tests the Unit3 class
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
|
@ -33,36 +33,36 @@ using namespace boost::assign;
|
|||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Sphere2)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Sphere2)
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Unit3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
|
||||
|
||||
//*******************************************************************************
|
||||
Point3 point3_(const Sphere2& p) {
|
||||
Point3 point3_(const Unit3& p) {
|
||||
return p.point3();
|
||||
}
|
||||
TEST(Sphere2, point3) {
|
||||
TEST(Unit3, point3) {
|
||||
vector<Point3> ps;
|
||||
ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
|
||||
/ sqrt(2);
|
||||
Matrix actualH, expectedH;
|
||||
BOOST_FOREACH(Point3 p,ps) {
|
||||
Sphere2 s(p);
|
||||
expectedH = numericalDerivative11<Point3, Sphere2>(point3_, s);
|
||||
Unit3 s(p);
|
||||
expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
|
||||
EXPECT(assert_equal(p, s.point3(actualH), 1e-8));
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
static Sphere2 rotate_(const Rot3& R, const Sphere2& p) {
|
||||
static Unit3 rotate_(const Rot3& R, const Unit3& p) {
|
||||
return R * p;
|
||||
}
|
||||
|
||||
TEST(Sphere2, rotate) {
|
||||
TEST(Unit3, rotate) {
|
||||
Rot3 R = Rot3::yaw(0.5);
|
||||
Sphere2 p(1, 0, 0);
|
||||
Sphere2 expected = Sphere2(R.column(1));
|
||||
Sphere2 actual = R * p;
|
||||
Unit3 p(1, 0, 0);
|
||||
Unit3 expected = Unit3(R.column(1));
|
||||
Unit3 actual = R * p;
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
|
|
@ -79,15 +79,15 @@ TEST(Sphere2, rotate) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
|
||||
static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
|
||||
return R.unrotate(p);
|
||||
}
|
||||
|
||||
TEST(Sphere2, unrotate) {
|
||||
TEST(Unit3, 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);
|
||||
Unit3 p(1, 0, 0);
|
||||
Unit3 expected = Unit3(1, 1, 0);
|
||||
Unit3 actual = R.unrotate(p);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
|
|
@ -104,8 +104,8 @@ TEST(Sphere2, unrotate) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, error) {
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
TEST(Unit3, error) {
|
||||
Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
r = p.retract((Vector(2) << 0.8, 0));
|
||||
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
|
||||
EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5));
|
||||
|
|
@ -114,22 +114,22 @@ TEST(Sphere2, error) {
|
|||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expected = numericalDerivative11<Sphere2>(
|
||||
boost::bind(&Sphere2::error, &p, _1, boost::none), q);
|
||||
expected = numericalDerivative11<Unit3>(
|
||||
boost::bind(&Unit3::error, &p, _1, boost::none), q);
|
||||
p.error(q, actual);
|
||||
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative11<Sphere2>(
|
||||
boost::bind(&Sphere2::error, &p, _1, boost::none), r);
|
||||
expected = numericalDerivative11<Unit3>(
|
||||
boost::bind(&Unit3::error, &p, _1, boost::none), r);
|
||||
p.error(r, actual);
|
||||
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, distance) {
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
TEST(Unit3, distance) {
|
||||
Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
r = p.retract((Vector(2) << 0.8, 0));
|
||||
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
|
||||
|
|
@ -138,44 +138,44 @@ TEST(Sphere2, distance) {
|
|||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expected = numericalGradient<Sphere2>(
|
||||
boost::bind(&Sphere2::distance, &p, _1, boost::none), q);
|
||||
expected = numericalGradient<Unit3>(
|
||||
boost::bind(&Unit3::distance, &p, _1, boost::none), q);
|
||||
p.distance(q, actual);
|
||||
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalGradient<Sphere2>(
|
||||
boost::bind(&Sphere2::distance, &p, _1, boost::none), r);
|
||||
expected = numericalGradient<Unit3>(
|
||||
boost::bind(&Unit3::distance, &p, _1, boost::none), r);
|
||||
p.distance(r, actual);
|
||||
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, localCoordinates0) {
|
||||
Sphere2 p;
|
||||
TEST(Unit3, localCoordinates0) {
|
||||
Unit3 p;
|
||||
Vector actual = p.localCoordinates(p);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, localCoordinates1) {
|
||||
Sphere2 p, q(1, 6.12385e-21, 0);
|
||||
TEST(Unit3, localCoordinates1) {
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, localCoordinates2) {
|
||||
Sphere2 p, q(-1, 0, 0);
|
||||
TEST(Unit3, localCoordinates2) {
|
||||
Unit3 p, q(-1, 0, 0);
|
||||
Vector expected = (Vector(2) << M_PI, 0);
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, basis) {
|
||||
Sphere2 p;
|
||||
TEST(Unit3, basis) {
|
||||
Unit3 p;
|
||||
Matrix expected(3, 2);
|
||||
expected << 0, 0, 0, -1, 1, 0;
|
||||
Matrix actual = p.basis();
|
||||
|
|
@ -183,23 +183,23 @@ TEST(Sphere2, basis) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, retract) {
|
||||
Sphere2 p;
|
||||
TEST(Unit3, retract) {
|
||||
Unit3 p;
|
||||
Vector v(2);
|
||||
v << 0.5, 0;
|
||||
Sphere2 expected(0.877583, 0, 0.479426);
|
||||
Sphere2 actual = p.retract(v);
|
||||
Unit3 expected(0.877583, 0, 0.479426);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, retract_expmap) {
|
||||
Sphere2 p;
|
||||
TEST(Unit3, retract_expmap) {
|
||||
Unit3 p;
|
||||
Vector v(2);
|
||||
v << (M_PI / 2.0), 0;
|
||||
Sphere2 expected(Point3(0, 0, 1));
|
||||
Sphere2 actual = p.retract(v);
|
||||
Unit3 expected(Point3(0, 0, 1));
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
|
|
@ -222,9 +222,9 @@ inline static Vector randomVector(const Vector& minLimits,
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Let x and y be two Sphere2's.
|
||||
// Let x and y be two Unit3's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
TEST(Sphere2, localCoordinates_retract) {
|
||||
TEST(Unit3, localCoordinates_retract) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
|
||||
|
|
@ -235,26 +235,26 @@ TEST(Sphere2, localCoordinates_retract) {
|
|||
// Sleep for the random number generator (TODO?: Better create all of them first).
|
||||
sleep(0);
|
||||
|
||||
// Create the two Sphere2s.
|
||||
// NOTE: You can not create two totally random Sphere2's because you cannot always compute
|
||||
// between two any Sphere2's. (For instance, they might be at the different sides of the circle).
|
||||
Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Create the two Unit3s.
|
||||
// NOTE: You can not create two totally random Unit3's because you cannot always compute
|
||||
// between two any Unit3's. (For instance, they might be at the different sides of the circle).
|
||||
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
Sphere2 s2 = s1.retract(v12);
|
||||
Unit3 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);
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Let x and y be two Sphere2's.
|
||||
// Let x and y be two Unit3's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
TEST(Sphere2, localCoordinates_retract_expmap) {
|
||||
TEST(Unit3, localCoordinates_retract_expmap) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
|
||||
|
|
@ -266,21 +266,21 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
|
|||
// Sleep for the random number generator (TODO?: Better create all of them first).
|
||||
sleep(0);
|
||||
|
||||
// Create the two Sphere2s.
|
||||
// Create the two Unit3s.
|
||||
// Unlike the above case, we can use any two sphers.
|
||||
Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Unit3 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);
|
||||
Unit3 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);
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
}
|
||||
}
|
||||
|
|
@ -326,28 +326,28 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
|
|||
//}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, Random) {
|
||||
TEST(Unit3, Random) {
|
||||
boost::random::mt19937 rng(42);
|
||||
// Check that is deterministic given same random seed
|
||||
Point3 expected(-0.667578, 0.671447, 0.321713);
|
||||
Point3 actual = Sphere2::Random(rng).point3();
|
||||
Point3 actual = Unit3::Random(rng).point3();
|
||||
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 + Unit3::Random(rng).point3();
|
||||
actualMean = actualMean / 100;
|
||||
EXPECT(assert_equal(expectedMean,actualMean,0.1));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Sphere2, FromPoint3) {
|
||||
TEST (Unit3, FromPoint3) {
|
||||
Matrix actualH;
|
||||
Point3 point(1, -2, 3); // arbitrary point
|
||||
Sphere2 expected(point);
|
||||
EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8));
|
||||
Matrix expectedH = numericalDerivative11<Sphere2, Point3>(
|
||||
boost::bind(Sphere2::FromPoint3, _1, boost::none), point);
|
||||
Unit3 expected(point);
|
||||
EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8));
|
||||
Matrix expectedH = numericalDerivative11<Unit3, Point3>(
|
||||
boost::bind(Unit3::FromPoint3, _1, boost::none), point);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -27,13 +27,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb,
|
|||
boost::optional<Matrix&> H) const {
|
||||
if (H) {
|
||||
Matrix D_nRef_R, D_e_nRef;
|
||||
Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R);
|
||||
Unit3 nRef = nRb.rotate(bRef_, D_nRef_R);
|
||||
Vector e = nZ_.error(nRef, D_e_nRef);
|
||||
H->resize(2, 3);
|
||||
H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R;
|
||||
return e;
|
||||
} else {
|
||||
Sphere2 nRef = nRb * bRef_;
|
||||
Unit3 nRef = nRb * bRef_;
|
||||
return nZ_.error(nRef);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -35,7 +35,7 @@ class AttitudeFactor {
|
|||
|
||||
protected:
|
||||
|
||||
const Sphere2 nZ_, bRef_; ///< Position measurement in
|
||||
const Unit3 nZ_, bRef_; ///< Position measurement in
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -48,7 +48,7 @@ public:
|
|||
* @param nZ measured direction in navigation frame
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
AttitudeFactor(const Sphere2& nZ, const Sphere2& bRef = Sphere2(0, 0, 1)) :
|
||||
AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
nZ_(nZ), bRef_(bRef) {
|
||||
}
|
||||
|
||||
|
|
@ -87,8 +87,8 @@ public:
|
|||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Rot3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
|
||||
const Sphere2& bRef = Sphere2(0, 0, 1)) :
|
||||
Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), AttitudeFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
|
|
@ -156,8 +156,8 @@ public:
|
|||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
|
||||
const Sphere2& bRef = Sphere2(0, 0, 1)) :
|
||||
Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), AttitudeFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ public:
|
|||
|
||||
/** Constructor */
|
||||
MagFactor(Key key, const Point3& measured, double scale,
|
||||
const Sphere2& direction, const Point3& bias,
|
||||
const Unit3& direction, const Point3& bias,
|
||||
const SharedNoiseModel& model) :
|
||||
NoiseModelFactor1<Rot2>(model, key), //
|
||||
measured_(measured), nM_(scale * direction), bias_(bias) {
|
||||
|
|
@ -85,7 +85,7 @@ public:
|
|||
|
||||
/** Constructor */
|
||||
MagFactor1(Key key, const Point3& measured, double scale,
|
||||
const Sphere2& direction, const Point3& bias,
|
||||
const Unit3& direction, const Point3& bias,
|
||||
const SharedNoiseModel& model) :
|
||||
NoiseModelFactor1<Rot3>(model, key), //
|
||||
measured_(measured), nM_(scale * direction), bias_(bias) {
|
||||
|
|
@ -154,7 +154,7 @@ public:
|
|||
* This version uses model measured bM = scale * bRn * direction + bias
|
||||
* and optimizes for both scale, direction, and the bias.
|
||||
*/
|
||||
class MagFactor3: public NoiseModelFactor3<LieScalar, Sphere2, Point3> {
|
||||
class MagFactor3: public NoiseModelFactor3<LieScalar, Unit3, Point3> {
|
||||
|
||||
const Point3 measured_; ///< The measured magnetometer values
|
||||
const Rot3 bRn_; ///< The assumed known rotation from nav to body
|
||||
|
|
@ -164,7 +164,7 @@ public:
|
|||
/** Constructor */
|
||||
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
|
||||
const Rot3& nRb, const SharedNoiseModel& model) :
|
||||
NoiseModelFactor3<LieScalar, Sphere2, Point3>(model, key1, key2, key3), //
|
||||
NoiseModelFactor3<LieScalar, Unit3, Point3>(model, key1, key2, key3), //
|
||||
measured_(measured), bRn_(nRb.inverse()) {
|
||||
}
|
||||
|
||||
|
|
@ -179,12 +179,12 @@ public:
|
|||
* @param nM (unknown) local earth magnetic field vector, in nav frame
|
||||
* @param bias (unknown) 3D bias
|
||||
*/
|
||||
Vector evaluateError(const LieScalar& scale, const Sphere2& direction,
|
||||
Vector evaluateError(const LieScalar& scale, const Unit3& direction,
|
||||
const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
|
||||
boost::none) const {
|
||||
// measured bM = nRbÕ * nM + b, where b is unknown bias
|
||||
Sphere2 rotated = bRn_.rotate(direction, boost::none, H2);
|
||||
Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
|
||||
Point3 hx = scale * rotated.point3() + bias;
|
||||
if (H1)
|
||||
*H1 = rotated.point3().vector();
|
||||
|
|
|
|||
|
|
@ -30,8 +30,8 @@ TEST( Rot3AttitudeFactor, Constructor ) {
|
|||
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
|
||||
// If pitch and roll are zero for an aerospace frame,
|
||||
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
|
||||
Sphere2 bZ(0, 0, 1); // reference direction is body Z axis
|
||||
Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
|
||||
Unit3 bZ(0, 0, 1); // reference direction is body Z axis
|
||||
Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
|
|
@ -63,8 +63,8 @@ TEST( Pose3AttitudeFactor, Constructor ) {
|
|||
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
|
||||
// If pitch and roll are zero for an aerospace frame,
|
||||
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
|
||||
Sphere2 bZ(0, 0, 1); // reference direction is body Z axis
|
||||
Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
|
||||
Unit3 bZ(0, 0, 1); // reference direction is body Z axis
|
||||
Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ Point3 scaled = scale * nM;
|
|||
Point3 measured = nRb.inverse() * (scale * nM) + bias;
|
||||
|
||||
LieScalar s(scale * nM.norm());
|
||||
Sphere2 dir(nM);
|
||||
Unit3 dir(nM);
|
||||
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
|
||||
|
|
@ -97,7 +97,7 @@ TEST( MagFactor, Factors ) {
|
|||
EXPECT(assert_equal(numericalDerivative11<LieScalar> //
|
||||
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
|
||||
H1, 1e-7));
|
||||
EXPECT(assert_equal(numericalDerivative11<Sphere2> //
|
||||
EXPECT(assert_equal(numericalDerivative11<Unit3> //
|
||||
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
|
||||
H2, 1e-7));
|
||||
EXPECT(assert_equal(numericalDerivative11<Point3> //
|
||||
|
|
|
|||
|
|
@ -66,7 +66,7 @@ public:
|
|||
*/
|
||||
class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
|
||||
|
||||
Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
|
||||
Unit3 p_, z_; ///< Predicted and measured directions, p = iRc * z
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> Base;
|
||||
typedef RotateDirectionsFactor This;
|
||||
|
|
@ -74,7 +74,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
|
|||
public:
|
||||
|
||||
/// Constructor
|
||||
RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z,
|
||||
RotateDirectionsFactor(Key key, const Unit3& p, const Unit3& z,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, key), p_(p), z_(z) {
|
||||
}
|
||||
|
|
@ -96,7 +96,7 @@ public:
|
|||
/// vector of errors returns 2D vector
|
||||
Vector evaluateError(const Rot3& R,
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
Sphere2 q = R * z_;
|
||||
Unit3 q = R * z_;
|
||||
Vector e = p_.error(q, H);
|
||||
if (H) {
|
||||
Matrix DR;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ TEST( EssentialMatrixConstraint, test ) {
|
|||
Key poseKey2(2);
|
||||
Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20);
|
||||
Point3 trueTranslation(+0.5, -1.0, +1.0);
|
||||
Sphere2 trueDirection(trueTranslation);
|
||||
Unit3 trueDirection(trueTranslation);
|
||||
EssentialMatrix measurement(trueRotation, trueDirection);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25);
|
||||
EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model);
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
|
|||
Rot3 iRc(cameraX, cameraY, cameraZ);
|
||||
|
||||
// Now, let's create some rotations around IMU frame
|
||||
Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
|
||||
Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
|
||||
Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), //
|
||||
i2Ri3 = Rot3::rodriguez(p2, 1), //
|
||||
i3Ri4 = Rot3::rodriguez(p3, 1);
|
||||
|
|
@ -39,7 +39,7 @@ c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
|
|||
c3Zc4 = iRc.inverse() * i3Ri4 * iRc;
|
||||
|
||||
// The corresponding rotated directions in the camera frame
|
||||
Sphere2 z1 = iRc.inverse() * p1, //
|
||||
Unit3 z1 = iRc.inverse() * p1, //
|
||||
z2 = iRc.inverse() * p2, //
|
||||
z3 = iRc.inverse() * p3;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue