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

Consistent and better formatting

release/4.3a0
Varun Agrawal 2020-12-02 17:15:10 -05:00
parent 0a55d31702
commit 70bab8e00c
14 changed files with 298 additions and 379 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/geometry/Cal3Bundler.cpp
View File

@ -15,11 +15,11 @@
* @author ydjian * @author ydjian
*/ */
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam { namespace gtsam {
@ -39,9 +39,7 @@ Vector4 Cal3Bundler::k() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Cal3Bundler::vector() const { Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
return Vector3(fx_, k1_, k2_);
}
/* ************************************************************************* */ /* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) { std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
@ -52,7 +50,8 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const { void Cal3Bundler::print(const std::string& s) const {
gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), s + ".K"); gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
s + ".K");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -64,8 +63,8 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, // Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { OptionalJacobian<2, 2> Dp) const {
// r = x² + y²; // r = x² + y²;
// g = (1 + k(1)*r + k(2)*r²); // g = (1 + k(1)*r + k(2)*r²);
// pi(:,i) = g * pn(:,i) // pi(:,i) = g * pn(:,i)
@ -93,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const { OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 // Copied from Cal3DS2
// but specialized with k1, k2 non-zero only and fx=fy and s=0 // but specialized with k1, k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_; double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y); const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary // initialize by ignoring the distortion at all, might be problematic for
// pixels around boundary
Point2 pn(x, y); Point2 pn(x, y);
// iterate until the uncalibrate is close to the actual pixel coordinate // iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10; const int maxIterations = 10;
int iteration; int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) { for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_) if (distance2(uncalibrate(pn), pi) <= tol_) break;
break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy; const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr); const double g = (1 + k1_ * rr + k2_ * rr * rr);
@ -118,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
if (iteration >= maxIterations) if (iteration >= maxIterations)
throw std::runtime_error( throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization"); "Cal3Bundler::calibrate fails to converge. need a better "
"initialization");
calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp); calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);

47
gtsam/geometry/Cal3Bundler.h
View File

@ -30,7 +30,6 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3Bundler : public Cal3 { class GTSAM_EXPORT Cal3Bundler : public Cal3 {
private: private:
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
double tol_ = 1e-5; ///< tolerance value when calibrating double tol_ = 1e-5; ///< tolerance value when calibrating
@ -40,7 +39,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
// but are treated as constants. // but are treated as constants.
public: public:
enum { dimension = 3 }; enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
@ -83,40 +81,28 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
/// @{ /// @{
/// distorsion parameter k1 /// distorsion parameter k1
inline double k1() const { inline double k1() const { return k1_; }
return k1_;
}
/// distorsion parameter k2 /// distorsion parameter k2
inline double k2() const { inline double k2() const { return k2_; }
return k2_;
}
/// image center in x /// image center in x
inline double px() const { inline double px() const { return u0_; }
return u0_;
}
/// image center in y /// image center in y
inline double py() const { inline double py() const { return v0_; }
return v0_;
}
Matrix3 K() const override; ///< Standard 3*3 calibration matrix Matrix3 K() const override; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector3 vector() const; Vector3 vector() const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/// get parameter u0 /// get parameter u0
inline double u0() const { inline double u0() const { return u0_; }
return u0_;
}
/// get parameter v0 /// get parameter v0
inline double v0() const { inline double v0() const { return v0_; }
return v0_;
}
#endif #endif
/** /**
@ -128,7 +114,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = boost::none) const;
/** /**
* Convert a pixel coordinate to ideal coordinate xy * Convert a pixel coordinate to ideal coordinate xy
@ -138,8 +124,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates * @return point in intrinsic coordinates
*/ */
Point2 calibrate(const Point2& pi, Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = boost::none) const;
/// @deprecated might be removed in next release, use uncalibrate /// @deprecated might be removed in next release, use uncalibrate
@ -172,15 +157,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
} }
private: private:
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template <class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) { void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp( ar& boost::serialization::make_nvp(
"Cal3Bundler", boost::serialization::base_object<Cal3>(*this)); "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
ar& BOOST_SERIALIZATION_NVP(k1_); ar& BOOST_SERIALIZATION_NVP(k1_);
@ -189,13 +173,12 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
} }
/// @} /// @}
}; };
template<> template <>
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {}; struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
template<> template <>
struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {}; struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
} // namespace gtsam } // namespace gtsam

11
gtsam/geometry/Cal3DS2.cpp
View File

@ -16,11 +16,11 @@
* @author Varun Agrawal * @author Varun Agrawal
*/ */
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2.h>
namespace gtsam { namespace gtsam {
@ -31,9 +31,7 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const { void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
Base::print(s_);
}
/* ************************************************************************* */ /* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
@ -50,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
return T2.vector() - vector(); return T2.vector() - vector();
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */

33
gtsam/geometry/Cal3DS2.h
View File

@ -11,7 +11,8 @@
/** /**
* @file Cal3DS2.h * @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base * @brief Calibration of a camera with radial distortion, calculations in base
* class Cal3DS2_Base
* @date Feb 28, 2010 * @date Feb 28, 2010
* @author ydjian * @author ydjian
* @autho Varun Agrawal * @autho Varun Agrawal
@ -31,11 +32,9 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
using Base = Cal3DS2_Base; using Base = Cal3DS2_Base;
public: public:
enum { dimension = 9 }; enum { dimension = 9 };
/// @name Standard Constructors /// @name Standard Constructors
@ -54,7 +53,7 @@ public:
/// @name Advanced Constructors /// @name Advanced Constructors
/// @{ /// @{
Cal3DS2(const Vector9 &v) : Base(v) {} Cal3DS2(const Vector9& v) : Base(v) {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -75,10 +74,10 @@ public:
/// @{ /// @{
/// Given delta vector, update calibration /// Given delta vector, update calibration
Cal3DS2 retract(const Vector& d) const ; Cal3DS2 retract(const Vector& d) const;
/// Given a different calibration, calculate update to obtain it /// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3DS2& T2) const ; Vector localCoordinates(const Cal3DS2& T2) const;
/// Return dimensions of calibration manifold object /// Return dimensions of calibration manifold object
virtual size_t dim() const override { return Dim(); } virtual size_t dim() const override { return Dim(); }
@ -97,30 +96,24 @@ public:
/// @} /// @}
private:
private:
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template <class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) void serialize(Archive& ar, const unsigned int /*version*/) {
{ ar& boost::serialization::make_nvp(
ar & boost::serialization::make_nvp("Cal3DS2", "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
boost::serialization::base_object<Cal3DS2_Base>(*this));
} }
/// @} /// @}
}; };
template<> template <>
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {}; struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
template<> template <>
struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {}; struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
} }

29
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -16,11 +16,11 @@
* @author Varun Agrawal * @author Varun Agrawal
*/ */
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam { namespace gtsam {
@ -54,23 +54,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Matrix29 D2dcalibration(double x, double y, double xx, static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
double yy, double xy, double rr, double r4, double pnx, double pny, double xy, double rr, double r4, double pnx,
const Matrix2& DK) { double pny, const Matrix2& DK) {
Matrix25 DR1; Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Matrix24 DR2; Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy; y * rr, y * r4, rr + 2 * yy, 2 * xy;
Matrix29 D; Matrix29 D;
D << DR1, DK * DR2; D << DR1, DK * DR2;
return D; return D;
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Matrix2 D2dintrinsic(double x, double y, double rr, static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
double g, double k1, double k2, double p1, double p2, double k2, double p1, double p2,
const Matrix2& DK) { const Matrix2& DK) {
const double drdx = 2. * x; const double drdx = 2. * x;
const double drdy = 2. * y; const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -84,8 +84,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Matrix2 DR; Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy; y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR; return DK * DR;
} }
@ -100,7 +100,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy; const double rr = xx + yy;
const double r4 = rr * rr; const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component // tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
@ -190,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */

41
gtsam/geometry/Cal3Unified.h
View File

@ -28,20 +28,21 @@
namespace gtsam { namespace gtsam {
/** /**
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion * @brief Calibration of a omni-directional camera with mirror + lens radial
* distortion
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
* *
* Similar to Cal3DS2, does distortion but has additional mirror parameter xi * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
* 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² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + P.y² + 1})] * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
* P.y² + 1})]
* r² = Pn.x² + Pn.y² * r² = Pn.x² + Pn.y²
* \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ; * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
* k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
using This = Cal3Unified; using This = Cal3Unified;
using Base = Cal3DS2_Base; using Base = Cal3DS2_Base;
@ -49,7 +50,6 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
double xi_ = 0.0f; ///< mirror parameter double xi_ = 0.0f; ///< mirror parameter
public: public:
enum { dimension = 10 }; enum { dimension = 10 };
/// @name Standard Constructors /// @name Standard Constructors
@ -90,10 +90,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
/// @{ /// @{
/// mirror parameter /// mirror parameter
inline double xi() const { return xi_;} inline double xi() const { return xi_; }
/// Return all parameters as a vector /// Return all parameters as a vector
Vector10 vector() const ; Vector10 vector() const;
/** /**
* convert intrinsic coordinates xy to image coordinates uv * convert intrinsic coordinates xy to image coordinates uv
@ -103,8 +103,8 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
OptionalJacobian<2,10> Dcal = boost::none, OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ; OptionalJacobian<2, 2> Dp = boost::none) const;
/// Conver a pixel coordinate to ideal coordinate /// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none, Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
@ -121,10 +121,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
/// @{ /// @{
/// Given delta vector, update calibration /// Given delta vector, update calibration
Cal3Unified retract(const Vector& d) const ; Cal3Unified retract(const Vector& d) const;
/// Given a different calibration, calculate update to obtain it /// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3Unified& T2) const ; Vector localCoordinates(const Cal3Unified& T2) const;
/// Return dimensions of calibration manifold object /// Return dimensions of calibration manifold object
virtual size_t dim() const override { return Dim(); } virtual size_t dim() const override { return Dim(); }
@ -134,25 +134,20 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
/// @} /// @}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template <class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) void serialize(Archive& ar, const unsigned int /*version*/) {
{ ar& boost::serialization::make_nvp(
ar & boost::serialization::make_nvp("Cal3Unified", "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
boost::serialization::base_object<Cal3DS2_Base>(*this)); ar& BOOST_SERIALIZATION_NVP(xi_);
ar & BOOST_SERIALIZATION_NVP(xi_);
} }
}; };
template<> template <>
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {}; struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
template<> template <>
struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {}; struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
} }

3
gtsam/geometry/Cal3_S2Stereo.cpp
View File

@ -38,7 +38,8 @@ void Cal3_S2Stereo::print(const std::string& s) const {
/* ************************************************************************* */ /* ************************************************************************* */
bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other); const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
return (Cal3_S2::equals(*base, tol) && std::fabs(b_ - other.baseline()) < tol); return (Cal3_S2::equals(*base, tol) &&
std::fabs(b_ - other.baseline()) < tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */

179
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -22,121 +22,116 @@
namespace gtsam { namespace gtsam {
/** /**
* @brief The most common 5DOF 3D->2D calibration, stereo version * @brief The most common 5DOF 3D->2D calibration, stereo version
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
private: private:
double b_ = 1.0f; ///< Stereo baseline. double b_ = 1.0f; ///< Stereo baseline.
public: public:
enum { dimension = 6 };
enum { dimension = 6 }; ///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
///< shared pointer to stereo calibration object /// @name Standard Constructors
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>; /// @
/// @name Standard Constructors /// default calibration leaves coordinates unchanged
/// @ Cal3_S2Stereo() = default;
/// default calibration leaves coordinates unchanged /// constructor from doubles
Cal3_S2Stereo() = default; Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
: Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
/// constructor from doubles /// constructor from vector
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, Cal3_S2Stereo(const Vector6& d)
double b) : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
: Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
/// constructor from vector /// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(const Vector6& d) Cal3_S2Stereo(double fov, int w, int h, double b)
: Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {} : Cal3_S2(fov, w, h), b_(b) {}
/// easy constructor; field-of-view in degrees, assumes zero skew /// @}
Cal3_S2Stereo(double fov, int w, int h, double b) /// @name Testable
: Cal3_S2(fov, w, h), b_(b) {} /// @{
/// @} /// Output stream operator
/// @name Testable GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
/// @{ const Cal3_S2Stereo& cal);
/// Output stream operator /// print with optional string
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, void print(const std::string& s = "") const override;
const Cal3_S2Stereo& cal);
/// print with optional string /// Check if equal up to specified tolerance
void print(const std::string& s = "") const override; bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
/// Check if equal up to specified tolerance /// @}
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; /// @name Standard Interface
/// @{
/// @} /// return calibration, same for left and right
/// @name Standard Interface const Cal3_S2& calibration() const { return *this; }
/// @{
/// return calibration, same for left and right /// return calibration matrix K, same for left and right
const Cal3_S2& calibration() const { return *this; } Matrix3 K() const override { return Cal3_S2::K(); }
/// return calibration matrix K, same for left and right /// return baseline
Matrix3 K() const override { return Cal3_S2::K(); } inline double baseline() const { return b_; }
/// return baseline /// vectorized form (column-wise)
inline double baseline() const { return b_; } Vector6 vector() const {
Vector6 v;
v << Cal3_S2::vector(), b_;
return v;
}
/// vectorized form (column-wise) /// @}
Vector6 vector() const { /// @name Manifold
Vector6 v; /// @{
v << Cal3_S2::vector(), b_;
return v;
}
/// @} /// return DOF, dimensionality of tangent space
/// @name Manifold inline size_t dim() const override { return Dim(); }
/// @{
/// return DOF, dimensionality of tangent space /// return DOF, dimensionality of tangent space
inline size_t dim() const override { return Dim(); } inline static size_t Dim() { return dimension; }
/// return DOF, dimensionality of tangent space /// Given 6-dim tangent vector, create new calibration
inline static size_t Dim() { return dimension; } inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
py() + d(4), b_ + d(5));
}
/// Given 6-dim tangent vector, create new calibration /// Unretraction for the calibration
inline Cal3_S2Stereo retract(const Vector& d) const { Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3), return T2.vector() - vector();
py() + d(4), b_ + d(5)); }
}
/// Unretraction for the calibration /// @}
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { /// @name Advanced Interface
return T2.vector() - vector(); /// @{
}
/// @} private:
/// @name Advanced Interface /** Serialization function */
/// @{ friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
ar& BOOST_SERIALIZATION_NVP(b_);
}
/// @}
};
private: // Define GTSAM traits
/** Serialization function */ template <>
friend class boost::serialization::access; struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
ar& BOOST_SERIALIZATION_NVP(b_);
}
/// @}
}; template <>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
// Define GTSAM traits } // \ namespace gtsam
template<>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
template<>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam

47
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -26,27 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3); static Point2 p(2, 3);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Bundler, vector) { TEST(Cal3Bundler, vector) {
Cal3Bundler K; Cal3Bundler K;
Vector expected(3); Vector expected(3);
expected << 1, 0, 0; expected << 1, 0, 0;
CHECK(assert_equal(expected,K.vector())); CHECK(assert_equal(expected, K.vector()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Bundler, uncalibrate) { TEST(Cal3Bundler, uncalibrate) {
Vector v = K.vector() ; Vector v = K.vector();
double r = p.x()*p.x() + p.y()*p.y() ; double r = p.x() * p.x() + p.y() * p.y();
double g = v[0]*(1+v[1]*r+v[2]*r*r) ; double g = v[0] * (1 + v[1] * r + v[2] * r * r);
Point2 expected (1000+g*p.x(), 2000+g*p.y()) ; Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
Point2 actual = K.uncalibrate(p); Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected, actual));
} }
TEST(Cal3Bundler, calibrate ) { TEST(Cal3Bundler, calibrate) {
Point2 pn(0.5, 0.5); Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn); Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi); Point2 pn_hat = K.calibrate(pi);
@ -54,20 +54,24 @@ TEST(Cal3Bundler, calibrate ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.uncalibrate(pt);
}
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate) { TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp; Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp); Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773); Point2 expected(2182, 3773);
CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1,Dcal,1e-7)); CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2,Dp,1e-7)); CHECK(assert_equal(numerical2, Dp, 1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -79,14 +83,12 @@ TEST(Cal3Bundler, Dcalibrate) {
CHECK(assert_equal(pn, actual, 1e-7)); CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5)); CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2,Dp,1e-5)); CHECK(assert_equal(numerical2, Dp, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Bundler, assert_equal) { TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
CHECK(assert_equal(K,K,1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Bundler, retract) { TEST(Cal3Bundler, retract) {
@ -99,8 +101,8 @@ TEST(Cal3Bundler, retract) {
Vector3 d; Vector3 d;
d << 10, 1e-3, 1e-3; d << 10, 1e-3, 1e-3;
Cal3Bundler actual = K.retract(d); Cal3Bundler 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -114,5 +116,8 @@ TEST(Cal3_S2, Print) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

7
gtsam/geometry/tests/testCal3DFisheye.cpp
View File

@ -191,16 +191,15 @@ Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Fisheye, Dcalibrate) TEST(Cal3Fisheye, Dcalibrate) {
{
Point2 p(0.5, 0.5); Point2 p(0.5, 0.5);
Point2 pi = K.uncalibrate(p); Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp; Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp); K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5)); CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2,Dp,1e-5)); CHECK(assert_equal(numerical2, Dp, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */

73
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -14,7 +14,6 @@
* @brief Unit tests for Cal3DS2 calibration model. * @brief Unit tests for Cal3DS2 calibration model.
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
@ -26,53 +25,53 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2) GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2) GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
static Point2 p(2,3); 4.0 * 1e-3);
static Point2 p(2, 3);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, uncalibrate) TEST(Cal3DS2, Uncalibrate) {
{ Vector k = K.k();
Vector k = K.k() ; double r = p.x() * p.x() + p.y() * p.y();
double r = p.x()*p.x() + p.y()*p.y() ; double g = 1 + k[0] * r + k[1] * r * r;
double g = 1+k[0]*r+k[1]*r*r ; double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x());
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y();
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished();
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished(); Vector v_i = K.K() * v_hat;
Vector v_i = K.K() * v_hat ; Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2));
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
Point2 q = K.uncalibrate(p); Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i)); CHECK(assert_equal(q, p_i));
} }
TEST(Cal3DS2, calibrate ) TEST(Cal3DS2, Calibrate) {
{
Point2 pn(0.5, 0.5); Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn); Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi); Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5)); CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
} }
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, Duncalibrate1) TEST(Cal3DS2, 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-5));
Matrix separate = K.D2d_calibration(p); Matrix separate = K.D2d_calibration(p);
CHECK(assert_equal(numerical,separate,1e-5)); CHECK(assert_equal(numerical, separate, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, Duncalibrate2) TEST(Cal3DS2, 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-5));
Matrix separate = K.D2d_intrinsic(p); Matrix separate = K.D2d_intrinsic(p);
CHECK(assert_equal(numerical,separate,1e-5)); CHECK(assert_equal(numerical, separate, 1e-5));
} }
Point2 calibrate_(const Cal3DS2& k, const Point2& pt) { Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
@ -80,8 +79,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, Dcalibrate) TEST(Cal3DS2, Dcalibrate) {
{
Point2 pn(0.5, 0.5); Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn); Point2 pi = K.uncalibrate(pn);
Matrix Dcal, Dp; Matrix Dcal, Dp;
@ -93,21 +91,21 @@ TEST(Cal3DS2, Dcalibrate)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, retract) { TEST(Cal3DS2, Retract) {
Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6, Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9); 2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9); EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
EXPECT_LONGS_EQUAL(expected.dim(), 9); EXPECT_LONGS_EQUAL(expected.dim(), 9);
Vector9 d; Vector9 d;
d << 1,2,3,4,5,6,7,8,9; d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3DS2 actual = K.retract(d); Cal3DS2 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -122,5 +120,8 @@ TEST(Cal3DS2, Print) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

67
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -20,8 +20,8 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unified.h> #include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
using namespace gtsam; using namespace gtsam;
@ -36,51 +36,49 @@ 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 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 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); static Point2 p(0.5, 0.7);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, uncalibrate) TEST(Cal3Unified, Uncalibrate) {
{ Point2 p_i(364.7791831734982, 305.6677211952602);
Point2 p_i(364.7791831734982, 305.6677211952602) ;
Point2 q = K.uncalibrate(p); Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i)); CHECK(assert_equal(q, p_i));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, spaceNplane) TEST(Cal3Unified, SpaceNplane) {
{
Point2 q = K.spaceToNPlane(p); Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
CHECK(assert_equal(p, K.nPlaneToSpace(q))); CHECK(assert_equal(p, K.nPlaneToSpace(q)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, calibrate) TEST(Cal3Unified, Calibrate) {
{
Point2 pi = K.uncalibrate(p); Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi); Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8)); CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
} }
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, Duncalibrate1) TEST(Cal3Unified, 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-6)); CHECK(assert_equal(numerical, computed, 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, Duncalibrate2) TEST(Cal3Unified, 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-6)); CHECK(assert_equal(numerical, computed, 1e-6));
} }
Point2 calibrate_(const Cal3Unified& k, const Point2& pt) { Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
@ -88,27 +86,24 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, Dcalibrate) TEST(Cal3Unified, Dcalibrate) {
{
Point2 pi = K.uncalibrate(p); Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp; Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp); K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5)); CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2,Dp,1e-5)); CHECK(assert_equal(numerical2, Dp, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, assert_equal) TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
{
CHECK(assert_equal(K,K,1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, retract) { TEST(Cal3Unified, Retract) {
Cal3Unified 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,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); 2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
0.1 + 1);
EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10); EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
EXPECT_LONGS_EQUAL(expected.dim(), 10); EXPECT_LONGS_EQUAL(expected.dim(), 10);
@ -116,13 +111,12 @@ TEST(Cal3Unified, retract) {
Vector10 d; Vector10 d;
d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
Cal3Unified actual = K.retract(d); Cal3Unified actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-9)); CHECK(assert_equal(expected, actual, 1e-9));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, DerivedValue) TEST(Cal3Unified, DerivedValue) {
{
Values values; Values values;
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10); Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
Key key = 1; Key key = 1;
@ -130,7 +124,7 @@ TEST(Cal3Unified, DerivedValue)
Cal3Unified calafter = values.at<Cal3Unified>(key); Cal3Unified calafter = values.at<Cal3Unified>(key);
CHECK(assert_equal(cal,calafter,1e-9)); CHECK(assert_equal(cal, calafter, 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -146,5 +140,8 @@ TEST(Cal3Unified, Print) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

61
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -34,27 +34,27 @@ static Point2 p_xy(2, 3);
TEST(Cal3_S2, Constructor) { TEST(Cal3_S2, Constructor) {
Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2); Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
double fov = 60; // degrees double fov = 60; // degrees
size_t w=640,h=480; size_t w = 640, h = 480;
Cal3_S2 actual(fov,w,h); Cal3_S2 actual(fov, w, h);
CHECK(assert_equal(expected,actual,1e-3)); CHECK(assert_equal(expected, actual, 1e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Calibrate) { TEST(Cal3_S2, Calibrate) {
Point2 intrinsic(2,3); Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740); Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic); Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage,imagecoordinates)); CHECK(assert_equal(expectedimage, imagecoordinates));
CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates))); CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, CalibrateHomogeneous) { TEST(Cal3_S2, CalibrateHomogeneous) {
Vector3 intrinsic(2, 3, 1); Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1); Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image))); CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -63,16 +63,18 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
} }
TEST(Cal3_S2, Duncalibrate1) { TEST(Cal3_S2, Duncalibrate1) {
Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix25 computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p); Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8)); CHECK(assert_equal(numerical, computed, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Duncalibrate2) { TEST(Cal3_S2, Duncalibrate2) {
Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p); Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical, computed, 1e-9));
} }
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) { Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
@ -81,42 +83,42 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1) { TEST(Cal3_S2, Dcalibrate1) {
Matrix computed; Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none); Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2) { TEST(Cal3_S2, Dcalibrate2) {
Matrix computed; Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed); Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Equal) { TEST(Cal3_S2, Equal) {
CHECK(assert_equal(K,K,1e-9)); CHECK(assert_equal(K, K, 1e-9));
Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2); Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
CHECK(assert_equal(K,K1,1e-9)); CHECK(assert_equal(K, K1, 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Retract) { TEST(Cal3_S2, Retract) {
Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5); Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5); EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
EXPECT_LONGS_EQUAL(expected.dim(), 5); EXPECT_LONGS_EQUAL(expected.dim(), 5);
Vector5 d; Vector5 d;
d << 1,2,3,4,5; d << 1, 2, 3, 4, 5;
Cal3_S2 actual = K.retract(d); Cal3_S2 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -124,7 +126,7 @@ TEST(Cal3_S2, between) {
Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9); Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
Matrix H1, H2; Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-I_5x5, H1)); EXPECT(assert_equal(-I_5x5, H1));
EXPECT(assert_equal(I_5x5, H2)); EXPECT(assert_equal(I_5x5, H2));
} }
@ -145,4 +147,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */

52
gtsam/geometry/tests/testCal3_S2Stereo.cpp
View File

@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3); static Point2 p_xy(2, 3);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2Stereo, easy_constructor) { TEST(Cal3_S2Stereo, Constructor) {
Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3); Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
double fov = 60; // degrees double fov = 60; // degrees
@ -42,7 +42,7 @@ TEST(Cal3_S2Stereo, easy_constructor) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2Stereo, calibrate) { TEST(Cal3_S2Stereo, Calibrate) {
Point2 intrinsic(2, 3); Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740); Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic); Point2 imagecoordinates = K.uncalibrate(intrinsic);
@ -51,56 +51,14 @@ TEST(Cal3_S2Stereo, calibrate) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2Stereo, calibrate_homogeneous) { TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
Vector3 intrinsic(2, 3, 1); Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1); Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image))); CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
} }
//TODO(Varun) Add calibrate and uncalibrate methods
// /* ************************************************************************* */
// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
// return k.uncalibrate(pt);
// }
// TEST(Cal3_S2Stereo, Duncalibrate1) {
// Matrix26 computed;
// K.uncalibrate(p, computed, boost::none);
// Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
// CHECK(assert_equal(numerical, computed, 1e-8));
// }
// /* ************************************************************************* */
// TEST(Cal3_S2Stereo, Duncalibrate2) {
// Matrix computed;
// K.uncalibrate(p, boost::none, computed);
// Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
// CHECK(assert_equal(numerical, computed, 1e-9));
// }
// Point2 calibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
// return k.calibrate(pt);
// }
// /* ************************************************************************* */
// TEST(Cal3_S2Stereo, Dcalibrate1) {
// Matrix computed;
// Point2 expected = K.calibrate(p_uv, computed, boost::none);
// Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
// CHECK(assert_equal(expected, p_xy, 1e-8));
// CHECK(assert_equal(numerical, computed, 1e-8));
// }
// /* ************************************************************************* */
// TEST(Cal3_S2Stereo, Dcalibrate2) {
// Matrix computed;
// Point2 expected = K.calibrate(p_uv, boost::none, computed);
// Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
// CHECK(assert_equal(expected, p_xy, 1e-8));
// CHECK(assert_equal(numerical, computed, 1e-8));
// }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2Stereo, assert_equal) { TEST(Cal3_S2Stereo, Equal) {
CHECK(assert_equal(K, K, 1e-9)); CHECK(assert_equal(K, K, 1e-9));
Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1); Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
@ -108,7 +66,7 @@ TEST(Cal3_S2Stereo, assert_equal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2Stereo, retract) { TEST(Cal3_S2Stereo, Retract) {
Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5, Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
7); 7);
EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6); EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);