opencv连杆操作

智能机器人智能机器人 实验报告实验报告 实验四实验四 连杆操作连杆操作 实验目的实验目的 理解 3D 点云运算 能够熟练编程实现连杆的空间运动模拟 实验条件实验条件 Ubuntu 系统 PCL 库 Opencv 库 实验原理实验原理 在对机器人运动学中旋转及平移操作进行充分理解的基础上 综 合运用旋转平移组合等操作 实现连杆移动的仿真 如下图所示 图中 3D 点云为单个连杆 运用 opencv 及 PCL 操作该连杆 实现两 连杆的连接以及旋转仿真 连杆 1连杆 2 连杆 1 连杆 2 45 度 85 度 实验内容实验内容 完成下列程序 要求使用完成下列程序 要求使用 opencvopencv 完成点云的旋转平移等操作 并将完成点云的旋转平移等操作 并将 操作后的点云转换为操作后的点云转换为 pclpcl 的的 PointCloudPointCloud 类型返回 以便之后的程序类型返回 以便之后的程序 显示结果 显示结果 pcl PointCloudpcl PointCloud linkageTransformationlinkageTransformation pcl PointCloud pcl PointCloud pointcloudinput pointcloudinput pcl PointCloudpcl PointCloud pointcloudoutput pointcloudoutput pleaseplease finishfinish thisthis programprogram toto implementimplement 3D3D pointspoints linkagelinkage Transformation Transformation input input pointcloudinputpointcloudinput output output pointcloudoutputpointcloudoutput description description calculatecalculate pointcloudoutputpointcloudoutput byby TransformatingTransformating linkagelinkage pointpoint cloudcloud cv Matcv Mat l1l1 cv Mat 3 pointcloudinput size CV 32F cv Mat 3 pointcloudinput size CV 32F cerrcerr endl endl for intfor int i 0 i pointcloudinput size i i 0 i pointcloudinput size i l1 at 0 l1 at 0 i i pointcloudinput at i x pointcloudinput at i x l1 at 1 l1 at 1 i i pointcloudinput at i y pointcloudinput at i y l1 at 2 l1 at 2 i i pointcloudinput at i z pointcloudinput at i z cerrcerr endl endl cv Matcv Mat l2l2 cv Mat 3 pointcloudinput size CV 32F cv Mat 3 pointcloudinput size CV 32F for intfor int i 0 i pointcloudinput size i i 0 i pointcloudinput size i l2 at 0 l2 at 0 i i pointcloudinput at i x pointcloudinput at i x l2 at 1 l2 at 1 i i pointcloudinput at i y pointcloudinput at i y l2 at 2 l2 at 2 i i pointcloudinput at i z pointcloudinput at i z cerrcerr endl endl cv Matcv Mat R1R1 cv Mat 3 3 CV 32F cv Mat 3 3 CV 32F R1 at 0 0 cos 3 14 4 R1 at 0 0 cos 3 14 4 R1 at 1 0 sin 3 14 4 R1 at 1 0 sin 3 14 4 R1 at 2 0 0 R1 at 2 0 0 R1 at 1 0 sin 3 14 4 R1 at 1 0 sin 3 14 4 R1 at 1 1 cos 3 14 4 R1 at 1 1 cos 3 14 4 R1 at 1 2 0 R1 at 1 2 0 R1 at 2 0 0 R1 at 2 0 0 R1 at 2 1 0 R1 at 2 1 0 R1 at 2 2 1 R1 at 2 2 1 l1l1 R1R1 l1 l1 cerrcerr endl endl cv Matcv Mat R2R2 cv Mat 3 3 CV 32F cv Mat 3 3 CV 32F R2 at 0 0 cos 1 48 R2 at 0 0 cos 1 48 R2 at 1 0 sin 1 48 R2 at 1 0 sin 1 48 R2 at 2 0 0 R2 at 2 0 0 R2 at 1 0 sin 1 48 R2 at 1 0 sin 1 48 R2 at 1 1 cos 1 48 R2 at 1 1 cos 1 48 R2 at 1 2 0 R2 at 1 2 0 R2 at 2 0 0 R2 at 2 0 0 R2 at 2 1 0 R2 at 2 1 0 R2 at 2 2 1 R2 at 2 2 1 l2l2 R2R2 l2 l2 cerrcerr endl endl for intfor int i i 0 0 i i pointcloudinput size i pointcloudinput size i l2 at 0 i l2 at 0 i 8 0f 8 0f cv Matcv Mat c c cv Mat 3 cv Mat 3 pointcloudinput size CV 32F pointcloudinput size CV 32F for intfor int i i 0 0 i i pointcloudinput size i pointcloudinput size i c at 0 i c at 0 i 2 344 2 344 c at 1 i c at 1 i 5 656 5 656 c at 2 i c at 2 i 0 0 l2l2 l2l2 c c pcl PointCloudpcl PointCloud pointcloudl1 pointcloudl2 pointcloudl1 pointcloudl2 pointcloudl1 resize l1 cols pointcloudl1 resize l1 cols pointcloudl2 resize l2 cols pointcloudl2 resize l2 cols for intfor int i i 0 0 i l1 cols i i l1 cols i pointcloudl1 at i xpointcloudl1 at i x l1 at 0 i l1 at 0 i pointcloudl1 at i ypointcloudl1 at i y l1 at 1 i l1 at 1 i pointcloudl1 at i zpointcloudl1 at i z l1 at 2 i l1 at 2 i pointcloudoutput push back pointcloudl1 at i pointcloudoutput push back pointcloudl1 at i for intfor int i i 0 0 i l2 cols i i l2 cols i pointcloudl2 at i xpointcloudl2 at i x l1 at 0 i l1 at 0 i pointcloudl2 at i ypointcloudl2 at i y l1 at 1 i l1 at 1 i pointcloudl2 at i zpointcloudl2 at i z l1 at 2 i l1 at 2 i pointcloudoutput push back point