12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Cal3DS2.h
View File

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

11
gtsam/geometry/Cal3Unify.cpp
View File

@ -26,22 +26,21 @@ namespace gtsam {
/* ************************************************************************* */
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 {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
return Base::K();
}
/* ************************************************************************* */
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 {
gtsam::print(K(), s + ".K");
gtsam::print(Vector(k()), s + ".k");
Base::print(s);
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,
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
if (H2) {

22
gtsam/geometry/Cal3Unify.h
View File

@ -22,7 +22,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -32,14 +32,14 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Unify : public DerivedValue<Cal3Unify> {
class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 {
typedef Cal3Unify This;
typedef Cal3DS2 Base;
private:
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 ]
// 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:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); }
Eigen::Vector4d k() const { return Base::k(); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// 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,
double k1, double k2, double k3 = 0.0, double k4 = 0.0) :
xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
Cal3Unify(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
/// @}
/// @name Advanced Constructors
@ -150,7 +150,6 @@ private:
{
ar & boost::serialization::make_nvp("Cal3Unify",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
@ -160,6 +159,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(k3_);
ar & BOOST_SERIALIZATION_NVP(k4_);
ar & BOOST_SERIALIZATION_NVP(xi_);
}

12
gtsam/geometry/tests/testCal3Unify.cpp
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
*/
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);
/* ************************************************************************* */
@ -67,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1)
Matrix computed;
K.uncalibrate(p, computed, boost::none);
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;
K.uncalibrate(p, boost::none, computed);
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)
{
Cal3Unify expected(0.1 + 1, 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);
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, 0.1 + 1);
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);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));