Merge branch 'origin/release/2.4.0'
Frank Dellaert
parent
ec067b4954
commit
886119bcf0
|
|
@ -9,6 +9,8 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -22,6 +24,7 @@ class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
|
|||
Vector vA_, vB_; ///< Homogeneous versions
|
||||
|
||||
typedef NoiseModelFactor1<EssentialMatrix> Base;
|
||||
typedef EssentialMatrixFactor This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -33,6 +36,12 @@ public:
|
|||
vB_(EssentialMatrix::Homogeneous(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)));
|
||||
}
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
|
|
@ -50,5 +59,94 @@ public:
|
|||
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
/**
|
||||
* Binary factor that optimizes for E and inverse depth: assumes measurement
|
||||
* in image 2 is perfect, and returns re-projection error in image 1
|
||||
*/
|
||||
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
|
||||
LieScalar> {
|
||||
|
||||
Point2 pA_, pB_; ///< Measurements in image A and B
|
||||
Cal3_S2 K_; ///< Calibration
|
||||
|
||||
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
|
||||
typedef EssentialMatrixFactor2 This;
|
||||
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
|
||||
const Cal3_S2& K, const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) {
|
||||
}
|
||||
|
||||
/// @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)));
|
||||
}
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s);
|
||||
std::cout << " EssentialMatrixFactor2 with measurements\n ("
|
||||
<< pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
|
||||
<< ")'" << std::endl;
|
||||
}
|
||||
|
||||
/// vector of errors returns 1D vector
|
||||
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
|
||||
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
|
||||
boost::none) const {
|
||||
|
||||
// We have point x,y in image 1
|
||||
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
|
||||
// We then convert to first camera by 2P = 1R2Õ*(P1-1T2)
|
||||
// The homogeneous coordinates of can be written as
|
||||
// 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
|
||||
// Note that this is just a homography for d==0
|
||||
Point3 dP1(pA_.x(), pA_.y(), 1);
|
||||
|
||||
// Project to normalized image coordinates, then uncalibrate
|
||||
Point2 pi;
|
||||
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);
|
||||
|
||||
} 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;
|
||||
|
||||
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);
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
if (Dd) // efficient backwards computation:
|
||||
// (2*2) * (2*3) * (3*3) * (3*1)
|
||||
*Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
|
||||
|
||||
}
|
||||
Point2 reprojectionError = pi - pB_;
|
||||
return reprojectionError.vector();
|
||||
}
|
||||
|
||||
};
|
||||
// EssentialMatrixFactor2
|
||||
|
||||
}// gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ class RotateFactor: public NoiseModelFactor1<Rot3> {
|
|||
Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> Base;
|
||||
typedef RotateFactor This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -34,6 +35,11 @@ public:
|
|||
Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
|
||||
}
|
||||
|
||||
/// @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))); }
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
|
|
@ -63,6 +69,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
|
|||
Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> Base;
|
||||
typedef RotateDirectionsFactor This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -72,6 +79,11 @@ public:
|
|||
Base(model, key), p_(p), z_(z) {
|
||||
}
|
||||
|
||||
/// @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))); }
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
|
|
|
|||
|
|
@ -18,11 +18,14 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef noiseModel::Isotropic::shared_ptr Model;
|
||||
|
||||
const string filename = findExampleDataFile("5pointExample1.txt");
|
||||
SfM_data data;
|
||||
bool readOK = readBAL(filename, data);
|
||||
Rot3 aRb = data.cameras[1].pose().rotation();
|
||||
Point3 aTb = data.cameras[1].pose().translation();
|
||||
double baseline = 0.1; // actual baseline of the camera
|
||||
|
||||
Point2 pA(size_t i) {
|
||||
return data.tracks[i].measurements[0].second;
|
||||
|
|
@ -37,6 +40,8 @@ Vector vB(size_t i) {
|
|||
return EssentialMatrix::Homogeneous(pB(i));
|
||||
}
|
||||
|
||||
Cal3_S2 K;
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor, testData) {
|
||||
CHECK(readOK);
|
||||
|
|
@ -77,18 +82,18 @@ TEST (EssentialMatrixFactor, factor) {
|
|||
// Check evaluation
|
||||
Vector expected(1);
|
||||
expected << 0;
|
||||
Matrix HActual;
|
||||
Vector actual = factor.evaluateError(trueE, HActual);
|
||||
Matrix Hactual;
|
||||
Vector actual = factor.evaluateError(trueE, Hactual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-7));
|
||||
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
Matrix HExpected;
|
||||
HExpected = numericalDerivative11<EssentialMatrix>(
|
||||
Matrix Hexpected;
|
||||
Hexpected = numericalDerivative11<EssentialMatrix>(
|
||||
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
|
||||
boost::none), trueE);
|
||||
|
||||
// Verify the Jacobian is correct
|
||||
EXPECT(assert_equal(HExpected, HActual, 1e-8));
|
||||
EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -102,8 +107,7 @@ TEST (EssentialMatrixFactor, fromConstraints) {
|
|||
// We start with a factor graph and add constraints to it
|
||||
// Noise sigma is 1cm, assuming metric measurements
|
||||
NonlinearFactorGraph graph;
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1,
|
||||
0.01);
|
||||
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));
|
||||
|
||||
|
|
@ -138,6 +142,77 @@ 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);
|
||||
|
||||
// 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);
|
||||
Point2 reprojectionError(pi - pB(i));
|
||||
Vector expected = reprojectionError.vector();
|
||||
|
||||
Matrix Hactual1, Hactual2;
|
||||
LieScalar d(baseline / P1.z());
|
||||
Vector actual = factor.evaluateError(E, d, Hactual1, Hactual2);
|
||||
EXPECT(assert_equal(expected, actual, 1e-7));
|
||||
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
Matrix Hexpected1, Hexpected2;
|
||||
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
|
||||
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
|
||||
boost::none, boost::none);
|
||||
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, E, d);
|
||||
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, E, d);
|
||||
|
||||
// Verify the Jacobian is correct
|
||||
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
|
||||
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor2, minimization) {
|
||||
// Here we want to optimize for E and inverse depths at the same time
|
||||
|
||||
// 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));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
truth.insert(100, trueE);
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
Point3 P1 = data.tracks[i].p;
|
||||
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(trueE, actual,1e-1));
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT(assert_equal(result.at<LieScalar>(i), truth.at<LieScalar>(i),1e-1));
|
||||
|
||||
// Check error at result
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue