110 lines
2.3 KiB
C++
110 lines
2.3 KiB
C++
/**
|
|
* @file Cal3_S2.h
|
|
* @brief The most common 5DOF 3D->2D calibration
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "Matrix.h"
|
|
#include "Point2.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/** The most common 5DOF 3D->2D calibration */
|
|
class Cal3_S2 {
|
|
private:
|
|
double fx_, fy_, s_, u0_, v0_;
|
|
|
|
public:
|
|
/**
|
|
* default calibration leaves coordinates unchanged
|
|
*/
|
|
Cal3_S2() :
|
|
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
|
|
}
|
|
|
|
/**
|
|
* 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) {
|
|
}
|
|
|
|
/**
|
|
* return DOF, dimensionality of tangent space
|
|
*/
|
|
size_t dim() const {
|
|
return 5;
|
|
}
|
|
|
|
/**
|
|
* load calibration from location (default name is calibration_info.txt)
|
|
*/
|
|
void load(std::string path);
|
|
|
|
/**
|
|
* Given 5-dim tangent vector, create new calibration
|
|
*/
|
|
Cal3_S2 exmap(const Vector& d) const {
|
|
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
|
|
}
|
|
|
|
/**
|
|
* return vectorized form (column-wise)
|
|
*/
|
|
Vector vector() const {
|
|
double r[] = { fx_, fy_, s_, u0_, v0_ };
|
|
Vector v(5);
|
|
copy(r, r + 5, v.begin());
|
|
return v;
|
|
}
|
|
|
|
/**
|
|
* return calibration matrix K
|
|
*/
|
|
Matrix matrix() const {
|
|
return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
|
|
}
|
|
|
|
/**
|
|
* convert intrinsic coordinates xy to image coordinates uv
|
|
*/
|
|
Point2 uncalibrate(const Point2& p) const {
|
|
const double x = p.x(), y = p.y();
|
|
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
|
|
}
|
|
|
|
void print(const std::string& s = "") const {
|
|
gtsam::print(matrix(), s);
|
|
}
|
|
|
|
std::string dump() const {
|
|
char buffer[100];
|
|
buffer[0] = 0;
|
|
sprintf(buffer, "%f %f %f %f %f", fx_, fy_, s_, u0_, v0_);
|
|
return std::string(buffer);
|
|
}
|
|
|
|
/**
|
|
* Check if equal up to specified tolerance
|
|
*/
|
|
bool equals(const Cal3_S2& K, double tol) const;
|
|
|
|
/** friends */
|
|
friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
|
|
};
|
|
|
|
/**
|
|
* assert for unit tests
|
|
*/
|
|
bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol = 1e-8);
|
|
|
|
/**
|
|
* convert intrinsic coordinates xy to image coordinates uv
|
|
*/
|
|
Point2 uncalibrate(const Cal3_S2& K, const Point2& p);
|
|
Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p);
|
|
Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
|
|
}
|