Point calcRectIntersection(Size imgSize, float rho, float theta, bool dir)
{
Point p1 = calcLineIntersection(imgSize, rho, theta, true, dir);
if (p1.x >= 0 && p1.y >= 0 && p1.x <= imgSize.width && p1.y <= imgSize.height)
return p1;
else
return calcLineIntersection(imgSize, rho, theta, false, dir);
}
cv::Mat ScaleEstimator::transformIn2Coord(int pntNum, int preIdx, int curIdx) {
bool _isLogData = false;
// 对 0,1坐标系 计算点的三维位置
cv::Mat matPosOrign[3];
for (auto& m : matPosOrign) m = cv::Mat(3, 1, CV_64FC1).clone();
matPosOrign[preIdx] = 0.0f;
matPosOrign[curIdx] = matPosOrign[preIdx] + ptrMotion[preIdx]->getMatTRef();
/*
7.188560000000e+02 0.000000000000e+00 6.071928000000e+02
0.000000000000e+00 7.188560000000e+02 1.852157000000e+02
0.000000000000e+00 0.000000000000e+00 1.000000000000e+00
*/
double Fu = 718.856f, Fv = 718.856f, Cu = 607.1928f, Cv = 185.2157f;
double arrCameraInverse[] = { 1 / Fu, 0, -Cu / Fu, 0, 1 / Fv, -Cv / Fv, 0, 0, 1 };
cv::Mat matCameraInv(3, 3, CV_64FC1, arrCameraInverse);
//0, 1 直线方向
//先摄像头转换, 再全部转换到0坐标系
matDirXYZ[preIdx] = matCameraInv*matPointUVW[preIdx];
matDirXYZ[curIdx] = ptrMotion[preIdx]->getMatRConst() * matCameraInv * matPointUVW[curIdx];
//计算 0,1 在0坐标系下的交点
matIntersection = cv::Mat(3, pntNum, CV_64FC1);
matIntersection = 0.0f;
cv::Mat tmp;
cv::Mat distMask(pntNum, 1,CV_64FC1);
distMask = 0;
for (int idxPnt = 0; idxPnt < pntNum; idxPnt++) {
double dist = calcLineIntersection(
matDirXYZ[preIdx].col(idxPnt),
matDirXYZ[curIdx].col(idxPnt),
matPosOrign[curIdx],
tmp
);
distMask.at<double>(idxPnt, 0) = dist;
for (int row = 0; row < 3; row++)
matIntersection.at<double>(row, idxPnt) = tmp.at<double>(row, 0);
}
if (CFG_bIsLogGlobal)
if (_isLogData) {
//0,1原点,在0坐标系中
std::cout << matPosOrign[preIdx] << std::endl;
std::cout << matPosOrign[curIdx] << std::endl;
//输出0,1图像坐标点
printf("0,1 图像中的坐标点 - 0\n");
std::cout << matPointUVW[preIdx] << std::endl;
printf("0,1 图像中的坐标点 - 1\n");
std::cout << matPointUVW[curIdx] << std::endl;
//输出0,1方向 在0坐标系中的
printf("0,1方向 在0坐标系中的 - 0\n");
std::cout << matDirXYZ[preIdx] << std::endl;
printf("0,1方向 在0坐标系中的 - 1\n");
std::cout << matDirXYZ[curIdx] << std::endl;
//输出0,1的交点，在0坐标系下
printf("0,1数据 在0坐标系中的 交点\n");
std::cout << matIntersection << std::endl;
}
return distMask.clone();
}
