空间三维坐标系对齐

Posted 2022-11-04 Thomas会写字

 

/*
坐标系转换 格式必须是 n行3列 每行 x y z double类型
DatasetFrom = 需要变换的坐标系
DatasetTo = 目标坐标系
DatasetTransResult = 转换结果
nArraySize = 数组大小
*/
void Vision::ComputeR_T(cv::Mat DatasetFrom, cv::Mat DatasetTo, cv::Mat & DatasetTransResult,
	cv::Mat & matR, cv::Mat & matT, cv::Mat & marErr, cv::Mat & matErrMean, double & dScanle)

	int rows = DatasetFrom.rows;
	int cols = DatasetFrom.cols;

	// 按列求均值 即计算中心点坐标
	cv::Mat  CentroidMatrixCamera = MeanAsCol(DatasetFrom);
	cv::Mat  CentroidMatrixRobot = MeanAsCol(DatasetTo);

	// 临时变量 每组数据（每行）都是中心坐标
	cv::Mat RepmatMatrixCamera = cv::Mat::zeros(rows, cols, CV_64FC1);
	cv::Mat RepmatMatrixRobot = cv::Mat::zeros(rows, cols, CV_64FC1);
	for (int i = 0; i < rows; i++)
	
		CentroidMatrixCamera.copyTo(RepmatMatrixCamera.rowRange(i, i + 1));
		CentroidMatrixRobot.copyTo(RepmatMatrixRobot.rowRange(i, i + 1));
	

	// 计算每个坐标与中心点的偏移量
	cv::Mat MatrixCameraMove = DatasetFrom - RepmatMatrixCamera;
	cv::Mat MatrixRobotMove = DatasetTo - RepmatMatrixRobot;

	// 因为图像坐标系与机械手坐标系的尺度因子不同 像素-毫米 
	// 所以需要缩放比例因子 lam2
	cv::Mat MatrixCameraNorm = MatrixCameraMove.mul(MatrixCameraMove);
	cv::Mat MatrixRobotNorm = MatrixRobotMove.mul(MatrixRobotMove);

	cv::Mat lam1 = SumAsRow(MatrixCameraNorm) / SumAsRow(MatrixRobotNorm);
	cv::Mat lam2 = MeanAsCol(lam1);
	double dLam2 = lam2.at<double>(0, 0);
	double dSqrtLam2 = sqrt(dLam2);

	// 通过奇异值分解得到旋转矩阵 计算 R 矩阵
	cv::Mat MatrixCameraMoveT;
	transpose(MatrixCameraMove, MatrixCameraMoveT);
	cv::Mat H = MatrixCameraMoveT * MatrixRobotMove;

	cv::Mat  W, U, V, R;
	cv::SVD::compute(H, W, U, V, 0);
	transpose(U, U);
	transpose(V, V);
	R = V * U;

	// 一个判断 忘记什么用了
	double detr = determinant(R);
	if (detr < 0)
		V.colRange(2, 3) = V.colRange(2, 3)*(-1);
		R = V*U;
	

	// 继续计算 T 矩阵 除以缩放因子 lam2
	cv::Mat T = R.mul(cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(-1))) /
		cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(dSqrtLam2));

	// 继续计算 R T 矩阵
	cv::Mat CentroidMatrixCameraT, CentroidMatrixRobotT;
	transpose(CentroidMatrixCamera, CentroidMatrixCameraT);
	transpose(CentroidMatrixRobot, CentroidMatrixRobotT);
	T = T * CentroidMatrixCameraT + CentroidMatrixRobotT;
	R = R / cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(dSqrtLam2));

	// 复制最终结果
	R.copyTo(matR);
	T.copyTo(matT);
	dScanle = dSqrtLam2;

	// 计算结果
	ComputeDatasetTransForm(DatasetFrom, R, T, DatasetTransResult);
	marErr = DatasetTransResult - DatasetTo;
	matErrMean = MeanAsCol(cv::abs(marErr));