midterm commit, now Cal3Unify live on Cal3DS2, jacobian sequnce changes

release/4.3a0
jing 2014-03-27 14:42:53 -04:00
parent 7ff7482792
commit 6d356fbf8e
4 changed files with 23 additions and 24 deletions

View File

@ -30,7 +30,7 @@ namespace gtsam {
*/ */
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> { class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
private: protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order double k1_, k2_ ; // radial 2nd-order and 4th-order

View File

@ -26,22 +26,21 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Cal3Unify::Cal3Unify(const Vector &v): Cal3Unify::Cal3Unify(const Vector &v):
xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Cal3Unify::K() const { Matrix Cal3Unify::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); return Base::K();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3Unify::vector() const { Vector Cal3Unify::vector() const {
return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); return (Vector(10) << Base::vector(), xi_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3Unify::print(const std::string& s) const { void Cal3Unify::print(const std::string& s) const {
gtsam::print(K(), s + ".K"); Base::print(s);
gtsam::print(Vector(k()), s + ".k");
gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi");
} }
@ -119,7 +118,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p,
fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
*H1 = collect(2, &DDS2U, &DDS2V); *H1 = collect(2, &DDS2V, &DDS2U);
} }
// Inlined derivative for points // Inlined derivative for points
if (H2) { if (H2) {

View File

@ -22,7 +22,7 @@
#pragma once #pragma once
#include <gtsam/base/DerivedValue.h> #include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
namespace gtsam { namespace gtsam {
@ -32,14 +32,14 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3Unify : public DerivedValue<Cal3Unify> { class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 {
typedef Cal3Unify This;
typedef Cal3DS2 Base;
private: private:
double xi_; // mirror parameter double xi_; // mirror parameter
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double k3_, k4_ ; // tangential distortion
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
@ -50,18 +50,18 @@ private:
public: public:
Matrix K() const ; Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } Eigen::Vector4d k() const { return Base::k(); }
Vector vector() const ; Vector vector() const ;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/// Default Constructor with only unit focal length /// Default Constructor with only unit focal length
Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} Cal3Unify() : Cal3DS2(), xi_(0) {}
Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, Cal3Unify(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3 = 0.0, double k4 = 0.0) : double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
@ -150,7 +150,6 @@ private:
{ {
ar & boost::serialization::make_nvp("Cal3Unify", ar & boost::serialization::make_nvp("Cal3Unify",
boost::serialization::base_object<Value>(*this)); boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_); ar & BOOST_SERIALIZATION_NVP(s_);
@ -160,6 +159,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(k3_); ar & BOOST_SERIALIZATION_NVP(k3_);
ar & BOOST_SERIALIZATION_NVP(k4_); ar & BOOST_SERIALIZATION_NVP(k4_);
ar & BOOST_SERIALIZATION_NVP(xi_);
} }

View File

@ -32,7 +32,7 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
*/ */
static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); static Cal3Unify K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
static Point2 p(0.5, 0.7); static Point2 p(0.5, 0.7);
/* ************************************************************************* */ /* ************************************************************************* */
@ -67,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1)
Matrix computed; Matrix computed;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5)); CHECK(assert_equal(numerical,computed,1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -76,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2)
Matrix computed; Matrix computed;
K.uncalibrate(p, boost::none, computed); K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5)); CHECK(assert_equal(numerical,computed,1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -88,10 +88,10 @@ TEST( Cal3Unify, assert_equal)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Unify, retract) TEST( Cal3Unify, retract)
{ {
Cal3Unify expected(0.1 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
Vector d(10); Vector d(10);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
Cal3Unify actual = K.retract(d); Cal3Unify actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));