Development of templated factors
dellaert
parent
01dbca8ce1
commit
fd188a4978
|
|
@ -20,26 +20,46 @@ namespace gtsam {
|
|||
*/
|
||||
class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
|
||||
|
||||
Point2 pA_, pB_; ///< Measurements in image A and B
|
||||
Vector vA_, vB_; ///< Homogeneous versions
|
||||
Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates
|
||||
|
||||
typedef NoiseModelFactor1<EssentialMatrix> Base;
|
||||
typedef EssentialMatrixFactor This;
|
||||
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
/**
|
||||
* Constructor
|
||||
* @param pA point in first camera, in calibrated coordinates
|
||||
* @param pB point in second camera, in calibrated coordinates
|
||||
* @param model noise model is about dot product in ideal, homogeneous coordinates
|
||||
*/
|
||||
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, key), pA_(pA), pB_(pB), //
|
||||
vA_(EssentialMatrix::Homogeneous(pA)), //
|
||||
vB_(EssentialMatrix::Homogeneous(pB)) {
|
||||
Base(model, key) {
|
||||
vA_ = EssentialMatrix::Homogeneous(pA);
|
||||
vB_ = EssentialMatrix::Homogeneous(pB);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param pA point in first camera, in pixel coordinates
|
||||
* @param pB point in second camera, in pixel coordinates
|
||||
* @param model noise model is about dot product in ideal, homogeneous coordinates
|
||||
* @param K calibration object, will be used only in constructor
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
|
||||
const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
|
||||
Base(model, key) {
|
||||
assert(K);
|
||||
vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
|
||||
vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
|
||||
}
|
||||
|
||||
/// @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)));
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/// print
|
||||
|
|
@ -47,14 +67,16 @@ public:
|
|||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s);
|
||||
std::cout << " EssentialMatrixFactor with measurements\n ("
|
||||
<< pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
|
||||
<< ")'" << std::endl;
|
||||
<< vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
/// vector of errors returns 1D vector
|
||||
Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
|
||||
boost::none) const {
|
||||
return (Vector(1) << E.error(vA_, vB_, H));
|
||||
Vector error(1);
|
||||
error << E.error(vA_, vB_, H);
|
||||
return error;
|
||||
}
|
||||
|
||||
};
|
||||
|
|
@ -67,26 +89,50 @@ class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
|
|||
LieScalar> {
|
||||
|
||||
Point3 dP1_; ///< 3D point corresponding to measurement in image 1
|
||||
Point2 p1_, p2_; ///< Measurements in image 1 and image 2
|
||||
Cal3_S2 K_; ///< Calibration
|
||||
Point2 pn_; ///< Measurement in image 2, in ideal coordinates
|
||||
double f_; ///< approximate conversion factor for error scaling
|
||||
|
||||
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
|
||||
typedef EssentialMatrixFactor2 This;
|
||||
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
/**
|
||||
* Constructor
|
||||
* @param pA point in first camera, in calibrated coordinates
|
||||
* @param pB point in second camera, in calibrated coordinates
|
||||
* @param model noise model should be in pixels, as well
|
||||
*/
|
||||
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
|
||||
const Cal3_S2& K, const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) {
|
||||
Point2 xy = K_.calibrate(p1_);
|
||||
dP1_ = Point3(xy.x(), xy.y(), 1);
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2) {
|
||||
dP1_ = Point3(pA.x(), pA.y(), 1);
|
||||
pn_ = pB;
|
||||
f_ = 1.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param pA point in first camera, in pixel coordinates
|
||||
* @param pB point in second camera, in pixel coordinates
|
||||
* @param K calibration object, will be used only in constructor
|
||||
* @param model noise model should be in pixels, as well
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
|
||||
const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
|
||||
Base(model, key1, key2) {
|
||||
assert(K);
|
||||
Point2 p1 = K->calibrate(pA);
|
||||
dP1_ = Point3(p1.x(), p1.y(), 1);
|
||||
pn_ = K->calibrate(pB);
|
||||
f_ = 0.5 * (K->fx() + K->fy());
|
||||
}
|
||||
|
||||
/// @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)));
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/// print
|
||||
|
|
@ -94,7 +140,7 @@ public:
|
|||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s);
|
||||
std::cout << " EssentialMatrixFactor2 with measurements\n ("
|
||||
<< p1_.vector().transpose() << ")' and (" << p2_.vector().transpose()
|
||||
<< dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose()
|
||||
<< ")'" << std::endl;
|
||||
}
|
||||
|
||||
|
|
@ -112,40 +158,38 @@ public:
|
|||
// The point d*P1 = (x,y,1) is computed in constructor as dP1_
|
||||
|
||||
// Project to normalized image coordinates, then uncalibrate
|
||||
Point2 pi;
|
||||
Point2 pn;
|
||||
if (!DE && !Dd) {
|
||||
|
||||
Point3 _1T2 = E.direction().point3();
|
||||
Point3 d1T2 = d * _1T2;
|
||||
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);
|
||||
Point2 pn = SimpleCamera::project_to_camera(dP2);
|
||||
pi = K_.uncalibrate(pn);
|
||||
pn = SimpleCamera::project_to_camera(dP2);
|
||||
|
||||
} else {
|
||||
|
||||
// Calculate derivatives. TODO if slow: optimize with Mathematica
|
||||
// 3*2 3*3 3*3 2*3 2*2
|
||||
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2, Dpi_pn;
|
||||
// 3*2 3*3 3*3 2*3
|
||||
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2;
|
||||
|
||||
Point3 _1T2 = E.direction().point3(D_1T2_dir);
|
||||
Point3 d1T2 = d * _1T2;
|
||||
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
|
||||
Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
|
||||
pi = K_.uncalibrate(pn, boost::none, Dpi_pn);
|
||||
pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
|
||||
|
||||
if (DE) {
|
||||
Matrix DdP2_E(3, 5);
|
||||
DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
|
||||
*DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5)
|
||||
*DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
|
||||
}
|
||||
|
||||
if (Dd) // efficient backwards computation:
|
||||
// (2*2) * (2*3) * (3*3) * (3*1)
|
||||
*Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
|
||||
// (2*3) * (3*3) * (3*1)
|
||||
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector()));
|
||||
|
||||
}
|
||||
Point2 reprojectionError = pi - p2_;
|
||||
return reprojectionError.vector();
|
||||
Point2 reprojectionError = pn - pn_;
|
||||
return f_ * reprojectionError.vector();
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -18,7 +18,11 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef noiseModel::Isotropic::shared_ptr Model;
|
||||
// Noise model for first type of factor is evaluating algebraic error
|
||||
noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1,
|
||||
0.01);
|
||||
// Noise model for second type of factor is evaluating pixel coordinates
|
||||
noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
|
||||
|
||||
namespace example1 {
|
||||
|
||||
|
|
@ -42,8 +46,6 @@ Vector vB(size_t i) {
|
|||
return EssentialMatrix::Homogeneous(pB(i));
|
||||
}
|
||||
|
||||
Cal3_S2 K;
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor, testData) {
|
||||
CHECK(readOK);
|
||||
|
|
@ -76,10 +78,9 @@ TEST (EssentialMatrixFactor, testData) {
|
|||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor, factor) {
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
|
||||
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
EssentialMatrixFactor factor(1, pA(i), pB(i), model);
|
||||
EssentialMatrixFactor factor(1, pA(i), pB(i), model1);
|
||||
|
||||
// Check evaluation
|
||||
Vector expected(1);
|
||||
|
|
@ -100,7 +101,7 @@ TEST (EssentialMatrixFactor, factor) {
|
|||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor, fromConstraints) {
|
||||
TEST (EssentialMatrixFactor, minimization) {
|
||||
// Here we want to optimize directly on essential matrix constraints
|
||||
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
|
||||
// but GTSAM does the equivalent anyway, provided we give the right
|
||||
|
|
@ -109,9 +110,8 @@ TEST (EssentialMatrixFactor, fromConstraints) {
|
|||
// We start with a factor graph and add constraints to it
|
||||
// Noise sigma is 1cm, assuming metric measurements
|
||||
NonlinearFactorGraph graph;
|
||||
Model model = noiseModel::Isotropic::Sigma(1, 0.01);
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model));
|
||||
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
|
|
@ -147,15 +147,13 @@ TEST (EssentialMatrixFactor, fromConstraints) {
|
|||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor2, factor) {
|
||||
EssentialMatrix E(aRb, aTb);
|
||||
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
|
||||
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model);
|
||||
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
|
||||
|
||||
// Check evaluation
|
||||
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
|
||||
const Point2 pn = SimpleCamera::project_to_camera(P2);
|
||||
const Point2 pi = K.uncalibrate(pn);
|
||||
const Point2 pi = SimpleCamera::project_to_camera(P2);
|
||||
Point2 reprojectionError(pi - pB(i));
|
||||
Vector expected = reprojectionError.vector();
|
||||
|
||||
|
|
@ -185,9 +183,8 @@ TEST (EssentialMatrixFactor2, minimization) {
|
|||
// We start with a factor graph and add constraints to it
|
||||
// Noise sigma is 1cm, assuming metric measurements
|
||||
NonlinearFactorGraph graph;
|
||||
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
|
||||
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
|
|
@ -235,8 +232,56 @@ Point2 pB(size_t i) {
|
|||
return data.tracks[i].measurements[1].second;
|
||||
}
|
||||
|
||||
// Matches Cal3Bundler K(500, 0, 0);
|
||||
Cal3_S2 K(500, 500, 0, 0, 0);
|
||||
boost::shared_ptr<Cal3Bundler> //
|
||||
K = boost::make_shared < Cal3Bundler > (500, 0, 0);
|
||||
|
||||
Vector vA(size_t i) {
|
||||
Point2 xy = K->calibrate(pA(i));
|
||||
return EssentialMatrix::Homogeneous(xy);
|
||||
}
|
||||
Vector vB(size_t i) {
|
||||
Point2 xy = K->calibrate(pB(i));
|
||||
return EssentialMatrix::Homogeneous(xy);
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor, extraTest) {
|
||||
// Additional test with camera moving in positive X direction
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
truth.insert(1, trueE);
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
|
||||
|
||||
// Check error at initial estimate
|
||||
Values initial;
|
||||
EssentialMatrix initialE = trueE.retract(
|
||||
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
|
||||
initial.insert(1, initialE);
|
||||
EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2);
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtParams parameters;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
// Check result
|
||||
EssentialMatrix actual = result.at<EssentialMatrix>(1);
|
||||
EXPECT(assert_equal(trueE, actual,1e-1));
|
||||
|
||||
// Check error at result
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
|
||||
// Check errors individually
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i),vB(i)), 1e-6);
|
||||
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor2, extraTest) {
|
||||
|
|
@ -245,9 +290,8 @@ TEST (EssentialMatrixFactor2, extraTest) {
|
|||
// We start with a factor graph and add constraints to it
|
||||
// Noise sigma is 1, assuming pixel measurements
|
||||
NonlinearFactorGraph graph;
|
||||
Model model = noiseModel::Isotropic::Sigma(2, 1);
|
||||
for (size_t i = 0; i < data.number_tracks(); i++)
|
||||
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
|
||||
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
|
|
|
|||
Loading…
Reference in New Issue