Ejemplo n.º 1
0
    mrpt::vision::pnp::epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints)
    {
      if (cameraMatrix.depth() == CV_32F)
          init_camera_parameters<float>(cameraMatrix);
      else
        init_camera_parameters<double>(cameraMatrix);

      number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));

      pws.resize(3 * number_of_correspondences);
      us.resize(2 * number_of_correspondences);

      if (opoints.depth() == ipoints.depth())
      {
        if (opoints.depth() == CV_32F)
          init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
        else
          init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
      }
      else if (opoints.depth() == CV_32F)
        init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
      else
        init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);

      alphas.resize(4 * number_of_correspondences);
      pcs.resize(3 * number_of_correspondences);

      max_nr = 0;
      A1 = NULL;
      A2 = NULL;
    }
Ejemplo n.º 2
0
int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
{
    CV_INSTRUMENT_REGION();

    double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
    std::vector<double> points;
    if (opoints.depth() == ipoints.depth())
    {
        if (opoints.depth() == CV_32F)
            extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
        else
            extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
    }
    else if (opoints.depth() == CV_32F)
        extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
    else
        extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);

    const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
    int solutions = solve(rotation_matrix, translation,
                          points[0], points[1], points[2], points[3], points[4],
                          points[5], points[6], points[7], points[8], points[9],
                          points[10], points[11], points[12], points[13], points[14],
                          points[15], points[16], points[17], points[18], points[19],
                          p4p);

    for (int i = 0; i < solutions; i++) {
        cv::Mat R, tvec;
        cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
        cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);

        Rs.push_back(R);
        tvecs.push_back(tvec);
    }

    return solutions;
}