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; }