merge in changes in develop branch recently
jing
commit
ca0a355498
|
|
@ -1,2 +1,3 @@
|
|||
/build*
|
||||
*.pyc
|
||||
*.DS_Store
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
/*
|
||||
* @file EssentialMatrix.cpp
|
||||
* @brief EssentialMatrix class
|
||||
* @author Frank Dellaert
|
||||
* @date December 5, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
|
||||
boost::optional<Matrix&> H) {
|
||||
const Rot3& _1R2_ = _1P2_.rotation();
|
||||
const Point3& _1T2_ = _1P2_.translation();
|
||||
if (!H) {
|
||||
// just make a direction out of translation and create E
|
||||
Sphere2 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
|
||||
Matrix D_direction_1T2;
|
||||
Sphere2 direction = Sphere2::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
|
||||
H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
|
||||
H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
|
||||
return EssentialMatrix(_1R2_, direction);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void EssentialMatrix::print(const string& s) const {
|
||||
cout << s;
|
||||
aRb_.print("R:\n");
|
||||
aTb_.print("d: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
|
||||
assert(xi.size() == 5);
|
||||
Vector3 omega(sub(xi, 0, 3));
|
||||
Vector2 z(sub(xi, 3, 5));
|
||||
Rot3 R = aRb_.retract(omega);
|
||||
Sphere2 t = aTb_.retract(z);
|
||||
return EssentialMatrix(R, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
|
||||
return Vector(5) << //
|
||||
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 EssentialMatrix::transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
|
||||
Pose3 pose(aRb_, aTb_.point3());
|
||||
Point3 q = pose.transform_to(p, DE, Dpoint);
|
||||
if (DE) {
|
||||
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
|
||||
// The last 3 columns are derivative with respect to change in translation
|
||||
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
|
||||
// Duy made an educated guess that this needs to be rotated to the local frame
|
||||
Matrix H(3, 5);
|
||||
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
|
||||
*DE = H;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
|
||||
boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
|
||||
|
||||
// The rotation must be conjugated to act in the camera frame
|
||||
Rot3 c1Rc2 = aRb_.conjugate(cRb);
|
||||
|
||||
if (!HE && !HR) {
|
||||
// Rotate translation direction and return
|
||||
Sphere2 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);
|
||||
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
|
||||
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
|
||||
}
|
||||
if (HR) {
|
||||
throw runtime_error(
|
||||
"EssentialMatrix::rotate: derivative HR not implemented yet");
|
||||
/*
|
||||
HR->resize(5, 3);
|
||||
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
|
||||
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
|
||||
*/
|
||||
}
|
||||
return EssentialMatrix(c1Rc2, c1Tc2);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
|
||||
boost::optional<Matrix&> H) const {
|
||||
if (H) {
|
||||
H->resize(1, 5);
|
||||
// See math.lyx
|
||||
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
|
||||
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
|
||||
* aTb_.basis();
|
||||
*H << HR, HD;
|
||||
}
|
||||
return dot(vA, E_ * vB);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream& operator <<(ostream& os, const EssentialMatrix& E) {
|
||||
Rot3 R = E.rotation();
|
||||
Sphere2 d = E.direction();
|
||||
os.precision(10);
|
||||
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
istream& operator >>(istream& is, EssentialMatrix& E) {
|
||||
double rx, ry, rz, dx, dy, dz;
|
||||
is >> rx >> ry >> rz; // Read the rotation rxyz
|
||||
is >> dx >> dy >> dz; // Read the translation dxyz
|
||||
|
||||
// Create EssentialMatrix from rotation and translation
|
||||
Rot3 rot = Rot3::RzRyRx(rx, ry, rz);
|
||||
Sphere2 dt = Sphere2(dx, dy, dz);
|
||||
E = EssentialMatrix(rot, dt);
|
||||
|
||||
return is;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // gtsam
|
||||
|
||||
|
|
@ -26,7 +26,7 @@ private:
|
|||
|
||||
Rot3 aRb_; ///< Rotation between a and b
|
||||
Sphere2 aTb_; ///< translation direction from a to b
|
||||
Matrix E_; ///< Essential matrix
|
||||
Matrix3 E_; ///< Essential matrix
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -48,6 +48,10 @@ public:
|
|||
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
|
||||
}
|
||||
|
||||
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
|
||||
static EssentialMatrix FromPose3(const Pose3& _1P2_,
|
||||
boost::optional<Matrix&> H = boost::none);
|
||||
|
||||
/// Random, using Rot3::Random and Sphere2::Random
|
||||
template<typename Engine>
|
||||
static EssentialMatrix Random(Engine & rng) {
|
||||
|
|
@ -60,11 +64,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << s;
|
||||
aRb_.print("R:\n");
|
||||
aTb_.print("d: ");
|
||||
}
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
|
||||
|
|
@ -87,20 +87,10 @@ public:
|
|||
}
|
||||
|
||||
/// Retract delta to manifold
|
||||
virtual EssentialMatrix retract(const Vector& xi) const {
|
||||
assert(xi.size() == 5);
|
||||
Vector3 omega(sub(xi, 0, 3));
|
||||
Vector2 z(sub(xi, 3, 5));
|
||||
Rot3 R = aRb_.retract(omega);
|
||||
Sphere2 t = aTb_.retract(z);
|
||||
return EssentialMatrix(R, t);
|
||||
}
|
||||
virtual EssentialMatrix retract(const Vector& xi) const;
|
||||
|
||||
/// Compute the coordinates in the tangent space
|
||||
virtual Vector localCoordinates(const EssentialMatrix& other) const {
|
||||
return Vector(5) << //
|
||||
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
|
||||
}
|
||||
virtual Vector localCoordinates(const EssentialMatrix& other) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -108,17 +98,17 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Rotation
|
||||
const Rot3& rotation() const {
|
||||
inline const Rot3& rotation() const {
|
||||
return aRb_;
|
||||
}
|
||||
|
||||
/// Direction
|
||||
const Sphere2& direction() const {
|
||||
inline const Sphere2& direction() const {
|
||||
return aTb_;
|
||||
}
|
||||
|
||||
/// Return 3*3 matrix representation
|
||||
const Matrix& matrix() const {
|
||||
inline const Matrix3& matrix() const {
|
||||
return E_;
|
||||
}
|
||||
|
||||
|
|
@ -131,20 +121,7 @@ public:
|
|||
*/
|
||||
Point3 transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> DE = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none) const {
|
||||
Pose3 pose(aRb_, aTb_.point3());
|
||||
Point3 q = pose.transform_to(p, DE, Dpoint);
|
||||
if (DE) {
|
||||
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
|
||||
// The last 3 columns are derivative with respect to change in translation
|
||||
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
|
||||
// Duy made an educated guess that this needs to be rotated to the local frame
|
||||
Matrix H(3, 5);
|
||||
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
|
||||
*DE = H;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
boost::optional<Matrix&> Dpoint = boost::none) const;
|
||||
|
||||
/**
|
||||
* Given essential matrix E in camera frame B, convert to body frame C
|
||||
|
|
@ -152,36 +129,7 @@ public:
|
|||
* @param E essential matrix E in camera frame C
|
||||
*/
|
||||
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
|
||||
boost::none, boost::optional<Matrix&> HR = boost::none) const {
|
||||
|
||||
// The rotation must be conjugated to act in the camera frame
|
||||
Rot3 c1Rc2 = aRb_.conjugate(cRb);
|
||||
|
||||
if (!HE && !HR) {
|
||||
// Rotate translation direction and return
|
||||
Sphere2 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);
|
||||
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
|
||||
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
|
||||
}
|
||||
if (HR) {
|
||||
throw std::runtime_error(
|
||||
"EssentialMatrix::rotate: derivative HR not implemented yet");
|
||||
/*
|
||||
HR->resize(5, 3);
|
||||
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
|
||||
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
|
||||
*/
|
||||
}
|
||||
return EssentialMatrix(c1Rc2, c1Tc2);
|
||||
}
|
||||
}
|
||||
boost::none, boost::optional<Matrix&> HR = boost::none) const;
|
||||
|
||||
/**
|
||||
* Given essential matrix E in camera frame B, convert to body frame C
|
||||
|
|
@ -194,17 +142,18 @@ public:
|
|||
|
||||
/// epipolar error, algebraic
|
||||
double error(const Vector& vA, const Vector& vB, //
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) {
|
||||
H->resize(1, 5);
|
||||
// See math.lyx
|
||||
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
|
||||
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
|
||||
* aTb_.basis();
|
||||
*H << HR, HD;
|
||||
}
|
||||
return dot(vA, E_ * vB);
|
||||
}
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Streaming operators
|
||||
/// @{
|
||||
|
||||
/// stream to stream
|
||||
friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
|
||||
|
||||
/// stream from stream
|
||||
friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3);
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool Point3::equals(const Point3 & q, double tol) const {
|
||||
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
|
||||
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
|
||||
&& fabs(z_ - q.z()) < tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -37,18 +38,18 @@ void Point3::print(const string& s) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
bool Point3::operator== (const Point3& q) const {
|
||||
bool Point3::operator==(const Point3& q) const {
|
||||
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::operator+(const Point3& q) const {
|
||||
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
|
||||
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::operator- (const Point3& q) const {
|
||||
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
|
||||
Point3 Point3::operator-(const Point3& q) const {
|
||||
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::add(const Point3 &q,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = eye(3,3);
|
||||
if (H2) *H2 = eye(3,3);
|
||||
Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (H1)
|
||||
*H1 = eye(3, 3);
|
||||
if (H2)
|
||||
*H2 = eye(3, 3);
|
||||
return *this + q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::sub(const Point3 &q,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = eye(3,3);
|
||||
if (H2) *H2 = -eye(3,3);
|
||||
Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (H1)
|
||||
*H1 = eye(3, 3);
|
||||
if (H2)
|
||||
*H2 = -eye(3, 3);
|
||||
return *this - q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::cross(const Point3 &q) const {
|
||||
return Point3( y_*q.z_ - z_*q.y_,
|
||||
z_*q.x_ - x_*q.z_,
|
||||
x_*q.y_ - y_*q.x_ );
|
||||
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
|
||||
x_ * q.y_ - y_ * q.x_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::dot(const Point3 &q) const {
|
||||
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
|
||||
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::norm() const {
|
||||
return sqrt( x_*x_ + y_*y_ + z_*z_ );
|
||||
return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Point3::normalize(boost::optional<Matrix&> H) const {
|
||||
Point3 normalized = *this / norm();
|
||||
if (H) {
|
||||
// 3*3 Derivative
|
||||
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
|
||||
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
|
||||
H->resize(3, 3);
|
||||
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
|
||||
*H /= pow(x2 + y2 + z2, 1.5);
|
||||
}
|
||||
return normalized;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -176,6 +176,9 @@ namespace gtsam {
|
|||
/** Distance of the point from the origin */
|
||||
double norm() const;
|
||||
|
||||
/** normalize, with optional Jacobian */
|
||||
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
|
||||
|
||||
/** cross product @return this x q */
|
||||
Point3 cross(const Point3 &q) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -28,6 +28,21 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::FromPoint3(const Point3& point,
|
||||
boost::optional<Matrix&> H) {
|
||||
Sphere2 direction(point);
|
||||
if (H) {
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix D_p_point;
|
||||
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
|
||||
// Calculate the 2*3 Jacobian
|
||||
H->resize(2, 3);
|
||||
*H << direction.basis().transpose() * D_p_point;
|
||||
}
|
||||
return direction;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
|
||||
// TODO allow any engine without including all of boost :-(
|
||||
|
|
|
|||
|
|
@ -70,6 +70,10 @@ public:
|
|||
p_ = p_ / p_.norm();
|
||||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
static Sphere2 FromPoint3(const Point3& point,
|
||||
boost::optional<Matrix&> H = boost::none);
|
||||
|
||||
/// Random direction, using boost::uniform_on_sphere
|
||||
static Sphere2 Random(boost::random::mt19937 & rng);
|
||||
|
||||
|
|
@ -90,7 +94,11 @@ public:
|
|||
/// @name Other functionality
|
||||
/// @{
|
||||
|
||||
/// Returns the local coordinate frame to tangent plane
|
||||
/**
|
||||
* Returns the local coordinate frame to tangent plane
|
||||
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
|
||||
* tangent to the sphere at the current direction.
|
||||
*/
|
||||
Matrix basis() const;
|
||||
|
||||
/// Return skew-symmetric associated with 3D point on unit sphere
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <sstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) {
|
|||
// Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, FromPose3_a) {
|
||||
Matrix actualH;
|
||||
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
|
||||
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
|
||||
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
|
||||
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, FromPose3_b) {
|
||||
Matrix actualH;
|
||||
Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
|
||||
Point3 c1Tc2(0.4, 0.5, 0.6);
|
||||
EssentialMatrix trueE(c1Rc2, c1Tc2);
|
||||
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
|
||||
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
|
||||
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
|
||||
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, streaming) {
|
||||
EssentialMatrix expected(c1Rc2, c1Tc2), actual;
|
||||
stringstream ss;
|
||||
ss << expected;
|
||||
ss >> actual;
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -14,73 +14,84 @@
|
|||
* @brief Unit tests for Point3 class
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Point3)
|
||||
GTSAM_CONCEPT_LIE_INST(Point3)
|
||||
|
||||
static Point3 P(0.2,0.7,-2);
|
||||
static Point3 P(0.2, 0.7, -2);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Point3, Lie) {
|
||||
Point3 p1(1,2,3);
|
||||
Point3 p2(4,5,6);
|
||||
Point3 p1(1, 2, 3);
|
||||
Point3 p2(4, 5, 6);
|
||||
Matrix H1, H2;
|
||||
|
||||
EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2)));
|
||||
EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2)));
|
||||
EXPECT(assert_equal(eye(3), H1));
|
||||
EXPECT(assert_equal(eye(3), H2));
|
||||
|
||||
EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2)));
|
||||
EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2)));
|
||||
EXPECT(assert_equal(-eye(3), H1));
|
||||
EXPECT(assert_equal(eye(3), H2));
|
||||
|
||||
EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.))));
|
||||
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
|
||||
EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point3, arithmetic)
|
||||
{
|
||||
CHECK(P*3==3*P);
|
||||
CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) ));
|
||||
CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1)));
|
||||
CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1)));
|
||||
CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2));
|
||||
CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3)));
|
||||
CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2));
|
||||
TEST( Point3, arithmetic) {
|
||||
CHECK(P * 3 == 3 * P);
|
||||
CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
|
||||
CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
|
||||
CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
|
||||
CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
|
||||
CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
|
||||
CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point3, equals)
|
||||
{
|
||||
TEST( Point3, equals) {
|
||||
CHECK(P.equals(P));
|
||||
Point3 Q;
|
||||
CHECK(!P.equals(Q));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point3, dot)
|
||||
{
|
||||
Point3 origin, ones(1,1,1);
|
||||
CHECK(origin.dot(Point3(1,1,0)) == 0);
|
||||
CHECK(ones.dot(Point3(1,1,0)) == 2);
|
||||
TEST( Point3, dot) {
|
||||
Point3 origin, ones(1, 1, 1);
|
||||
CHECK(origin.dot(Point3(1, 1, 0)) == 0);
|
||||
CHECK(ones.dot(Point3(1, 1, 0)) == 2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point3, stream)
|
||||
{
|
||||
Point3 p(1,2, -3);
|
||||
TEST( Point3, stream) {
|
||||
Point3 p(1, 2, -3);
|
||||
std::ostringstream os;
|
||||
os << p;
|
||||
EXPECT(os.str() == "[1, 2, -3]';");
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Point3, normalize) {
|
||||
Matrix actualH;
|
||||
Point3 point(1, -2, 3); // arbitrary point
|
||||
Point3 expected(point / sqrt(14));
|
||||
EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
|
||||
Matrix expectedH = numericalDerivative11<Point3, Point3>(
|
||||
boost::bind(&Point3::normalize, _1, boost::none), point);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include <boost/foreach.hpp>
|
||||
#include <boost/random.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
|
|
@ -324,6 +325,17 @@ TEST(Sphere2, Random) {
|
|||
EXPECT(assert_equal(expectedMean,actualMean,0.1));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Sphere2, 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);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
|
|
|
|||
|
|
@ -74,6 +74,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
|
|||
return graph_.linearize(state_.values);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LevenbergMarquardtOptimizer::increaseLambda(){
|
||||
state_.lambda *= params_.lambdaFactor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LevenbergMarquardtOptimizer::decreaseLambda(){
|
||||
state_.lambda /= params_.lambdaFactor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LevenbergMarquardtOptimizer::iterate() {
|
||||
|
||||
|
|
@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
if (error <= state_.error) {
|
||||
state_.values.swap(newValues);
|
||||
state_.error = error;
|
||||
state_.lambda /= params_.lambdaFactor;
|
||||
decreaseLambda();
|
||||
break;
|
||||
} else {
|
||||
// Either we're not cautious, or the same lambda was worse than the current error.
|
||||
|
|
@ -157,7 +167,10 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
|
||||
break;
|
||||
} else {
|
||||
state_.lambda *= params_.lambdaFactor;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl;
|
||||
|
||||
increaseLambda();
|
||||
}
|
||||
}
|
||||
} catch (IndeterminantLinearSystemException& e) {
|
||||
|
|
@ -172,7 +185,7 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
|
||||
break;
|
||||
} else {
|
||||
state_.lambda *= params_.lambdaFactor;
|
||||
increaseLambda();
|
||||
}
|
||||
}
|
||||
// Frank asks: why would we do that?
|
||||
|
|
|
|||
|
|
@ -174,6 +174,12 @@ public:
|
|||
return state_.lambda;
|
||||
}
|
||||
|
||||
// Apply policy to increase lambda if the current update was successful
|
||||
virtual void increaseLambda();
|
||||
|
||||
// Apply policy to decrease lambda if the current update was NOT successful
|
||||
virtual void decreaseLambda();
|
||||
|
||||
/// Access the current number of inner iterations
|
||||
int getInnerIterations() const {
|
||||
return state_.totalNumberInnerIterations;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,75 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2014, 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 EssentialMatrixConstraint.cpp
|
||||
* @author Frank Dellaert
|
||||
* @author Pablo Alcantarilla
|
||||
* @date Jan 5, 2014
|
||||
**/
|
||||
|
||||
#include <gtsam/slam/EssentialMatrixConstraint.h>
|
||||
//#include <gtsam/linear/GaussianFactor.h>
|
||||
//#include <gtsam/base/Testable.h>
|
||||
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void EssentialMatrixConstraint::print(const std::string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
|
||||
<< "," << keyFormatter(this->key2()) << ")\n";
|
||||
measuredE_.print(" measured: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This *e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& this->measuredE_.equals(e->measuredE_, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
|
||||
const Pose3& p2, boost::optional<Matrix&> Hp1,
|
||||
boost::optional<Matrix&> Hp2) const {
|
||||
|
||||
// compute relative Pose3 between p1 and p2
|
||||
Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
|
||||
|
||||
// convert to EssentialMatrix
|
||||
Matrix D_hx_1P2;
|
||||
EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_,
|
||||
(Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none);
|
||||
|
||||
// Calculate derivatives if needed
|
||||
if (Hp1) {
|
||||
// Hp1 will already contain the 6*6 derivative D_1P2_p1
|
||||
const Matrix& D_1P2_p1 = *Hp1;
|
||||
// The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
|
||||
*Hp1 = D_hx_1P2 * D_1P2_p1;
|
||||
}
|
||||
if (Hp2) {
|
||||
// Hp2 will already contain the 6*6 derivative D_1P2_p1
|
||||
const Matrix& D_1P2_p2 = *Hp2;
|
||||
// The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
|
||||
*Hp2 = D_hx_1P2 * D_1P2_p2;
|
||||
}
|
||||
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return measuredE_.localCoordinates(hx); // 5D error
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2014, 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 EssentialMatrixConstraint.h
|
||||
* @author Frank Dellaert
|
||||
* @author Pablo Alcantarilla
|
||||
* @date Jan 5, 2014
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
class EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
|
||||
|
||||
private:
|
||||
|
||||
typedef EssentialMatrixConstraint This;
|
||||
typedef NoiseModelFactor2<Pose3, Pose3> Base;
|
||||
|
||||
EssentialMatrix measuredE_; /** The measurement is an essential matrix */
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
EssentialMatrixConstraint() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key1 key for first pose
|
||||
* @param key2 key for second pose
|
||||
* @param measuredE measured EssentialMatrix
|
||||
* @param model noise model, 5D !
|
||||
*/
|
||||
EssentialMatrixConstraint(Key key1, Key key2,
|
||||
const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), measuredE_(measuredE) {
|
||||
}
|
||||
|
||||
virtual ~EssentialMatrixConstraint() {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Pose3& p1, const Pose3& p2,
|
||||
boost::optional<Matrix&> Hp1 = boost::none, //
|
||||
boost::optional<Matrix&> Hp2 = boost::none) const;
|
||||
|
||||
/** return the measured */
|
||||
const EssentialMatrix& measured() const {
|
||||
return measuredE_;
|
||||
}
|
||||
|
||||
/** number of variables attached to this factor */
|
||||
std::size_t size() const {
|
||||
return 2;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(measuredE_);
|
||||
}
|
||||
};
|
||||
// \class EssentialMatrixConstraint
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -671,27 +671,9 @@ bool writeBAL(const string& filename, SfM_data &data)
|
|||
|
||||
bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
|
||||
|
||||
// CHECKS
|
||||
// Store poses or cameras in SfM_data
|
||||
Values valuesPoses = values.filter<Pose3>();
|
||||
if( valuesPoses.size() != data.number_cameras()){
|
||||
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
|
||||
<<") and values (#cameras " << valuesPoses.size() << ")!!" << endl;
|
||||
return false;
|
||||
}
|
||||
Values valuesPoints = values.filter<Point3>();
|
||||
if( valuesPoints.size() != data.number_tracks()){
|
||||
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
|
||||
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
|
||||
}
|
||||
if(valuesPoints.size() + valuesPoses.size() != values.size()){
|
||||
cout << "writeBALfromValues write only poses and points values!!" << endl;
|
||||
return false;
|
||||
}
|
||||
if(valuesPoints.size()==0 || valuesPoses.size()==0){
|
||||
cout << "writeBALfromValues: No point or pose in values!!" << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
|
||||
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
|
||||
Key poseKey = symbol('x',i);
|
||||
Pose3 pose = values.at<Pose3>(poseKey);
|
||||
|
|
@ -699,6 +681,27 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
|
|||
PinholeCamera<Cal3Bundler> camera(pose, K);
|
||||
data.cameras[i] = camera;
|
||||
}
|
||||
} else {
|
||||
Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
|
||||
if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration
|
||||
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
|
||||
Key cameraKey = symbol('c',i);
|
||||
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
|
||||
data.cameras[i] = camera;
|
||||
}
|
||||
}else{
|
||||
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
|
||||
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Store 3D points in SfM_data
|
||||
Values valuesPoints = values.filter<Point3>();
|
||||
if( valuesPoints.size() != data.number_tracks()){
|
||||
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
|
||||
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
|
||||
}
|
||||
|
||||
for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
|
||||
Key pointKey = symbol('l',j);
|
||||
|
|
@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
|
|||
}
|
||||
}
|
||||
|
||||
// Write SfM_data to file
|
||||
return writeBAL(filename, data);
|
||||
}
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
|
|||
* while camera poses and values are read from Values)
|
||||
* @param filename The name of the BAL file to write
|
||||
* @param data SfM structure where the data is stored
|
||||
* @param values structure where the graph values are stored
|
||||
* @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
|
||||
* cameras, and should be Point3 for the 3D points). Note that the current version
|
||||
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
|
||||
* @return true if the parsing was successful, false otherwise
|
||||
*/
|
||||
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testEssentialMatrixConstraint.cpp
|
||||
* @brief Unit tests for EssentialMatrixConstraint Class
|
||||
* @author Frank Dellaert
|
||||
* @author Pablo Alcantarilla
|
||||
* @date Jan 5, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/EssentialMatrixConstraint.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( EssentialMatrixConstraint, test ) {
|
||||
// Create a factor
|
||||
Key poseKey1(1);
|
||||
Key poseKey2(2);
|
||||
Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20);
|
||||
Point3 trueTranslation(+0.5, -1.0, +1.0);
|
||||
Sphere2 trueDirection(trueTranslation);
|
||||
EssentialMatrix measurement(trueRotation, trueDirection);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25);
|
||||
EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model);
|
||||
|
||||
// Create a linearization point at the zero-error point
|
||||
Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
|
||||
Pose3 pose2(
|
||||
Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
|
||||
Point3(-3.37493895, 6.14660244, -8.93650986));
|
||||
|
||||
Vector expected = zero(5);
|
||||
Vector actual = factor.evaluateError(pose1,pose2);
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
|
||||
// Calculate numerical derivatives
|
||||
Matrix expectedH1 = numericalDerivative11<Pose3>(
|
||||
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
|
||||
boost::none, boost::none), pose1);
|
||||
Matrix expectedH2 = numericalDerivative11<Pose3>(
|
||||
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
|
||||
boost::none, boost::none), pose2);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH1;
|
||||
Matrix actualH2;
|
||||
factor.evaluateError(pose1, pose2, actualH1, actualH2);
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
|
||||
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) {
|
|||
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
|
||||
|
||||
// Check evaluation
|
||||
Point3 P1 = data.tracks[i].p;
|
||||
const Point2 pi = camera2.project(P1);
|
||||
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
|
||||
const Point2 pi = SimpleCamera::project_to_camera(P2);
|
||||
Point2 reprojectionError(pi - pB(i));
|
||||
Vector expected = reprojectionError.vector();
|
||||
|
||||
|
|
@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) {
|
|||
truth.insert(i, LieScalar(baseline / P1.z()));
|
||||
}
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtParams parameters;
|
||||
// parameters.setVerbosity("ERROR");
|
||||
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
// Check result
|
||||
EssentialMatrix actual = result.at<EssentialMatrix>(100);
|
||||
EXPECT(assert_equal(bodyE, actual, 1e-1));
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
|
||||
|
||||
// Check error at result
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
}
|
||||
|
||||
} // namespace example1
|
||||
|
|
|
|||
Loading…
Reference in New Issue