GPU_PERF_TEST(SolvePnPRansac, cv::gpu::DeviceInfo, Count) { cv::gpu::DeviceInfo devInfo = GET_PARAM(0); cv::gpu::setDevice(devInfo.deviceID()); int count = GET_PARAM(1); cv::Mat object(1, count, CV_32FC3); fill(object, -100, 100); cv::Mat camera_mat(3, 3, CV_32FC1); fill(camera_mat, 0.5, 1); camera_mat.at<float>(0, 1) = 0.f; camera_mat.at<float>(1, 0) = 0.f; camera_mat.at<float>(2, 0) = 0.f; camera_mat.at<float>(2, 1) = 0.f; cv::Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0)); std::vector<cv::Point2f> image_vec; cv::Mat rvec_gold(1, 3, CV_32FC1); fill(rvec_gold, 0, 1); cv::Mat tvec_gold(1, 3, CV_32FC1); fill(tvec_gold, 0, 1); cv::projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec); cv::Mat image(1, count, CV_32FC2, &image_vec[0]); cv::Mat rvec; cv::Mat tvec; cv::gpu::solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec); declare.time(3.0); TEST_CYCLE() { cv::gpu::solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec); } }
void SolvePNP_method(vector <Point3f> points3d, vector <Point2f> imagePoints, MatrixXf &R,Vector3f &t, MatrixXf &xcam_, MatrixXf K, const PointCloud<PointXYZRGB>::Ptr& cloudRGB,//projected data const PointCloud<PointXYZ>::Ptr& cloud_laser_cord,//laser coordinates MatrixXf& points_projected,Mat image, PointCloud<PointXYZ>::ConstPtr cloud) { Mat_<float> camera_matrix (3, 3); camera_matrix(0,0)=K(0,0); camera_matrix(0,1)=K(0,1); camera_matrix(0,2)=K(0,2); camera_matrix(1,0)=K(1,0); camera_matrix(1,1)=K(1,1); camera_matrix(1,2)=K(1,2); camera_matrix(2,0)=K(2,0); camera_matrix(2,1)=K(2,1); camera_matrix(2,2)=K(2,2); vector<double> rv(3), tv(3); Mat rvec(rv),tvec(tv); Mat_<float> dist_coef (5, 1); dist_coef = 0; cout <<camera_matrix<<endl; cout<< imagePoints<<endl; cout<< points3d <<endl; solvePnP( points3d, imagePoints, camera_matrix, dist_coef, rvec, tvec); //////////////////////////////////////////////////7 //convert data //////////////////////////////////////////////////7 double rot[9] = {0}; Mat R_2(3,3,CV_64FC1,rot); //change results only debugging Rodrigues(rvec, R_2); R=Matrix3f::Zero(3,3); t=Vector3f(0,0,0); double* _r = R_2.ptr<double>(); double* _t = tvec.ptr<double>(); t(0)=_t[0]; t(1)=_t[1]; t(2)=_t[2]; R(0,0)=_r[0]; R(0,1)=_r[1]; R(0,2)=_r[2]; R(1,0)=_r[3]; R(1,1)=_r[4]; R(1,2)=_r[5]; R(2,0)=_r[6]; R(2,1)=_r[7]; R(2,2)=_r[8]; points_projected=MatrixXf::Zero(2,cloud->points.size ()); for (int j=0;j<cloud->points.size ();j++){ PointCloud<PointXYZRGB> PointAuxRGB; PointAuxRGB.points.resize(1); Vector3f xw=Vector3f(cloud->points[j].x, cloud->points[j].y, cloud->points[j].z); xw=K*( R*xw+t); xw= xw/xw(2); int col,row; col=(int)xw(0); row=(int)xw(1); points_projected(0,j)=(int)xw(0); points_projected(1,j)=(int)xw(1); int b,r,g; if ((col<0 || row<0) || (col>image.cols || row>image.rows)){ r=0; g=0; b=0; }else{ // b = img->imageData[img->widthStep * row+ col * 3]; // g = img->imageData[img->widthStep * row+ col * 3 + 1]; //r = img->imageData[img->widthStep * row+ col * 3 + 2]; r=image.at<cv::Vec3b>(row,col)[0]; g=image.at<cv::Vec3b>(row,col)[1]; b=image.at<cv::Vec3b>(row,col)[2]; //std::cout << image.at<cv::Vec3b>(row,col)[0] << " " << image.at<cv::Vec3b>(row,col)[1] << " " << image.at<cv::Vec3b>(row,col)[2] << std::endl; } //std::cout << "( "<<r <<","<<g<<","<<b<<")"<<std::endl; uint8_t r_i = r; uint8_t g_i = g; uint8_t b_i = b; uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); //int32_t rgb = (r_i << 16) | (g_i << 8) | b_i; PointAuxRGB.points[0].x=cloud->points[j].x; PointAuxRGB.points[0].y=cloud->points[j].y; PointAuxRGB.points[0].z=cloud->points[j].z; PointAuxRGB.points[0].rgb=*reinterpret_cast<float*>(&rgb); cloudRGB->points.push_back (PointAuxRGB.points[0]); //project point to the image } }