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

change name from Cal3Unify to Cal3Unified

release/4.3a0
jing 2014-03-27 15:15:30 -04:00
parent 77f494b341
commit 8eeb7e8cfb
3 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/geometry/Cal3Unify.cpp → gtsam/geometry/Cal3Unified.cpp
View File

@ -18,34 +18,34 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3Unify.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <cmath>
namespace gtsam {
/* ************************************************************************* */
Cal3Unify::Cal3Unify(const Vector &v):
Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
Cal3Unified::Cal3Unified(const Vector &v):
Base(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 Cal3Unified::K() const {
return Base::K();
}
/* ************************************************************************* */
Vector Cal3Unify::vector() const {
Vector Cal3Unified::vector() const {
return (Vector(10) << Base::vector(), xi_);
}
/* ************************************************************************* */
void Cal3Unify::print(const std::string& s) const {
void Cal3Unified::print(const std::string& s) const {
Base::print(s);
gtsam::print((Vector)(Vector(1) << xi_), s + ".xi");
}
/* ************************************************************************* */
bool Cal3Unify::equals(const Cal3Unify& K, double tol) const {
bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol ||
@ -55,7 +55,7 @@ bool Cal3Unify::equals(const Cal3Unify& K, double tol) const {
}
/* ************************************************************************* */
Point2 Cal3Unify::uncalibrate(const Point2& p,
Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
@ -103,7 +103,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p,
}
/* ************************************************************************* */
Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
// calibrate point to Nplane use base class::calibrate()
Point2 pnplane = Base::calibrate(pi, tol);
@ -112,7 +112,7 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
return this->nPlaneToSpace(pnplane);
}
/* ************************************************************************* */
Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const {
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
const double x = p.x(), y = p.y();
const double xy2 = x * x + y * y;
@ -122,7 +122,7 @@ Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const {
}
/* ************************************************************************* */
Point2 Cal3Unify::spaceToNPlane(const Point2& p) const {
Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
const double x = p.x(), y = p.y();
const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
@ -131,12 +131,12 @@ Point2 Cal3Unify::spaceToNPlane(const Point2& p) const {
}
/* ************************************************************************* */
Cal3Unify Cal3Unify::retract(const Vector& d) const {
return Cal3Unify(vector() + d);
Cal3Unified Cal3Unified::retract(const Vector& d) const {
return Cal3Unified(vector() + d);
}
/* ************************************************************************* */
Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const {
Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
return T2.vector() - vector();
}

20
gtsam/geometry/Cal3Unify.h → gtsam/geometry/Cal3Unified.h
View File

@ -32,9 +32,9 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 {
class GTSAM_EXPORT Cal3Unified : protected Cal3DS2 {
typedef Cal3Unify This;
typedef Cal3Unified This;
typedef Cal3DS2 Base;
private:
@ -57,17 +57,17 @@ public:
/// @{
/// Default Constructor with only unit focal length
Cal3Unify() : Cal3DS2(), xi_(0) {}
Cal3Unified() : Base(), xi_(0) {}
Cal3Unify(double fx, double fy, double s, double u0, double v0,
Cal3Unified(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) {}
Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3Unify(const Vector &v) ;
Cal3Unified(const Vector &v) ;
/// @}
/// @name Testable
@ -77,7 +77,7 @@ public:
void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3Unify& K, double tol = 10e-9) const;
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
@ -126,10 +126,10 @@ public:
/// @{
/// Given delta vector, update calibration
Cal3Unify retract(const Vector& d) const ;
Cal3Unified retract(const Vector& d) const ;
/// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3Unify& T2) const ;
Vector localCoordinates(const Cal3Unified& T2) const ;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
@ -148,7 +148,7 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Unify",
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);

28
gtsam/geometry/tests/testCal3Unify.cpp
View File

@ -17,12 +17,12 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unify.h>
#include <gtsam/geometry/Cal3Unified.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify)
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified)
/*
ground truth from matlab, code :
@ -32,11 +32,11 @@ 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(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
static Cal3Unified 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);
/* ************************************************************************* */
TEST( Cal3Unify, uncalibrate)
TEST( Cal3Unified, uncalibrate)
{
Point2 p_i(364.7791831734982, 305.6677211952602) ;
Point2 q = K.uncalibrate(p);
@ -44,7 +44,7 @@ TEST( Cal3Unify, uncalibrate)
}
/* ************************************************************************* */
TEST( Cal3Unify, spaceNplane)
TEST( Cal3Unified, spaceNplane)
{
Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
@ -52,17 +52,17 @@ TEST( Cal3Unify, spaceNplane)
}
/* ************************************************************************* */
TEST( Cal3Unify, calibrate)
TEST( Cal3Unified, calibrate)
{
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
CHECK( p.equals(pn_hat, 1e-8));
}
Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Unify, Duncalibrate1)
TEST( Cal3Unified, Duncalibrate1)
{
Matrix computed;
K.uncalibrate(p, computed, boost::none);
@ -71,7 +71,7 @@ TEST( Cal3Unify, Duncalibrate1)
}
/* ************************************************************************* */
TEST( Cal3Unify, Duncalibrate2)
TEST( Cal3Unified, Duncalibrate2)
{
Matrix computed;
K.uncalibrate(p, boost::none, computed);
@ -80,19 +80,19 @@ TEST( Cal3Unify, Duncalibrate2)
}
/* ************************************************************************* */
TEST( Cal3Unify, assert_equal)
TEST( Cal3Unified, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
}
/* ************************************************************************* */
TEST( Cal3Unify, retract)
TEST( Cal3Unified, retract)
{
Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
Cal3Unified 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 << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
Cal3Unify actual = K.retract(d);
Cal3Unified actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-9));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
}