10 #include "vision-precomp.h" 14 #include <Eigen/Dense> 30 Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n, n), A(n,n), Y(n,3), E(n,3), E_old(n,3), U,V, I3 =Eigen::MatrixXd::Identity(3, 3), PR, Z=Eigen::MatrixXd::Zero(n, n);
31 Eigen::VectorXd
e(n), II(n), c(3), Zmindiag(n);
34 II.fill(1.0/((
double)n));
36 A=I-
e*
e.transpose()/n;
38 double err=std::numeric_limits<double>::infinity();
47 Eigen::JacobiSVD<Eigen::MatrixXd> svd(
P.transpose()*Z*A*
S, Eigen::ComputeThinU | Eigen::ComputeThinV);
51 I3(2,2) = (U*V.transpose()).determinant();
59 Zmindiag=((PR*Y.transpose()).diagonal()).array() / ((
P.array()*
P.array()).rowwise().sum()).array();
61 for (
int i = 0; i < n; i++)
66 Z=Zmindiag.asDiagonal();
Vector3< double > Vector3d
Eigen::MatrixXd S
Image points in pixels.
ppnp(const Eigen::MatrixXd &obj_pts, const Eigen::MatrixXd &img_pts, const Eigen::MatrixXd &cam_intrinsic)
Constructor for the P-PnP class.
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
bool compute_pose(Eigen::Matrix3d &R, Eigen::Vector3d &t, int n)
Function to compute pose.
auto transpose(T const &p) -> decltype(details::Transpose< T >::impl(T()))
Matrix3< double > Matrix3d
Eigen::MatrixXd C
Object points in Camera Co-ordinate system.
CONTAINER::Scalar norm(const CONTAINER &v)