Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
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();
}