C++ opencv viz位姿可视化

        相对位姿的一种表示方法是刚体变换(旋转矩阵和平移向量)。在做位姿估计或三维重建相关任务时，为了更直观的理解和判断算法的过程和结果，需要把相对位姿可视化。博主了解和学习了两种位姿可视化方法，分别是调用matlab showExtrinsic函数和调用opencv viz模块(需要编译VTK库)。

目录

1.opencv viz模块编译(联合编译contrib和vtk)

2.相对位姿表示

3.代码

1.opencv viz模块编译(联合编译contrib和vtk)

        首先下载opencv4.x及其对应的contrib模块(建议用opencv4以上，其viz模块在contrib模块中)。然后下载vtk库。博主vtk库是用vcpkg一键下载的，但是后续联合编译遇到了很多bug，应该是博主对cmake和vcpkg使用还不熟练。在vcpkg安装路径下shift和右键打开powershell。键入  vcpkg install VTK:x64-windows --recurse 。这里从官网vtk下载和cmake编译是一样。编译好vtk后，编译opencv cotrib viz。用cmake编译，主要注意三个点：

        1.with_VTK一定要勾选 2.build_viz一定要勾选 3.OPENCV_EXTRA_MODULES_PATH选择contrib源码下module所在的路径。

        生成解决方案后，检查modules下有没有viz模块，有就可以进行最后一步生成库了；如果没有说明前面3步出了问题，重新configure一下

2.相对位姿表示

        用刚体变换(旋转矩阵和平移向量)表示如下；右边X=[X,Y,Z]表示坐标系1下3D点坐标，左边X'=[X',Y',Z']表示同名3D点在坐标系2下的坐标。但是为了可视化两个坐标系间的相对位姿，直接用这里的R和T是不行的。(博主认为，3D点的刚体变换，和可视化坐标系刚体变换是相逆的)

        

        将上式写成等价的其次坐标形式，使得整个变换为乘法形式。

        

        经常我们可以得到上述变换A，但是位姿可视化需要上述的逆变换，也就是inv(A).

3.代码

        首先是matlab。博主用之前matlab 张正友标定的相机参数。

showExtrinsics(stereoParams2)

   

         接下来我们把同样的参数导入到C++ opencv viz模块

  void test()
     {
         cv::Matx33d r;
         
         cv::Vec3d t;
         r <<
             0.965187427746172, 0.00676895643700972, -0.261471624750146,
             -0.00517875663081780, 0.999963671140472, 0.00677029386577335,
             0.261507953608463, -0.00518050461094595, 0.965187418365723;
         t << -432.087309575979/432, - 9.19798126882100 / 432,	39.9247425340855 / 432;
         r = r.t(); 
         cv::viz::Viz3d myWindow("Coordinate Frame");
         cv::Vec3d cam1_pose(0, 0, 0), cam1_focalPoint(0, 0, 1000), cam1_y_dir(0, 1, 0); 
         cv::Affine3d cam_init_pose = cv::viz::makeCameraPose(cam1_pose, cam1_focalPoint,         
          cam1_y_dir); 
         myWindow.showWidget("World_coordinate", cv::viz::WCoordinateSystem(), 
         cam_init_pose); 
 
  
         cv::Affine3d transpose1(r, t);
         transpose1 = transpose1.inv();
         myWindow.showWidget("Cam0", cv::viz::WCoordinateSystem(), transpose1);
         myWindow.spin();
     }

        这里需要主要r=r.t()是因为matlab双目标定工具箱中获取旋转矩阵是从2相机到1相机下的R，转置后才是从1到2的R。以及最后transpose1.inv()是之前提到的将变换取逆变才是要可视化的坐标系变换。

        最后得到的可视化结果，rgb分别表示xyz轴(viz自带设定)，这样看，和上面matlab的外参显示相同。至此，用C++ opencv viz成功将获得的刚体变换可视化出来了。