From e4673e2cd5180f3b0bd148d12decf27de3f7cdda Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Thu, 11 Nov 2010 19:43:13 +0000 Subject: [PATCH] add more calibrations and a general camera type --- gtsam/geometry/Cal3Bundler.cpp | 140 +++++++++++++++++++++ gtsam/geometry/Cal3Bundler.h | 74 +++++++++++ gtsam/geometry/Cal3DS2.cpp | 119 ++++++++++++++++++ gtsam/geometry/Cal3DS2.h | 95 +++++++++++++++ gtsam/geometry/GeneralCameraT.h | 210 ++++++++++++++++++++++++++++++++ gtsam/geometry/Makefile.am | 3 +- 6 files changed, 640 insertions(+), 1 deletion(-) create mode 100644 gtsam/geometry/Cal3Bundler.cpp create mode 100644 gtsam/geometry/Cal3Bundler.h create mode 100644 gtsam/geometry/Cal3DS2.cpp create mode 100644 gtsam/geometry/Cal3DS2.h create mode 100644 gtsam/geometry/GeneralCameraT.h diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp new file mode 100644 index 000000000..bdc30711b --- /dev/null +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * Cal3Bundler.cpp + * + * Created on: Sep 25, 2010 + * Author: ydjian + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {} +Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {} +Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {} + +Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); } +Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; } +Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; } +void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; } + +bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { + if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol) + return false; + return true ; +} + +Point2 Cal3Bundler::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { +// r = x^2 + y^2 ; +// g = (1 + k(1)*r + k(2)*r^2) ; +// pi(:,i) = g * pn(:,i) + const double x = p.x(), y = p.y() ; + const double r = x*x + y*y ; + const double r2 = r*r ; + const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply + const double fg = f_*g ; + + // semantic meaningful version + //if (H1) *H1 = D2d_calibration(p) ; + //if (H2) *H2 = D2d_intrinsic(p) ; + + // unrolled version, much faster + if ( H1 || H2 ) { + + const double fx = f_*x, fy = f_*y ; + if ( H1 ) { + *H1 = Matrix_(2, 3, + g*x, fx*r , fx*r2, + g*y, fy*r , fy*r2) ; + } + + if ( H2 ) { + const double dr_dx = 2*x ; + const double dr_dy = 2*y ; + const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; + const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; + *H2 = Matrix_(2, 2, + fg + fx*dg_dx, fx*dg_dy, + fy*dg_dx , fg + fy*dg_dy) ; + } + } + + return Point2(fg * x, fg * y) ; +} + +Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; + const double r = xx + yy ; + const double dr_dx = 2*x ; + const double dr_dy = 2*y ; + const double g = 1 + (k1_+k2_*r) * r ; // save one multiply + //const double g = 1 + k1_*r + k2_*r*r ; + const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; + const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; + + Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_); + Matrix DR = Matrix_(2, 2, + g + x*dg_dx, x*dg_dy, + y*dg_dx , g + y*dg_dy) ; + return prod(DK,DR) ; +} + +Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { + + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; + const double r = xx + yy ; + const double r2 = r*r ; + const double f = f_ ; + const double g = 1 + (k1_+k2_*r) * r ; // save one multiply + //const double g = (1+k1_*r+k2_*r2) ; + + return Matrix_(2, 3, + g*x, f*x*r , f*x*r2, + g*y, f*y*r , f*y*r2); +} + +Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { + + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; + const double r = xx + yy ; + const double r2 = r*r; + const double dr_dx = 2*x ; + const double dr_dy = 2*y ; + const double g = 1 + (k1_*r) + (k2_*r2) ; + const double f = f_ ; + const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; + const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; + + Matrix H = Matrix_(2,5, + f*(g + x*dg_dx), f*(x*dg_dy), g*x, f*x*r , f*x*r2, + f*(y*dg_dx) , f*(g + y*dg_dy), g*y, f*y*r , f*y*r2); + + return H ; +} + + +Cal3Bundler Cal3Bundler::expmap(const Vector& d) const { return Cal3Bundler(vector() + d) ; } +Vector Cal3Bundler::logmap(const Cal3Bundler& T2) const { return vector() - T2.vector(); } +Cal3Bundler Cal3Bundler::Expmap(const Vector& v) { return Cal3Bundler(v) ; } +Vector Cal3Bundler::Logmap(const Cal3Bundler& p) { return p.vector(); } + + + +} diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h new file mode 100644 index 000000000..466f888f3 --- /dev/null +++ b/gtsam/geometry/Cal3Bundler.h @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * Cal3Bundler.h + * + * Created on: Sep 25, 2010 + * Author: ydjian + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +class Cal3Bundler: public Testable { + +private: + double f_, k1_, k2_ ; + +public: + + Cal3Bundler() ; + Cal3Bundler(const Vector &v) ; + Cal3Bundler(const double f, const double k1, const double k2) ; + Matrix K() const ; + Vector k() const ; + + Vector vector() const; + void print(const std::string& s = "") const; + bool equals(const Cal3Bundler& K, double tol = 10e-9) const; + + Point2 uncalibrate(const Point2& p, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const ; + + Matrix D2d_intrinsic(const Point2& p) const ; + Matrix D2d_calibration(const Point2& p) const ; + Matrix D2d_intrinsic_calibration(const Point2& p) const ; + + Cal3Bundler expmap(const Vector& d) const ; + Vector logmap(const Cal3Bundler& T2) const ; + + static Cal3Bundler Expmap(const Vector& v) ; + static Vector Logmap(const Cal3Bundler& p) ; + + int dim() const { return 3 ; } + static size_t Dim() { return 3; } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(f_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + } + +}; + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp new file mode 100644 index 000000000..c2376b72e --- /dev/null +++ b/gtsam/geometry/Cal3DS2.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * Cal3DS2.cpp + * + * Created on: Feb 28, 2010 + * Author: ydjian + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +Cal3DS2::Cal3DS2():fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0){} + +// Construction +Cal3DS2::Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4) : + fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + +Cal3DS2::Cal3DS2(const Vector &v): + fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} + +Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } +Vector Cal3DS2::k() const { return Vector_(4, k1_, k2_, k3_, k4_); } +Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; } +void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(k(), s + ".k") ; } + +bool Cal3DS2::equals(const Cal3DS2& 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) + return false ; + return true ; +} + +Point2 Cal3DS2::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { +// r = x^2 + y^2 ; +// g = (1 + k(1)*r + k(2)*r^2) ; +// dp = [2*k(3)*x*y + k(4)*(r + 2*x^2) ; 2*k(4)*x*y + k(3)*(r + 2*y^2)] ; +// pi(:,i) = g * pn(:,i) + dp ; + const double x = p.x(), y = p.y(), xy = x*y, xx = x*x, yy = y*y ; + const double r = xx + yy ; + const double g = (1+k1_*r+k2_*r*r) ; + const double dx = 2*k3_*xy + k4_*(r+2*xx) ; + const double dy = 2*k4_*xy + k3_*(r+2*yy) ; + + const double x2 = g*x + dx ; + const double y2 = g*y + dy ; + + if (H1) *H1 = D2d_calibration(p) ; + if (H2) *H2 = D2d_intrinsic(p) ; + + return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ; +} + +Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { + //const double fx = fx_, fy = fy_, s = s_ ; + const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; + //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ; + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; + const double r = xx + yy ; + const double dr_dx = 2*x ; + const double dr_dy = 2*y ; + const double r2 = r*r ; + const double g = 1 + k1*r + k2*r2 ; + const double dg_dx = k1*dr_dx + k2*2*r*dr_dx ; + const double dg_dy = k1*dr_dy + k2*2*r*dr_dy ; + + // Dx = 2*k3*xy + k4*(r+2*xx) ; + // Dy = 2*k4*xy + k3*(r+2*yy) ; + const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x) ; + const double dDx_dy = 2*k3*x + k4*dr_dy ; + const double dDy_dx = 2*k4*y + k3*dr_dx ; + const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y) ; + + Matrix DK = Matrix_(2, 2, fx_, s_, 0.0, fy_); + Matrix DR = Matrix_(2, 2, g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, + y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy) ; + + return prod(DK,DR) ; +} + + +Matrix Cal3DS2::D2d_calibration(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ; + const double r = xx + yy ; + const double r2 = r*r ; + const double fx = fx_, fy = fy_, s = s_ ; + const double g = (1+k1_*r+k2_*r2) ; + + return Matrix_(2, 9, + g*x, 0.0, g*y, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy), + 0.0, g*y, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) ); +} + +Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; } +Vector Cal3DS2::logmap(const Cal3DS2& T2) const { return vector() - T2.vector(); } +Cal3DS2 Cal3DS2::Expmap(const Vector& v) { return Cal3DS2(v) ; } +Vector Cal3DS2::Logmap(const Cal3DS2& p) { return p.vector(); } + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h new file mode 100644 index 000000000..95e4f8f45 --- /dev/null +++ b/gtsam/geometry/Cal3DS2.h @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * Cal3DS2.h + * + * Created on: Feb 28, 2010 + * Author: ydjian + */ + + +#pragma once +#ifndef CAL3DS2_H_ +#define CAL3DS2_H_ + +#include +#include +#include + +namespace gtsam { + + /** This class a camera with radial distortion */ + class Cal3DS2 : Testable { + private: + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_ ; // radial 2nd-order and 4th-order + double k3_, k4_ ; // tagential distortion + + // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + // r = Pn.x^2 + Pn.y^2 + // \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ; + // k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + + public: + // Default Constructor with only unit focal length + Cal3DS2(); + + // Construction + Cal3DS2(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3, double k4) ; + + Cal3DS2(const Vector &v) ; + + Matrix K() const ; + Vector k() const ; + Vector vector() const ; + void print(const std::string& s = "") const ; + bool equals(const Cal3DS2& K, double tol = 10e-9) const; + + + Point2 uncalibrate(const Point2& p, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const ; + + Matrix D2d_intrinsic(const Point2& p) const ; + Matrix D2d_calibration(const Point2& p) const ; + + Cal3DS2 expmap(const Vector& d) const ; + Vector logmap(const Cal3DS2& T2) const ; + + static Cal3DS2 Expmap(const Vector& v) ; + static Vector Logmap(const Cal3DS2& p) ; + + int dim() const { return 9 ; } + static size_t Dim() { return 9; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(k3_); + ar & BOOST_SERIALIZATION_NVP(k4_); + } + + }; +} + +#endif /* CAL3DS2_H_ */ diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h new file mode 100644 index 000000000..32aab0534 --- /dev/null +++ b/gtsam/geometry/GeneralCameraT.h @@ -0,0 +1,210 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * DistortedCameraT.h + * + * Created on: Mar 1, 2010 + * Author: ydjian + */ + +#pragma once +#ifndef GENERALCAMERAT_H_ +#define GENERALCAMERAT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +namespace gtsam { + +template +class GeneralCameraT { + + private: + Camera calibrated_; // Calibrated camera + Calibration calibration_; // Calibration + + public: + GeneralCameraT(){} + GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} + GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} + GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} + GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} + GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} + + // Vector Initialization + GeneralCameraT(const Vector &v) : + calibrated_(subrange(v, 0, Camera::Dim())), + calibration_(subrange(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} + + inline const Pose3& pose() const { return calibrated_.pose(); } + inline const Camera& calibrated() const { return calibrated_; } + inline const Calibration& calibration() const { return calibration_; } + + std::pair projectSafe( + const Point3& P, + boost::optional H1, + boost::optional H2) const { + + Point3 cameraPoint = calibrated_.pose().transform_to(P); + return std::pair( + project(P,H1,H2), + cameraPoint.z() > 0); + } + + /** + * project a 3d point to the camera + * P is point in camera coordinate + * H1 is respect to pose + calibration + * H2 is respect to landmark + */ + Point2 project(const Point3& P, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + if (!H1 && !H2) { + Point2 intrinsic = calibrated_.project(P); + return calibration_.uncalibrate(intrinsic) ; + } + + Matrix I1, I2, J1, J2 ; + Point2 intrinsic = calibrated_.project(P, I1, I2); + Point2 projection = calibration_.uncalibrate(intrinsic, J1, J2); + + if ( H1 ) { Matrix T = prod(J2,I1); *H1 = collect(2, &T, &J1) ; } + if ( H2 ) *H2 = prod(J2, I2) ; + + return projection; + } + + // dump functions for compilation for now + bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { + return calibrated_.equals(camera.calibrated_, tol) && + calibration_.equals(camera.calibration_, tol) ; + } + + void print(const std::string& s = "") const { + calibrated_.pose().print(s + ".camera.") ; + calibration_.print(s + ".calibration.") ; + } + + GeneralCameraT expmap(const Vector &v) const { + return GeneralCameraT( + calibrated_.expmap(subrange(v,0,Camera::Dim())), + calibration_.expmap(subrange(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()))); + } + + Vector logmap(const GeneralCameraT &C) const { + //std::cout << "logmap" << std::endl; + const Vector v1(calibrated().logmap(C.calibrated())), + v2(calibration().logmap(C.calibration())); + return concatVectors(2,&v1,&v2) ; + } + + static GeneralCameraT Expmap(const Vector& v) { + //std::cout << "Expmap" << std::endl; + return GeneralCameraT( + Camera::Expmap(subrange(v,0,Camera::Dim())), + Calibration::Expmap(subrange(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) + ); + } + + static Vector Logmap(const GeneralCameraT& p) { + //std::cout << "Logmap" << std::endl; + const Vector v1(Camera::Logmap(p.calibrated())), + v2(Calibration::Logmap(p.calibration())); + return concatVectors(2,&v1,&v2); + + } + + inline GeneralCameraT compose(const Pose3 &p) const { + return GeneralCameraT( pose().compose(p), calibration_ ) ; + } + + Matrix D2d_camera(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = prod(D_2d_intrinsic,D_intrinsic_pose); + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); + + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; + Matrix D(2,n1+n2) ; + + subrange(D,0,2,0,n1) = D_2d_pose ; + subrange(D,0,2,n1,n1+n2) = D_2d_calibration ; + return D; + } + + Matrix D2d_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + return prod(D_2d_intrinsic,D_intrinsic_3d); + } + + Matrix D2d_camera_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = prod(D_2d_intrinsic,D_intrinsic_pose); + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); + + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_3d = prod(D_2d_intrinsic,D_intrinsic_3d); + + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; + + Matrix D(2,n1+n2+3) ; + + subrange(D,0,2,0,n1) = D_2d_pose ; + subrange(D,0,2,n1,n1+n2) = D_2d_calibration ; + subrange(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; + return D; + } + + //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } + inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } + static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } + +private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(calibrated_); + ar & BOOST_SERIALIZATION_NVP(calibration_); + } + + }; + +typedef GeneralCameraT Cal3BundlerCamera; +typedef GeneralCameraT Cal3DS2Camera; +typedef GeneralCameraT Cal3_S2Camera; + +} + +#endif diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 265200d6c..ab65acf81 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -15,7 +15,8 @@ sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3 # Cameras -sources += Cal3_S2.cpp CalibratedCamera.cpp SimpleCamera.cpp +headers += GeneralCameraT.h +sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera # Stereo