bool mrpt::vision::pnp::CPnP::so3(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
try{
// Input 2d/3d correspondences and camera intrinsic matrix
Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
// Check for consistency of input matrix dimensions
if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
throw(2);
else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
throw(3);
if(obj_pts.rows() < obj_pts.cols())
{
cam_in_eig=cam_intrinsic.transpose();
img_pts_eig=img_pts.transpose().block(0,0,n,2);
obj_pts_eig=obj_pts.transpose();
}
else
{
cam_in_eig=cam_intrinsic;
img_pts_eig=img_pts.block(0,0,n,2);
obj_pts_eig=obj_pts;
}
// Output pose
Eigen::Matrix3d R;
Eigen::Vector3d t;
// Compute pose
mrpt::vision::pnp::p3p p(cam_in_eig);
p.solve(R,t, obj_pts_eig, img_pts_eig);
mrpt::vision::pnp::so3 s(obj_pts_eig, img_pts_eig, cam_in_eig, n);
bool ret = s.compute_pose(R,t);
Eigen::Quaterniond q(R);
pose_mat<<t,q.vec();
return ret;
}
catch(int e)
{
switch(e)
{
case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
}
return false;
}
}
void softmax<T>::compute_gradient(Eigen::Ref<const EigenMat> const &train,
Eigen::Ref<const EigenMat> const &weight,
Eigen::Ref<const EigenMat> const &ground_truth)
{
grad_.noalias() =
(ground_truth.array() - hypothesis_.array())
.matrix() * train.transpose();
auto const NSamples = static_cast<double>(train.cols());
grad_.array() = grad_.array() / -NSamples +
params_.lambda_ * weight.array();
}
int softmax<T>::predict(Eigen::Ref<const EigenMat> const &input)
{
CV_Assert(input.cols() == 1);
compute_hypothesis(input, weight_);
probability_ = (hypothesis_ * input.transpose()).
rowwise().sum();
EigenMat::Index max_row = 0, max_col = 0;
probability_.maxCoeff(&max_row, &max_col);
return max_row;
}
void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
{
out = x.transpose().normalized();
}
bool mrpt::vision::pnp::CPnP::dls(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
try{
#if MRPT_HAS_OPENCV==1
// Input 2d/3d correspondences and camera intrinsic matrix
Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
// Check for consistency of input matrix dimensions
if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
throw(2);
else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
throw(3);
if(obj_pts.rows() < obj_pts.cols())
{
cam_in_eig=cam_intrinsic.transpose();
img_pts_eig=img_pts.transpose().block(0,0,n,2);
obj_pts_eig=obj_pts.transpose();
}
else
{
cam_in_eig=cam_intrinsic;
img_pts_eig=img_pts.block(0,0,n,2);
obj_pts_eig=obj_pts;
}
// Output pose
Eigen::Matrix3d R_eig;
Eigen::MatrixXd t_eig;
// Compute pose
cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);
cv::eigen2cv(cam_in_eig, cam_in_cv);
cv::eigen2cv(img_pts_eig, img_pts_cv);
cv::eigen2cv(obj_pts_eig, obj_pts_cv);
mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
bool ret = d.compute_pose(R_cv,t_cv);
cv::cv2eigen(R_cv, R_eig);
cv::cv2eigen(t_cv, t_eig);
Eigen::Quaterniond q(R_eig);
pose_mat << t_eig,q.vec();
return ret;
#else
throw(-1)
#endif
}
catch(int e)
{
switch(e)
{
case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
}
return false;
}
}
