Refactor Cal3_S2 and Cal3_S2Stereo classes
Varun Agrawal
parent
04fb3390be
commit
ad66a5927d
|
|
@ -24,92 +24,51 @@
|
|||
namespace gtsam {
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(double fov, int w, int h) :
|
||||
s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
|
||||
double a = fov * M_PI / 360.0; // fov/2 in radians
|
||||
fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a);
|
||||
fy_ = fx_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(const std::string &path) :
|
||||
fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
|
||||
|
||||
char buffer[200];
|
||||
buffer[0] = 0;
|
||||
sprintf(buffer, "%s/calibration_info.txt", path.c_str());
|
||||
std::ifstream infile(buffer, std::ios::in);
|
||||
|
||||
if (infile)
|
||||
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
|
||||
else {
|
||||
throw std::runtime_error("Cal3_S2: Unable to load the calibration");
|
||||
}
|
||||
|
||||
infile.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream& operator<<(ostream& os, const Cal3_S2& cal) {
|
||||
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
|
||||
<< ", py:" << cal.py() << "}";
|
||||
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew()
|
||||
<< ", px:" << cal.px() << ", py:" << cal.py() << "}";
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3_S2::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
gtsam::print((Matrix)K(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
|
||||
if (std::abs(fx_ - K.fx_) > tol)
|
||||
return false;
|
||||
if (std::abs(fy_ - K.fy_) > tol)
|
||||
return false;
|
||||
if (std::abs(s_ - K.s_) > tol)
|
||||
return false;
|
||||
if (std::abs(u0_ - K.u0_) > tol)
|
||||
return false;
|
||||
if (std::abs(v0_ - K.v0_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
return Cal3::equals(K, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
const double x = p.x(), y = p.y();
|
||||
if (Dcal)
|
||||
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
|
||||
if (Dp)
|
||||
*Dp << fx_, s_, 0.0, fy_;
|
||||
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
|
||||
if (Dp) *Dp << fx_, s_, 0.0, fy_;
|
||||
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
|
||||
OptionalJacobian<2,2> Dp) const {
|
||||
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
const double u = p.x(), v = p.y();
|
||||
double delta_u = u - u0_, delta_v = v - v0_;
|
||||
double inv_fx = 1/ fx_, inv_fy = 1/fy_;
|
||||
double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
|
||||
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
|
||||
inv_fy_delta_v);
|
||||
if(Dcal)
|
||||
*Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(),
|
||||
-inv_fx, inv_fx_s_inv_fy,
|
||||
0, -inv_fy * point.y(), 0, 0, -inv_fy;
|
||||
if(Dp)
|
||||
*Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
|
||||
double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
|
||||
double inv_fy_delta_v = inv_fy * delta_v,
|
||||
inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
|
||||
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
|
||||
if (Dcal)
|
||||
*Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
|
||||
-inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
|
||||
0, 0, -inv_fy;
|
||||
if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
|
||||
return point;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Cal3_S2::calibrate(const Vector3& p) const {
|
||||
return matrix_inverse() * p;
|
||||
}
|
||||
Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -30,31 +31,24 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3_S2 {
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_;
|
||||
|
||||
public:
|
||||
class GTSAM_EXPORT Cal3_S2 : public Cal3 {
|
||||
public:
|
||||
enum { dimension = 5 };
|
||||
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
|
||||
typedef boost::shared_ptr<Cal3_S2>
|
||||
shared_ptr; ///< shared pointer to calibration object
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Create a default calibration that leaves coordinates unchanged
|
||||
Cal3_S2() :
|
||||
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
|
||||
}
|
||||
Cal3_S2() : Cal3() {}
|
||||
|
||||
/// constructor from doubles
|
||||
Cal3_S2(double fx, double fy, double s, double u0, double v0) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
|
||||
}
|
||||
Cal3_S2(double fx, double fy, double s, double u0, double v0)
|
||||
: Cal3(fx, fy, s, u0, v0) {}
|
||||
|
||||
/// constructor from vector
|
||||
Cal3_S2(const Vector &d) :
|
||||
fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
|
||||
}
|
||||
Cal3_S2(const Vector& d) : Cal3(d) {}
|
||||
|
||||
/**
|
||||
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
|
||||
|
|
@ -62,132 +56,59 @@ public:
|
|||
* @param w image width
|
||||
* @param h image height
|
||||
*/
|
||||
Cal3_S2(double fov, int w, int h);
|
||||
Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
/**
|
||||
* Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// load calibration from location (default name is calibration_info.txt)
|
||||
Cal3_S2(const std::string &path);
|
||||
/**
|
||||
* Convert image coordinates uv to intrinsic coordinates xy
|
||||
* @param p point in image coordinates
|
||||
* @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/**
|
||||
* Convert homogeneous image coordinates to intrinsic coordinates
|
||||
* @param p point in image coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Vector3 calibrate(const Vector3& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
|
||||
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
|
||||
const Cal3_S2& cal);
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "Cal3_S2") const;
|
||||
void print(const std::string& s = "Cal3_S2") const override;
|
||||
|
||||
/// Check if equal up to specified tolerance
|
||||
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const {
|
||||
return fx_;
|
||||
}
|
||||
|
||||
/// focal length y
|
||||
inline double fy() const {
|
||||
return fy_;
|
||||
}
|
||||
|
||||
/// aspect ratio
|
||||
inline double aspectRatio() const {
|
||||
return fx_/fy_;
|
||||
}
|
||||
|
||||
/// skew
|
||||
inline double skew() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// image center in x
|
||||
inline double px() const {
|
||||
return u0_;
|
||||
}
|
||||
|
||||
/// image center in y
|
||||
inline double py() const {
|
||||
return v0_;
|
||||
}
|
||||
|
||||
/// return the principal point
|
||||
Point2 principalPoint() const {
|
||||
return Point2(u0_, v0_);
|
||||
}
|
||||
|
||||
/// vectorized form (column-wise)
|
||||
Vector5 vector() const {
|
||||
Vector5 v;
|
||||
v << fx_, fy_, s_, u0_, v0_;
|
||||
return v;
|
||||
}
|
||||
|
||||
/// return calibration matrix K
|
||||
Matrix3 K() const {
|
||||
Matrix3 K;
|
||||
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
|
||||
return K;
|
||||
}
|
||||
|
||||
/** @deprecated The following function has been deprecated, use K above */
|
||||
Matrix3 matrix() const {
|
||||
return K();
|
||||
}
|
||||
|
||||
/// return inverted calibration matrix inv(K)
|
||||
Matrix3 matrix_inverse() const {
|
||||
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
|
||||
Matrix3 K_inverse;
|
||||
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
|
||||
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
|
||||
return K_inverse;
|
||||
}
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
|
||||
OptionalJacobian<2,2> Dp = boost::none) const;
|
||||
|
||||
/**
|
||||
* convert image coordinates uv to intrinsic coordinates xy
|
||||
* @param p point in image coordinates
|
||||
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
|
||||
OptionalJacobian<2,2> Dp = boost::none) const;
|
||||
|
||||
/**
|
||||
* convert homogeneous image coordinates to intrinsic coordinates
|
||||
* @param p point in image coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Vector3 calibrate(const Vector3& p) const;
|
||||
|
||||
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
|
||||
inline Cal3_S2 between(const Cal3_S2& q,
|
||||
OptionalJacobian<5,5> H1=boost::none,
|
||||
OptionalJacobian<5,5> H2=boost::none) const {
|
||||
if(H1) *H1 = -I_5x5;
|
||||
if(H2) *H2 = I_5x5;
|
||||
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
|
||||
OptionalJacobian<5, 5> H1 = boost::none,
|
||||
OptionalJacobian<5, 5> H2 = boost::none) const {
|
||||
if (H1) *H1 = -I_5x5;
|
||||
if (H2) *H2 = I_5x5;
|
||||
return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
|
||||
q.v0_ - v0_);
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
|
@ -212,27 +133,22 @@ public:
|
|||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
private:
|
||||
|
||||
private:
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(fx_);
|
||||
ar & BOOST_SERIALIZATION_NVP(fy_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3_S2", boost::serialization::base_object<Cal3>(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -24,14 +24,15 @@ using namespace std;
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Cal3_S2Stereo::print(const std::string& s) const {
|
||||
K_.print(s+"K: ");
|
||||
std::cout << s << "Baseline: " << b_ << std::endl;
|
||||
}
|
||||
std::cout << s << (s != "" ? " " : "");
|
||||
print("K: ");
|
||||
std::cout << "Baseline: " << b_ << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
|
||||
if (std::abs(b_ - other.b_) > tol) return false;
|
||||
return K_.equals(other.K_,tol);
|
||||
const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
|
||||
return Cal3_S2::equals(*base, tol) && (std::abs(b_ - other.baseline()) < tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -27,43 +27,40 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3_S2Stereo {
|
||||
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
|
||||
private:
|
||||
|
||||
Cal3_S2 K_;
|
||||
double b_;
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 };
|
||||
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
|
||||
///< shared pointer to stereo calibration object
|
||||
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @
|
||||
|
||||
/// default calibration leaves coordinates unchanged
|
||||
Cal3_S2Stereo() :
|
||||
K_(1, 1, 0, 0, 0), b_(1.0) {
|
||||
}
|
||||
Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {}
|
||||
|
||||
/// constructor from doubles
|
||||
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
|
||||
K_(fx, fy, s, u0, v0), b_(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
|
||||
Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
|
||||
Cal3_S2Stereo(const Vector& 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) :
|
||||
K_(fov, w, h), b_(b) {
|
||||
}
|
||||
Cal3_S2Stereo(double fov, int w, int h, double b)
|
||||
: Cal3_S2(fov, w, h), b_(b) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
void print(const std::string& s = "") const;
|
||||
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;
|
||||
|
|
@ -73,28 +70,10 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// return calibration, same for left and right
|
||||
const Cal3_S2& calibration() const { return K_;}
|
||||
const Cal3_S2& calibration() const { return *this; }
|
||||
|
||||
/// return calibration matrix K, same for left and right
|
||||
Matrix matrix() const { return K_.matrix();}
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const { return K_.fx();}
|
||||
|
||||
/// focal length x
|
||||
inline double fy() const { return K_.fy();}
|
||||
|
||||
/// skew
|
||||
inline double skew() const { return K_.skew();}
|
||||
|
||||
/// image center in x
|
||||
inline double px() const { return K_.px();}
|
||||
|
||||
/// image center in y
|
||||
inline double py() const { return K_.py();}
|
||||
|
||||
/// return the principal point
|
||||
Point2 principalPoint() const { return K_.principalPoint();}
|
||||
Matrix3 K() const { return K(); }
|
||||
|
||||
/// return baseline
|
||||
inline double baseline() const { return b_; }
|
||||
|
|
@ -102,7 +81,7 @@ namespace gtsam {
|
|||
/// vectorized form (column-wise)
|
||||
Vector6 vector() const {
|
||||
Vector6 v;
|
||||
v << K_.vector(), b_;
|
||||
v << vector(), b_;
|
||||
return v;
|
||||
}
|
||||
|
||||
|
|
@ -118,7 +97,8 @@ namespace gtsam {
|
|||
|
||||
/// Given 6-dim tangent vector, create new calibration
|
||||
inline Cal3_S2Stereo retract(const Vector& d) const {
|
||||
return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
|
||||
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
|
||||
|
|
@ -137,8 +117,9 @@ namespace gtsam {
|
|||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/)
|
||||
{
|
||||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
ar & BOOST_SERIALIZATION_NVP(b_);
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(b_);
|
||||
}
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -125,7 +125,6 @@ TEST(Cal3_S2, between) {
|
|||
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));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue