SimpleCamera(
const Mat3 & K = Mat3::Identity(),
const Mat3 & R = Mat3::Identity(),
const Vec3 & t = Vec3::Zero())
: _K(K), _R(R), _t(t)
{
_C = -R.transpose() * t;
P_From_KRt(_K, _R, _t, &_P);
}
// Solve the problem of camera pose.
static void Solve(const Mat &pt2D, const Mat &pt3D, std::vector<Mat34> *models)
{
Mat3 R;
Vec3 t;
Mat34 P;
if (EuclideanResectionEPnP(pt2D, pt3D, &R, &t)) {
P_From_KRt(Mat3::Identity(), R, t, &P); // K = Id
models->push_back(P);
}
}
/// Estimates the root mean square error (2D)
double RootMeanSquareError(const Mat2X &x_image,
const Mat3X &X_world,
const Mat3 &K,
const Mat3 &R,
const Vec3 &t) {
Mat34 P;
P_From_KRt(K, R, t, &P);
size_t num_points = x_image.cols();
Mat2X dx = Project(P, X_world) - x_image;
return dx.norm() / num_points;
}
void Fit(const std::vector<size_t> &samples, std::vector<Model> *models) const {
const Mat2X x = ExtractColumns(x_camera_, samples);
const Mat3X X = ExtractColumns(X_, samples);
Mat34 P;
Mat3 R;
Vec3 t;
if (EuclideanResectionEPnP(x, X, &R, &t))
{
P_From_KRt(K_, R, t, &P);
models->push_back(P);
}
}
BrownPinholeCamera(
double focal,
double ppx,
double ppy,
const Mat3 & R = Mat3::Identity(),
const Vec3 & t = Vec3::Zero(),
double k1 = 0.0,
double k2 = 0.0,
double k3 = 0.0)
: _R(R), _t(t), _f(focal), _ppx(ppx), _ppy(ppy),
_k1(k1), _k2(k2), _k3(k3)
{
_C = -R.transpose() * t;
_K << _f, 0, _ppx,
0, _f, _ppy,
0, 0, 1;
P_From_KRt(_K, _R, _t, &_P);
}
Mat34 NViewDataSet::P(size_t i)const {
assert(i < _n);
Mat34 P;
P_From_KRt(_K[i], _R[i], _t[i], &P);
return P;
}
virtual Mat34 get_projective_equivalent(const geometry::Pose3 & pose) const
{
Mat34 P;
P_From_KRt(K(), pose.rotation(), pose.translation(), &P);
return P;
}
Mat34 NViewDataSet::P(size_t i)const {
assert(i < actual_camera_num_);
Mat34 P;
P_From_KRt(camera_matrix_[i], rotation_matrix_[i], translation_vector_[i], &P);
return P;
}
/// From the essential matrix test the 4 possible solutions
/// and return the best one. (point in front of the selected solution)
bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2,
const Mat & x1, const Mat & x2,
const Mat3 & E, const std::vector<size_t> & vec_inliers,
Mat3 * R, Vec3 * t)
{
bool bOk = false;
// Accumulator to find the best solution
std::vector<size_t> f(4, 0);
std::vector<Mat3> Es; // Essential,
std::vector<Mat3> Rs; // Rotation matrix.
std::vector<Vec3> ts; // Translation matrix.
Es.push_back(E);
// Recover best rotation and translation from E.
MotionFromEssential(E, &Rs, &ts);
//-> Test the 4 solutions will all the point
assert(Rs.size() == 4);
assert(ts.size() == 4);
Mat34 P1, P2;
Mat3 R1 = Mat3::Identity();
Vec3 t1 = Vec3::Zero();
P_From_KRt(K1, R1, t1, &P1);
for (int i = 0; i < 4; ++i) {
const Mat3 &R2 = Rs[i];
const Vec3 &t2 = ts[i];
P_From_KRt(K2, R2, t2, &P2);
Vec3 X;
for (size_t k = 0; k < vec_inliers.size(); ++k)
{
const Vec2 & x1_ = x1.col(vec_inliers[k]);
const Vec2 & x2_ = x2.col(vec_inliers[k]);
TriangulateDLT(P1, x1_, P2, x2_, &X);
// Test if point is front to the two cameras.
if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) {
++f[i];
}
}
}
// Check the solution :
std::cout << "\t Number of points in front of both cameras:"
<< f[0] << " " << f[1] << " " << f[2] << " " << f[3] << std::endl;
std::vector<size_t>::iterator iter = max_element(f.begin(), f.end());
if(*iter != 0)
{
size_t index = std::distance(f.begin(), iter);
(*R) = Rs[index];
(*t) = ts[index];
bOk = true;
}
else {
std::cerr << std::endl << "/!\\There is no right solution,"
<<" probably intermediate results are not correct or no points"
<<" in front of both cameras" << std::endl;
bOk = false;
}
return bOk;
}
