//cv::Mat uvp = cv::Mat::zeros(2, probp.size(), CV_8UC1);
cv::Mat uvp = cv::Mat::zeros(2, probp.size(), CV_64FC1);//data-type...
for (unsigned int i = 0; i <probp.size(); i++ )
{
uvp.at<float>(0, i) = probp[i].x;
uvp.at<float>(1, i) = probp[i].y;
}
cv::Mat ipmps, xyp;
imagetoground(uvp, xyp, h, roll, pitch, camera_param_KK);
//cv::Mat uvp = cv::Mat::zeros(2, probp.size(), CV_8UC1);
cv::Mat uvp = cv::Mat::zeros(2, probp.size(), CV_64FC1);//data-type...
for (unsigned int i = 0; i <probp.size(); i++ )
{
uvp.at<float>(0, i) = probp[i].x;
uvp.at<float>(1, i) = probp[i].y;
}
cv::Mat ipmps, xyp;
imagetoground(uvp, xyp, h, roll, pitch, camera_param_KK);
//imagetoground...
void imagetoground(cv::Mat& uv, cv::Mat& xy, double ht, double roll, double pitch, double* camera_param_KK)
{
std::cout << "111" << std::endl;
int height = (int)(ht*1000); //unit-mm.
double fx = camera_param_KK[0];
double fy = camera_param_KK[4];
double cx = camera_param_KK[2];
double cy = camera_param_KK[5];
double c1 = cos(pitch*PI / 180);
double s1 = sin(pitch*PI / 180);
double c2 = cos(roll*PI / 180);
double s2 = sin(roll*PI / 180);
cv::Mat Tf = (cv::Mat_<double>(3, 3) << fx, 0, cx,
0, fy, cy,
0, 0, 1);
cv::Mat Tx = (cv::Mat_<double>(3, 3) << 1, 0, 0,
0, c1, s1,
0, -s1, c1);
cv::Mat T = Tx.inv() * Tf.inv();
T.rowRange(0, 2) = T.rowRange(0, 2)*(-height);
cv::Mat temp = cv::Mat::ones(3, uv.cols, CV_64FC1);
uv.copyTo(temp.rowRange(0, 2) ) ;
cv::Mat xyt = T*temp;
xyt.rowRange(0, 1) = xyt.rowRange(0, 1) / xyt.rowRange(2, 3);
xyt.rowRange(1, 2) = xyt.rowRange(1, 2) / xyt.rowRange(2, 3);
xyt.rowRange(0, 2).copyTo(xy);//deep-copy...
}
注意
错误的描述一般是内存冲突memory crruption,abort等等...
完