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

Merged in feature/merge-2.4 (pull request #5) 

Merge in changes from 2.4 release branch
release/4.3a0
Richard Roberts 2014-01-27 11:41:43 -05:00
parent 5aa7d55691 837085e5d4
commit f38b599a8e
14 changed files with 604 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
gtsam/geometry/EssentialMatrix.cpp Normal file
View File

@ -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

101
gtsam/geometry/EssentialMatrix.h
View File

@ -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);
/// @}

40
gtsam/geometry/Point3.cpp
View File

@ -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);
}
/* ************************************************************************* */
@ -62,25 +63,28 @@ 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_,
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
}
@ -94,6 +98,20 @@ double Point3::norm() const {
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;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";

3
gtsam/geometry/Point3.h
View File

@ -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;

15
gtsam/geometry/Sphere2.cpp
View File

@ -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 :-(

10
gtsam/geometry/Sphere2.h
View File

@ -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

33
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -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;

33
gtsam/geometry/tests/testPoint3.cpp
View File

@ -14,9 +14,10 @@
* @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;
@ -44,8 +45,7 @@ TEST(Point3, Lie) {
}
/* ************************************************************************* */
TEST( Point3, arithmetic)
{
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)));
@ -56,31 +56,42 @@ TEST( Point3, arithmetic)
}
/* ************************************************************************* */
TEST( Point3, equals)
{
TEST( Point3, equals) {
CHECK(P.equals(P));
Point3 Q;
CHECK(!P.equals(Q));
}
/* ************************************************************************* */
TEST( Point3, dot)
{
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)
{
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);
}
/* ************************************************************************* */

12
gtsam/geometry/tests/testSphere2.cpp
View File

@ -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));

4
gtsam/linear/NoiseModel.cpp
View File

@ -370,8 +370,6 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector invsigmas = reciprocal(sigmas_);
Vector weights = emul(invsigmas,invsigmas); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding
@ -383,7 +381,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
// Calculate weighted pseudo-inverse and corresponding precision
gttic(constrained_QR_weightedPseudoinverse);
double precision = weightedPseudoinverse(a, weights, pseudo);
double precision = weightedPseudoinverse(a, precisions_, pseudo);
gttoc(constrained_QR_weightedPseudoinverse);
// If precision is zero, no information on this column

75
gtsam/slam/EssentialMatrixConstraint.cpp Normal file
View File

@ -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

109
gtsam/slam/EssentialMatrixConstraint.h Normal file
View File

@ -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

76
gtsam/slam/tests/testEssentialMatrixConstraint.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

19
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -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