xyz坐标转换ybc_四、双目坐标系与云台坐标系变换

1.确定坐标系，图中RGB分别对应XYZ。

1.1 双目坐标系原点位于左摄像机光心，注意'左'是沿着摄像头采集图像的方向看哦，光心射出的方向为Z+，另外光心这东西无法精准定位，本demo中为臆测，。

1.2 云台坐标系原点为基座舵机旋转轴中心，沿着激光射出的方向为X+。

2.如何确定变换矩阵

坐标点变换的思路为: 双目获区坐标位置CamPos -> 矩阵变换 ->云台坐标系ServoPos。

servoPos = camFrame2ServoFrame*camPos

我们要求相机坐标系到云台坐标系的变换，按照下面的规则：

         Mat camPosMat = (Mat_ < float>(4, 1) << camPos[0], camPos[1], camPos[2], 1);

	//先绕自身坐标系X逆时针旋转90°
	Mat rotateX_90 = (Mat_<double>(3, 3) << 1, 0, 0,
		0, 0, 1,
		0, -1, 0);
	//在上一步旋转后的坐标系基础上，再绕坐标系Z逆时针旋转90°
	Mat rotateZ_90 = (Mat_<double>(3, 3) << 0, 1, 0,
		-1, 0, 0,
		0, 0, 1);
	Mat rotation = rotateZ_90*rotateX_90;
        //平移向量为手动量取
	Mat camFrame2ServoFrame = (Mat_<float>(4, 4) <<
		rotation.at<double>(0, 0), rotation.at<double>(0, 1), rotation.at<double>(0, 2), 15,
		rotation.at<double>(1, 0), rotation.at<double>(1, 1), rotation.at<double>(1, 2), -90,
		rotation.at<double>(2, 0), rotation.at<double>(2, 1), rotation.at<double>(2, 2), 135,
		0, 0, 0, 1);
	Mat  objPosMat = camFrame2ServoFrame*camPosMat;
	//发送指令给云台
	int x = objPosMat.at<float>(0, 0);//float 型
	int y = objPosMat.at<float>(1, 0);
	int z = objPosMat.at<float>(2, 0);

至此，变换后坐标计算完毕。

后续有需要我会写一下坐标系变换与工业机器人中坐标系的概念。

感觉此专栏的重点都写完了，剩下的是单片机端的执行操作，除了使用STM32外，arduino和树莓派都可以作为执行单元。如果有需要STM32代码的同学可以私信我。

还有一些制作细节说明一下：

1. 云台舵机旋转角度和PWM的占空比需要标定，即确定占空比与舵机旋转角度的函数关系，两个舵机的关系可能不一样。
2. 可以添加目标跟踪的代码让激关照射动作更加平滑。

PS：有想复制本demo的同学可以私信我，我可以发亚克力板加工图和材料选型给你们。