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,24 +81,16 @@ 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)
|
||||
|
|
@ -109,14 +99,10 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
|
||||
#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
|
||||
|
||||
/**
|
||||
|
|
@ -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,7 +157,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
|
@ -189,7 +173,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template <>
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
||||
enum { dimension = 9 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
|
|
@ -97,23 +96,19 @@ public:
|
|||
|
||||
/// @}
|
||||
|
||||
|
||||
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));
|
||||
void serialize(Archive& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template <>
|
||||
|
|
@ -121,6 +116,4 @@ struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
|
|||
|
||||
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,9 +54,9 @@ 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;
|
||||
|
|
@ -68,8 +68,8 @@ static Matrix29 D2dcalibration(double x, double y, double xx,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix2 D2dintrinsic(double x, double y, double rr,
|
||||
double g, double k1, double k2, double p1, double p2,
|
||||
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;
|
||||
|
|
@ -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
|
||||
|
|
@ -135,17 +135,14 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
|
|||
/// @}
|
||||
|
||||
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));
|
||||
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 <>
|
||||
|
|
@ -153,6 +150,4 @@ struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
|
|||
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -32,7 +32,6 @@ namespace gtsam {
|
|||
double b_ = 1.0f; ///< Stereo baseline.
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 };
|
||||
|
||||
///< shared pointer to stereo calibration object
|
||||
|
|
@ -45,8 +44,7 @@ namespace gtsam {
|
|||
Cal3_S2Stereo() = default;
|
||||
|
||||
/// constructor from doubles
|
||||
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0,
|
||||
double b)
|
||||
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
|
||||
|
|
@ -120,20 +118,17 @@ namespace gtsam {
|
|||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/)
|
||||
{
|
||||
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> {
|
||||
};
|
||||
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
|
||||
|
||||
template <>
|
||||
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
|
||||
|
|
|
|||
|
|
@ -54,9 +54,13 @@ 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) {
|
||||
|
|
@ -84,9 +88,7 @@ TEST(Cal3Bundler, Dcalibrate) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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) {
|
||||
|
|
@ -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,8 +191,7 @@ 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;
|
||||
|
|
|
|||
|
|
@ -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,12 +25,12 @@ 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 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)
|
||||
{
|
||||
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;
|
||||
|
|
@ -44,19 +43,19 @@ TEST(Cal3DS2, uncalibrate)
|
|||
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));
|
||||
}
|
||||
|
||||
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);
|
||||
|
|
@ -66,9 +65,9 @@ TEST(Cal3DS2, Duncalibrate1)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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));
|
||||
Matrix separate = K.D2d_intrinsic(p);
|
||||
|
|
@ -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,10 +91,10 @@ 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);
|
||||
|
||||
|
|
@ -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,38 +36,37 @@ 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)
|
||||
{
|
||||
TEST(Cal3Unified, Uncalibrate) {
|
||||
Point2 p_i(364.7791831734982, 305.6677211952602);
|
||||
Point2 q = K.uncalibrate(p);
|
||||
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));
|
||||
}
|
||||
|
||||
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);
|
||||
|
|
@ -75,8 +74,7 @@ TEST(Cal3Unified, Duncalibrate1)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, Duncalibrate2)
|
||||
{
|
||||
TEST(Cal3Unified, Duncalibrate2) {
|
||||
Matrix computed;
|
||||
K.uncalibrate(p, boost::none, computed);
|
||||
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
|
||||
|
|
@ -88,8 +86,7 @@ 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);
|
||||
|
|
@ -100,15 +97,13 @@ TEST(Cal3Unified, Dcalibrate)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
|
|
@ -121,8 +116,7 @@ TEST(Cal3Unified, retract) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Unified, DerivedValue)
|
||||
{
|
||||
TEST(Cal3Unified, DerivedValue) {
|
||||
Values values;
|
||||
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
|
||||
Key key = 1;
|
||||
|
|
@ -146,5 +140,8 @@ TEST(Cal3Unified, Print) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -63,14 +63,16 @@ 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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));
|
||||
}
|
||||
|
|
@ -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