EssentialMatrix class (moved from testEssentialMatrix.cpp)
Frank Dellaert
parent
6459053cf9
commit
f8d2c93303
|
|
@ -0,0 +1,125 @@
|
||||||
|
/*
|
||||||
|
* @file EssentialMatrix.h
|
||||||
|
* @brief EssentialMatrix class
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date December 17, 2013
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/geometry/Rot3.h>
|
||||||
|
#include <gtsam/geometry/Sphere2.h>
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
||||||
|
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.
|
||||||
|
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
|
||||||
|
*/
|
||||||
|
class EssentialMatrix: public DerivedValue<EssentialMatrix> {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
Rot3 aRb_; ///< Rotation between a and b
|
||||||
|
Sphere2 aTb_; ///< translation direction from a to b
|
||||||
|
Matrix E_; ///< Essential matrix
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Static function to convert Point2 to homogeneous coordinates
|
||||||
|
static Vector Homogeneous(const Point2& p) {
|
||||||
|
return Vector(3) << p.x(), p.y(), 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @name Constructors and named constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// Construct from rotation and translation
|
||||||
|
EssentialMatrix(const Rot3& aRb, const Sphere2& aTb) :
|
||||||
|
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Value
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// Return the dimensionality of the tangent space
|
||||||
|
virtual size_t dim() const {
|
||||||
|
return 5;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the coordinates in the tangent space
|
||||||
|
virtual Vector localCoordinates(const EssentialMatrix& value) const {
|
||||||
|
return Vector(5) << 0, 0, 0, 0, 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// print with optional string
|
||||||
|
void print(const std::string& s) const {
|
||||||
|
std::cout << s;
|
||||||
|
aRb_.print("R:\n");
|
||||||
|
aTb_.print("d: ");
|
||||||
|
}
|
||||||
|
|
||||||
|
/// assert equality up to a tolerance
|
||||||
|
bool equals(const EssentialMatrix& other, double tol) const {
|
||||||
|
return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Essential matrix methods
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// Rotation
|
||||||
|
const Rot3& rotation() const {
|
||||||
|
return aRb_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Direction
|
||||||
|
const Sphere2& direction() const {
|
||||||
|
return aTb_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Return 3*3 matrix representation
|
||||||
|
const Matrix& matrix() const {
|
||||||
|
return E_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// epipolar error, algebraic
|
||||||
|
double error(const Vector& vA, const Vector& vB, //
|
||||||
|
boost::optional<Matrix&> H = boost::none) const {
|
||||||
|
if (H) {
|
||||||
|
H->resize(1, 5);
|
||||||
|
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
|
||||||
|
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
|
||||||
|
* aTb_.getBasis();
|
||||||
|
*H << HR, HD;
|
||||||
|
}
|
||||||
|
return dot(vA, E_ * vB);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
} // gtsam
|
||||||
|
|
||||||
|
|
@ -5,8 +5,7 @@
|
||||||
* @date December 17, 2013
|
* @date December 17, 2013
|
||||||
*/
|
*/
|
||||||
|
|
||||||
//#include <gtsam/geometry/EssentialMatrix.h>
|
#include <gtsam/geometry/EssentialMatrix.h>
|
||||||
#include <gtsam/geometry/Sphere2.h>
|
|
||||||
#include <gtsam/geometry/CalibratedCamera.h>
|
#include <gtsam/geometry/CalibratedCamera.h>
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
|
@ -22,119 +21,6 @@ using namespace std;
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
using namespace gtsam;
|
using 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.
|
|
||||||
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
|
|
||||||
*/
|
|
||||||
class EssentialMatrix: public DerivedValue<EssentialMatrix> {
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
Rot3 aRb_; ///< Rotation between a and b
|
|
||||||
Sphere2 aTb_; ///< translation direction from a to b
|
|
||||||
Matrix E_; ///< Essential matrix
|
|
||||||
|
|
||||||
/// Static function to convert Point2 to 3D
|
|
||||||
static Point3 Upgrade(const Point2& p) {
|
|
||||||
return Point3(p.x(), p.y(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/// Static function to convert Point2 to homogeneous coordinates
|
|
||||||
static Vector Homogeneous(const Point2& p) {
|
|
||||||
return Vector(3) << p.x(), p.y(), 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @name Constructors and named constructors
|
|
||||||
/// @{
|
|
||||||
|
|
||||||
/// Construct from rotation and translation
|
|
||||||
EssentialMatrix(const Rot3& aRb, const Sphere2& aTb) :
|
|
||||||
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
|
||||||
|
|
||||||
/// @name Value
|
|
||||||
/// @{
|
|
||||||
|
|
||||||
/// Return the dimensionality of the tangent space
|
|
||||||
virtual size_t dim() const {
|
|
||||||
return 5;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// 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);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Compute the coordinates in the tangent space
|
|
||||||
virtual Vector localCoordinates(const EssentialMatrix& value) const {
|
|
||||||
return Vector(5) << 0, 0, 0, 0, 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
|
||||||
|
|
||||||
/// @name Testable
|
|
||||||
/// @{
|
|
||||||
|
|
||||||
/// print with optional string
|
|
||||||
void print(const string& s) const {
|
|
||||||
cout << s;
|
|
||||||
aRb_.print("R:\n");
|
|
||||||
aTb_.print("d: ");
|
|
||||||
}
|
|
||||||
|
|
||||||
/// assert equality up to a tolerance
|
|
||||||
bool equals(const EssentialMatrix& other, double tol) const {
|
|
||||||
return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
|
||||||
|
|
||||||
/// @name Essential matrix methods
|
|
||||||
/// @{
|
|
||||||
|
|
||||||
/// Rotation
|
|
||||||
const Rot3& rotation() const {
|
|
||||||
return aRb_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Direction
|
|
||||||
const Sphere2& direction() const {
|
|
||||||
return aTb_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Return 3*3 matrix representation
|
|
||||||
const Matrix& matrix() const {
|
|
||||||
return E_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// epipolar error, algebraic
|
|
||||||
double error(const Vector& vA, const Vector& vB, //
|
|
||||||
boost::optional<Matrix&> H = boost::none) const {
|
|
||||||
if (H) {
|
|
||||||
H->resize(1, 5);
|
|
||||||
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
|
|
||||||
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
|
|
||||||
* aTb_.getBasis();
|
|
||||||
*H << HR, HD;
|
|
||||||
}
|
|
||||||
return dot(vA, E_ * vB);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Factor that evaluates epipolar error p'Ep for given essential matrix
|
* Factor that evaluates epipolar error p'Ep for given essential matrix
|
||||||
*/
|
*/
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue