77 lines
2.0 KiB
C++
77 lines
2.0 KiB
C++
/**
|
|
* @file Cal3_S2.cpp
|
|
* @brief The most common 5DOF 3D->2D calibration
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <fstream>
|
|
#include <iostream>
|
|
|
|
#include "Cal3_S2.h"
|
|
|
|
namespace gtsam {
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
void Cal3_S2::load(std::string path) {
|
|
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 {
|
|
printf("Unable to load the calibration\n");
|
|
exit(0);
|
|
}
|
|
|
|
infile.close();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
|
|
if (fabs(fx_ - K.fx_) > tol) return false;
|
|
if (fabs(fy_ - K.fy_) > tol) return false;
|
|
if (fabs(s_ - K.s_) > tol) return false;
|
|
if (fabs(u0_ - K.u0_) > tol) return false;
|
|
if (fabs(v0_ - K.v0_) > tol) return false;
|
|
return true;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol) {
|
|
bool ret = actual.equals(expected, tol);
|
|
if (!ret) {
|
|
cout << "Not Equal:" << endl;
|
|
actual.print("Actual");
|
|
expected.print("Expected");
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
Point2 uncalibrate(const Cal3_S2& K, const Point2& p) {
|
|
return K.uncalibrate(p);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p) {
|
|
return Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,
|
|
1.0);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p) {
|
|
|
|
return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // namespace gtsam
|