Пример #1
0
 /**
  * @returns species rotational specific heat at constant volume.
  *
  */
 doublereal PecosGasPhase::cv_rot (double atom) const
 { return std::max(cv_tr(atom) - cv_trans(), 0.); }
Пример #2
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;
}