/**
* @returns species rotational specific heat at constant volume.
*
*/
doublereal PecosGasPhase::cv_rot (double atom) const
{ return std::max(cv_tr(atom) - cv_trans(), 0.); }
vector<int> PosePnPRansac(
const std::shared_ptr<CameraInterface<double>> cam,
const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& img_pts,
const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& ideal_pts,
const vector<int> & candidate_map,
int robust_3pt_its,
float robust_3pt_tol,
Sophus::SE3d * T) {
vector<int> inlier_map(candidate_map.size(), -1);
std::vector<cv::Point3f> cv_obj;
std::vector<cv::Point2f> cv_img;
std::vector<int> idx_vec;
cv::Mat cv_coeff;
cv::Mat cv_rot(3,1,CV_64F);
cv::Mat cv_trans(3,1,CV_64F);
cv::Mat cv_K(3,3,CV_64F);
// cv::eigen2cv(cam.K(), cv_K);
cv::setIdentity(cv_K);
for (size_t i = 0; i<img_pts.size(); ++i)
{
int ideal_point_id = candidate_map[i];
if (ideal_point_id>=0)
{
// TODO: This is really bad for cameras > 180 FOV
// Replace with PNP for general camera.
const Eigen::Vector3d img_center_pts = cam->Unproject(img_pts[i]);
Eigen::Vector2d center;
center << img_center_pts[0]/img_center_pts[2], img_center_pts[1]/img_center_pts[2];
// const Eigen::Vector2d center = cam.Unmap(img_pts[i]);
const Eigen::Vector3d & c3d = ideal_pts[ideal_point_id];
cv_img.push_back(cv::Point2f(center.x(), center.y()));
cv_obj.push_back(cv::Point3f(c3d.x(), c3d.y(), c3d.z()));
idx_vec.push_back(i);
}
}
std::vector<int> cv_inliers;
if(cv_img.size() < 4)
return cv_inliers;
if(robust_3pt_its > 0) {
cv::solvePnPRansac(cv_obj, cv_img, cv_K, cv_coeff, cv_rot, cv_trans,
false, robust_3pt_its, robust_3pt_tol / cam->K()(0,0), 60, cv_inliers);
}else{
cv::solvePnP(cv_obj, cv_img, cv_K, cv_coeff, cv_rot, cv_trans, false);
}
Vector3d rot, trans;
cv::cv2eigen(cv_rot, rot);
cv::cv2eigen(cv_trans, trans);
if(std::isnan(rot[0]) || std::isnan(rot[1]) || std::isnan(rot[2]))
return inlier_map;
for (size_t i = 0; i<cv_inliers.size(); ++i)
{
int idx = cv_inliers[i];
inlier_map.at(idx_vec.at(idx)) = candidate_map.at(idx_vec.at(idx));
}
*T = Sophus::SE3d(Sophus::SO3d::exp(rot), trans);
return inlier_map;
}
