成像平面倾斜校正,四元数,旋转矩阵

最近遇到一个问题,困扰很久
使用工业相机进行拍照的时候,如果相机发生了倾斜,我需要对这个现象进行倾斜校正
目前我的想法是:在平面摆放三个不在同一直线的物体,通过这三个物体的二维坐标和深度信息,三点确定一个平面。
既能获得这个倾斜平面的平面公式(法向量),那么我又已知我要校正成水平面即法向量为(0,0,1)。
通过这两个法向量,求得它们的四元数,再通过四元数转换为欧拉角,就能实现这两个平面的旋转。
再取旋转后的同一点的不同坐标求平移向量。
最后通过旋转矩阵和平移向量进行倾斜校正。但是最后的测试结果不尽人意。
不知道问题出在哪里

百度 透视变换