New "between factor" that is agnostic about scale.
Frank Dellaert
parent
3cd45be423
commit
832a6fe5c7
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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/linear/GaussianFactor.h>
|
||||||
|
#include <gtsam/geometry/EssentialMatrix.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
||||||
|
#include <ostream>
|
||||||
|
|
||||||
|
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 {
|
||||||
|
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
|
||||||
|
<< "," << keyFormatter(this->key2()) << ")\n";
|
||||||
|
measuredE_.print(" measured: ");
|
||||||
|
this->noiseModel_->print(" noise model: ");
|
||||||
|
}
|
||||||
|
|
||||||
|
/** equals */
|
||||||
|
virtual bool equals(const NonlinearFactor& expected,
|
||||||
|
double tol = 1e-9) const {
|
||||||
|
const This *e = dynamic_cast<const This*>(&expected);
|
||||||
|
return e != NULL && Base::equals(*e, tol)
|
||||||
|
&& this->measuredE_.equals(e->measuredE_, tol);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** implement functions needed to derive from Factor */
|
||||||
|
|
||||||
|
/** vector of errors */
|
||||||
|
Vector evaluateError(const Pose3& p1, const Pose3& p2,
|
||||||
|
boost::optional<Matrix&> Hp1 = boost::none, //
|
||||||
|
boost::optional<Matrix&> Hp2 = boost::none) 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
|
||||||
|
}
|
||||||
|
|
||||||
|
/** 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
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
Loading…
Reference in New Issue