Consistent and better formatting
Varun Agrawal
parent
0a55d31702
commit
70bab8e00c
|
|
@ -15,11 +15,11 @@
|
|||
* @author ydjian
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -39,9 +39,7 @@ Vector4 Cal3Bundler::k() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Cal3Bundler::vector() const {
|
||||
return Vector3(fx_, k1_, k2_);
|
||||
}
|
||||
Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 {
|
||||
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, //
|
||||
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
|
||||
Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// r = x² + y²;
|
||||
// g = (1 + k(1)*r + k(2)*r²);
|
||||
// pi(:,i) = g * pn(:,i)
|
||||
|
|
@ -93,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Bundler::calibrate(const Point2& pi,
|
||||
OptionalJacobian<2, 3> Dcal,
|
||||
Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// Copied from Cal3DS2
|
||||
// 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_;
|
||||
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);
|
||||
|
||||
// iterate until the uncalibrate is close to the actual pixel coordinate
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||
if (distance2(uncalibrate(pn), pi) <= tol_)
|
||||
break;
|
||||
if (distance2(uncalibrate(pn), pi) <= tol_) break;
|
||||
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
|
|
@ -118,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
|
|||
|
||||
if (iteration >= maxIterations)
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@ namespace gtsam {
|
|||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
||||
|
||||
private:
|
||||
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
|
||||
double tol_ = 1e-5; ///< tolerance value when calibrating
|
||||
|
|
@ -40,7 +39,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
// but are treated as constants.
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
|
|
@ -83,40 +81,28 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
/// @{
|
||||
|
||||
/// distorsion parameter k1
|
||||
inline double k1() const {
|
||||
return k1_;
|
||||
}
|
||||
inline double k1() const { return k1_; }
|
||||
|
||||
/// distorsion parameter k2
|
||||
inline double k2() const {
|
||||
return k2_;
|
||||
}
|
||||
inline double k2() const { return k2_; }
|
||||
|
||||
/// image center in x
|
||||
inline double px() const {
|
||||
return u0_;
|
||||
}
|
||||
inline double px() const { return u0_; }
|
||||
|
||||
/// image center in y
|
||||
inline double py() const {
|
||||
return v0_;
|
||||
}
|
||||
inline double py() const { return v0_; }
|
||||
|
||||
Matrix3 K() const override; ///< Standard 3*3 calibration matrix
|
||||
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
|
||||
Matrix3 K() const override; ///< Standard 3*3 calibration matrix
|
||||
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
|
||||
|
||||
Vector3 vector() const;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
/// get parameter u0
|
||||
inline double u0() const {
|
||||
return u0_;
|
||||
}
|
||||
inline double u0() const { return u0_; }
|
||||
|
||||
/// get parameter v0
|
||||
inline double v0() const {
|
||||
return v0_;
|
||||
}
|
||||
inline double v0() const { return v0_; }
|
||||
#endif
|
||||
|
||||
/**
|
||||
|
|
@ -128,7 +114,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
* @return point in image coordinates
|
||||
*/
|
||||
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
|
||||
|
|
@ -138,8 +124,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& pi,
|
||||
OptionalJacobian<2, 3> Dcal = boost::none,
|
||||
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
|
|
@ -172,15 +157,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(k1_);
|
||||
|
|
@ -189,13 +173,12 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,11 +16,11 @@
|
|||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -31,9 +31,7 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3DS2::print(const std::string& s_) const {
|
||||
Base::print(s_);
|
||||
}
|
||||
void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -11,7 +11,8 @@
|
|||
|
||||
/**
|
||||
* @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
|
||||
* @author ydjian
|
||||
* @autho Varun Agrawal
|
||||
|
|
@ -31,11 +32,9 @@ namespace gtsam {
|
|||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
|
||||
|
||||
using Base = Cal3DS2_Base;
|
||||
|
||||
public:
|
||||
|
||||
public:
|
||||
enum { dimension = 9 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
|
|
@ -54,7 +53,7 @@ public:
|
|||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3DS2(const Vector9 &v) : Base(v) {}
|
||||
Cal3DS2(const Vector9& v) : Base(v) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -75,10 +74,10 @@ public:
|
|||
/// @{
|
||||
|
||||
/// 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
|
||||
Vector localCoordinates(const Cal3DS2& T2) const ;
|
||||
Vector localCoordinates(const Cal3DS2& T2) const;
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
virtual size_t dim() const override { return Dim(); }
|
||||
|
|
@ -97,30 +96,24 @@ public:
|
|||
|
||||
/// @}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
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("Cal3DS2",
|
||||
boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -16,11 +16,11 @@
|
|||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.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/Point3.h>
|
||||
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||
|
||||
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,
|
||||
double yy, double xy, double rr, double r4, double pnx, double pny,
|
||||
const Matrix2& DK) {
|
||||
static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
|
||||
double xy, double rr, double r4, double pnx,
|
||||
double pny, const Matrix2& DK) {
|
||||
Matrix25 DR1;
|
||||
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
|
||||
Matrix24 DR2;
|
||||
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
|
||||
y * rr, y * r4, rr + 2 * yy, 2 * xy;
|
||||
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
|
||||
y * rr, y * r4, rr + 2 * yy, 2 * xy;
|
||||
Matrix29 D;
|
||||
D << DR1, DK * DR2;
|
||||
return D;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix2 D2dintrinsic(double x, double y, double rr,
|
||||
double g, double k1, double k2, double p1, double p2,
|
||||
const Matrix2& DK) {
|
||||
static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
|
||||
double k2, double p1, double p2,
|
||||
const Matrix2& DK) {
|
||||
const double drdx = 2. * x;
|
||||
const double drdy = 2. * y;
|
||||
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);
|
||||
|
||||
Matrix2 DR;
|
||||
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
|
||||
y * dgdx + dDydx, g + y * dgdy + dDydy;
|
||||
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
|
||||
y * dgdx + dDydx, g + y * dgdy + dDydy;
|
||||
|
||||
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 rr = xx + yy;
|
||||
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
|
||||
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_;
|
||||
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -28,20 +28,21 @@
|
|||
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
|
||||
* \nosubgrouping
|
||||
*
|
||||
* Similar to Cal3DS2, does distortion but has additional mirror parameter xi
|
||||
* 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²
|
||||
* \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 ]
|
||||
* pi = K*pn
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
|
||||
|
||||
using This = Cal3Unified;
|
||||
using Base = Cal3DS2_Base;
|
||||
|
||||
|
|
@ -49,7 +50,6 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
|
|||
double xi_ = 0.0f; ///< mirror parameter
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 10 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
|
|
@ -90,10 +90,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
|
|||
/// @{
|
||||
|
||||
/// mirror parameter
|
||||
inline double xi() const { return xi_;}
|
||||
inline double xi() const { return xi_; }
|
||||
|
||||
/// Return all parameters as a vector
|
||||
Vector10 vector() const ;
|
||||
Vector10 vector() const;
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
|
|
@ -103,8 +103,8 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
|
|||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,10> Dcal = boost::none,
|
||||
OptionalJacobian<2,2> Dp = boost::none) const ;
|
||||
OptionalJacobian<2, 10> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
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
|
||||
Cal3Unified retract(const Vector& d) const ;
|
||||
Cal3Unified retract(const Vector& d) const;
|
||||
|
||||
/// 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
|
||||
virtual size_t dim() const override { return Dim(); }
|
||||
|
|
@ -134,25 +134,20 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
|
|||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/)
|
||||
{
|
||||
ar & boost::serialization::make_nvp("Cal3Unified",
|
||||
boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(xi_);
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(xi_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,8 @@ void Cal3_S2Stereo::print(const std::string& s) const {
|
|||
/* ************************************************************************* */
|
||||
bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -22,121 +22,116 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration, stereo version
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
|
||||
private:
|
||||
double b_ = 1.0f; ///< Stereo baseline.
|
||||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration, stereo version
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
|
||||
private:
|
||||
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
|
||||
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
|
||||
/// @name Standard Constructors
|
||||
/// @
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @
|
||||
/// default calibration leaves coordinates unchanged
|
||||
Cal3_S2Stereo() = default;
|
||||
|
||||
/// default calibration leaves coordinates unchanged
|
||||
Cal3_S2Stereo() = default;
|
||||
/// constructor from doubles
|
||||
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
|
||||
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 vector
|
||||
Cal3_S2Stereo(const Vector6& d)
|
||||
: Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
|
||||
|
||||
/// constructor from vector
|
||||
Cal3_S2Stereo(const Vector6& d)
|
||||
: Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
|
||||
/// easy constructor; field-of-view in degrees, assumes zero skew
|
||||
Cal3_S2Stereo(double fov, int w, int h, double b)
|
||||
: 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)
|
||||
: Cal3_S2(fov, w, h), b_(b) {}
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
|
||||
const Cal3_S2Stereo& cal);
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
|
||||
const Cal3_S2Stereo& cal);
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const override;
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const override;
|
||||
/// Check if equal up to specified tolerance
|
||||
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
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
/// return calibration, same for left and right
|
||||
const Cal3_S2& calibration() const { return *this; }
|
||||
|
||||
/// return calibration, same for left and right
|
||||
const Cal3_S2& calibration() const { return *this; }
|
||||
/// return calibration matrix K, same for left and right
|
||||
Matrix3 K() const override { return Cal3_S2::K(); }
|
||||
|
||||
/// return calibration matrix K, same for left and right
|
||||
Matrix3 K() const override { return Cal3_S2::K(); }
|
||||
/// return baseline
|
||||
inline double baseline() const { return b_; }
|
||||
|
||||
/// return baseline
|
||||
inline double baseline() const { return b_; }
|
||||
/// vectorized form (column-wise)
|
||||
Vector6 vector() const {
|
||||
Vector6 v;
|
||||
v << Cal3_S2::vector(), b_;
|
||||
return v;
|
||||
}
|
||||
|
||||
/// vectorized form (column-wise)
|
||||
Vector6 vector() const {
|
||||
Vector6 v;
|
||||
v << Cal3_S2::vector(), b_;
|
||||
return v;
|
||||
}
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline size_t dim() const override { return Dim(); }
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline size_t dim() const override { return Dim(); }
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline static size_t Dim() { return dimension; }
|
||||
/// Given 6-dim tangent vector, create new calibration
|
||||
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
|
||||
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));
|
||||
}
|
||||
/// Unretraction for the calibration
|
||||
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
/// Unretraction for the calibration
|
||||
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
private:
|
||||
/** 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:
|
||||
/** 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_);
|
||||
}
|
||||
/// @}
|
||||
// Define GTSAM traits
|
||||
template <>
|
||||
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
|
||||
|
||||
};
|
||||
template <>
|
||||
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
template<>
|
||||
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -26,27 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
|
|||
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
|
||||
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
|
||||
static Point2 p(2,3);
|
||||
static Point2 p(2, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, vector) {
|
||||
Cal3Bundler K;
|
||||
Vector expected(3);
|
||||
expected << 1, 0, 0;
|
||||
CHECK(assert_equal(expected,K.vector()));
|
||||
CHECK(assert_equal(expected, K.vector()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, uncalibrate) {
|
||||
Vector v = K.vector() ;
|
||||
double r = p.x()*p.x() + p.y()*p.y() ;
|
||||
double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
|
||||
Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
|
||||
Vector v = K.vector();
|
||||
double r = p.x() * p.x() + p.y() * p.y();
|
||||
double g = v[0] * (1 + v[1] * r + v[2] * r * r);
|
||||
Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
|
||||
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 pi = K.uncalibrate(pn);
|
||||
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) {
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = K.uncalibrate(p, Dcal, Dp);
|
||||
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 numerical2 = numericalDerivative22(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical1,Dcal,1e-7));
|
||||
CHECK(assert_equal(numerical2,Dp,1e-7));
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-7));
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -79,14 +83,12 @@ TEST(Cal3Bundler, Dcalibrate) {
|
|||
CHECK(assert_equal(pn, actual, 1e-7));
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical1,Dcal,1e-5));
|
||||
CHECK(assert_equal(numerical2,Dp,1e-5));
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-5));
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, assert_equal) {
|
||||
CHECK(assert_equal(K,K,1e-7));
|
||||
}
|
||||
TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, retract) {
|
||||
|
|
@ -99,8 +101,8 @@ TEST(Cal3Bundler, retract) {
|
|||
Vector3 d;
|
||||
d << 10, 1e-3, 1e-3;
|
||||
Cal3Bundler actual = K.retract(d);
|
||||
CHECK(assert_equal(expected,actual,1e-7));
|
||||
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
|
||||
CHECK(assert_equal(expected, 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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 pi = K.uncalibrate(p);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
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);
|
||||
CHECK(assert_equal(numerical2,Dp,1e-5));
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -14,7 +14,6 @@
|
|||
* @brief Unit tests for Cal3DS2 calibration model.
|
||||
*/
|
||||
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
|
@ -26,53 +25,53 @@ using namespace gtsam;
|
|||
GTSAM_CONCEPT_TESTABLE_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 Point2 p(2,3);
|
||||
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
|
||||
4.0 * 1e-3);
|
||||
static Point2 p(2, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3DS2, uncalibrate)
|
||||
{
|
||||
Vector k = K.k() ;
|
||||
double r = p.x()*p.x() + p.y()*p.y() ;
|
||||
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 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_i = K.K() * v_hat ;
|
||||
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
|
||||
TEST(Cal3DS2, Uncalibrate) {
|
||||
Vector k = K.k();
|
||||
double r = p.x() * p.x() + p.y() * p.y();
|
||||
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 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_i = K.K() * v_hat;
|
||||
Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2));
|
||||
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 pi = K.uncalibrate(pn);
|
||||
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;
|
||||
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-5));
|
||||
Matrix separate = K.D2d_calibration(p);
|
||||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
CHECK(assert_equal(numerical, separate, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3DS2, Duncalibrate2)
|
||||
{
|
||||
Matrix computed; K.uncalibrate(p, boost::none, computed);
|
||||
TEST(Cal3DS2, 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-5));
|
||||
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) {
|
||||
|
|
@ -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 pi = K.uncalibrate(pn);
|
||||
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,
|
||||
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(expected.dim(), 9);
|
||||
|
||||
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);
|
||||
CHECK(assert_equal(expected,actual,1e-7));
|
||||
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
|
||||
CHECK(assert_equal(expected, 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -20,8 +20,8 @@
|
|||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
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
|
||||
*/
|
||||
|
||||
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);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, uncalibrate)
|
||||
{
|
||||
Point2 p_i(364.7791831734982, 305.6677211952602) ;
|
||||
TEST(Cal3Unified, Uncalibrate) {
|
||||
Point2 p_i(364.7791831734982, 305.6677211952602);
|
||||
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);
|
||||
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
|
||||
CHECK(assert_equal(p, K.nPlaneToSpace(q)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, calibrate)
|
||||
{
|
||||
TEST(Cal3Unified, Calibrate) {
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
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;
|
||||
K.uncalibrate(p, computed, boost::none);
|
||||
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;
|
||||
K.uncalibrate(p, boost::none, computed);
|
||||
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) {
|
||||
|
|
@ -88,27 +86,24 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, Dcalibrate)
|
||||
{
|
||||
TEST(Cal3Unified, Dcalibrate) {
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
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);
|
||||
CHECK(assert_equal(numerical2,Dp,1e-5));
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, assert_equal)
|
||||
{
|
||||
CHECK(assert_equal(K,K,1e-9));
|
||||
}
|
||||
TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, retract) {
|
||||
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);
|
||||
TEST(Cal3Unified, Retract) {
|
||||
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);
|
||||
|
||||
EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
|
||||
EXPECT_LONGS_EQUAL(expected.dim(), 10);
|
||||
|
|
@ -116,13 +111,12 @@ TEST(Cal3Unified, retract) {
|
|||
Vector10 d;
|
||||
d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
|
||||
Cal3Unified actual = K.retract(d);
|
||||
CHECK(assert_equal(expected,actual,1e-9));
|
||||
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
|
||||
CHECK(assert_equal(expected, actual, 1e-9));
|
||||
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, DerivedValue)
|
||||
{
|
||||
TEST(Cal3Unified, DerivedValue) {
|
||||
Values values;
|
||||
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
|
||||
Key key = 1;
|
||||
|
|
@ -130,7 +124,7 @@ TEST(Cal3Unified, DerivedValue)
|
|||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -34,27 +34,27 @@ static Point2 p_xy(2, 3);
|
|||
TEST(Cal3_S2, Constructor) {
|
||||
Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
|
||||
|
||||
double fov = 60; // degrees
|
||||
size_t w=640,h=480;
|
||||
Cal3_S2 actual(fov,w,h);
|
||||
double fov = 60; // degrees
|
||||
size_t w = 640, h = 480;
|
||||
Cal3_S2 actual(fov, w, h);
|
||||
|
||||
CHECK(assert_equal(expected,actual,1e-3));
|
||||
CHECK(assert_equal(expected, actual, 1e-3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3_S2, Calibrate) {
|
||||
Point2 intrinsic(2,3);
|
||||
Point2 intrinsic(2, 3);
|
||||
Point2 expectedimage(1320.3, 1740);
|
||||
Point2 imagecoordinates = K.uncalibrate(intrinsic);
|
||||
CHECK(assert_equal(expectedimage,imagecoordinates));
|
||||
CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
|
||||
CHECK(assert_equal(expectedimage, imagecoordinates));
|
||||
CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3_S2, CalibrateHomogeneous) {
|
||||
Vector3 intrinsic(2, 3, 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) {
|
||||
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
|
||||
Matrix25 computed;
|
||||
K.uncalibrate(p, computed, boost::none);
|
||||
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical,computed,1e-8));
|
||||
CHECK(assert_equal(numerical, computed, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
CHECK(assert_equal(numerical,computed,1e-9));
|
||||
CHECK(assert_equal(numerical, computed, 1e-9));
|
||||
}
|
||||
|
||||
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) {
|
||||
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));
|
||||
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_S2, 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));
|
||||
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_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);
|
||||
CHECK(assert_equal(K,K1,1e-9));
|
||||
CHECK(assert_equal(K, K1, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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(expected.dim(), 5);
|
||||
|
||||
Vector5 d;
|
||||
d << 1,2,3,4,5;
|
||||
d << 1, 2, 3, 4, 5;
|
||||
Cal3_S2 actual = K.retract(d);
|
||||
CHECK(assert_equal(expected,actual,1e-7));
|
||||
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
|
||||
CHECK(assert_equal(expected, 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);
|
||||
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, H2));
|
||||
}
|
||||
|
|
@ -145,4 +147,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740);
|
|||
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);
|
||||
|
||||
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 expectedimage(1320.3, 1740);
|
||||
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 image(1320.3, 1740, 1);
|
||||
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));
|
||||
|
||||
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,
|
||||
7);
|
||||
EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);
|
||||
|
|
|
|||
Loading…
Reference in New Issue