10 #include "vision-precomp.h" 13 #include <Eigen/Dense> 22 img_pts=img_pts_.block(0,0,n0,2);
24 R=Eigen::MatrixXd::Identity(3,3);
25 t=Eigen::VectorXd::Zero(3);
32 obj_vecs=Eigen::MatrixXd::Zero(n0,3);
37 img_vecs = Eigen::MatrixXd::Zero(n0,2);
46 double I0_norm, J0_norm;
61 scale=(I0_norm + J0_norm)/2;
84 Eigen::FullPivLU<Eigen::MatrixXd> lu(
obj_pts);
123 if(iCount>0 && imageDiff==0)
126 if(iCount==LOOP_MAX_COUNT)
128 std::cout<<
"Solution Not converged"<<std::endl<<std::endl;
Vector3< double > Vector3d
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Computes pose using iterative computation of POS()
Eigen::VectorXd epsilons
Focal Length from camera intrinsic matrix.
Eigen::MatrixXd obj_vecs
Translation Vector.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Eigen::MatrixXd R
Number of 2d/3d correspondences.
int n
Co-efficients used for scaling.
Eigen::VectorXd t
Rotation Matrix.
posit(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd camera_intrinsic_, int n)
Used to store img_vecs from previous iteration.
Eigen::MatrixXd img_pts
Object Points in Camera Co-ordinate system.
long get_img_diff()
Function to check for convergence.
Eigen::MatrixXd img_vecs
Object Points relative to 1st object point.
double f
Pseudo-Inverse of Object Points matrix.
Pose from Orthogonality and Scaling (POSIT) - Eigen Implementation.
Eigen::MatrixXd obj_matrix
Camera Intrinsic matrix.
Eigen::MatrixXd cam_intrinsic
Image Points in pixels.
Eigen::MatrixXd img_vecs_old
Image Points relative to 1st image point.
void POS()
Function used to compute pose from orthogonality.