-
可以按绕xyz轴的角度和各方向平移
1 #pragma warning(disable:4996)
2 #include <pcl/registration/ia_ransac.h>//采样一致性
3 #include <pcl/point_types.h>
4 #include <pcl/point_cloud.h>
5 #include <pcl/features/normal_3d.h>
6 #include <pcl/features/fpfh.h>
7 #include <pcl/features/pfh.h>
8 #include <pcl/search/kdtree.h>
9 #include <pcl/io/pcd_io.h>
10 #include <pcl/io/ply_io.h>
11 #include <pcl/filters/voxel_grid.h>//
12 #include <pcl/filters/filter.h>//
13 #include <pcl/registration/icp.h>//icp配准
14 #include <pcl/registration/gicp.h>
15 #include <pcl/visualization/pcl_visualizer.h>//可视化
16 #include <time.h>//时间
17 #include <math.h>
18
19 using pcl::NormalEstimation;
20 using pcl::search::KdTree;
21 typedef pcl::PointXYZ PointT;
22 typedef pcl::PointCloud<PointT> PointCloud;
23 void y_transform(Eigen::Matrix4d &y_trans, float theta);
24 void z_transform(Eigen::Matrix4d &z_trans, float theta);
25 void x_transform(Eigen::Matrix4d &x_trans, float theta);
26 void y_transform1(Eigen::Matrix4d &y_trans);
27 void transform(Eigen::Matrix4d &x_trans, float theta);
28 void xyz_transform(Eigen::Matrix4f &y_trans, float t1, float t2, float t3,float a, float b, float c);
29 int main()
30 {
31
32 PointCloud::Ptr pcd_src(new pcl::PointCloud<pcl::PointXYZ>);
33 PointCloud::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
34
35 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 255, 0, 0);//定义颜色 旋转后的 红色
36 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_srch(cloud_src, 0, 255, 0);//定义颜色 旋转前的 绿色
37
38 pcl::io::loadPCDFile("bun000.pcd", *cloud_src); //输入要转换的矩阵
39
40 std::vector<int> indices_src2; //保存去除的点的索引
41 pcl::removeNaNFromPointCloud(*cloud_src, *cloud_src, indices_src2);
42 std::cout << "remove *cloud_src_o nan" << endl;
43
44 float t1,t2,t3,xx,yy,zz;
45 t1 = 20; //绕x轴旋转角度
46 t2 = 50;//绕y轴旋转角度
47 t3 = -28;//绕z轴旋转角度
48 xx = 0; //x方向平移
49 yy = 0;//y方向平移
50 zz = 0;//z方向平移
51
52
53 t1 /= 57.3;//弧度转换角度
54 t2 /= 57.3;
55 t3 /= 57.3;
56
57
58 Eigen::Matrix4f x_trans1;
59 xyz_transform(x_trans1,t1,t2,t3,xx,yy,zz); //在上面修改旋转角度 平移参数 的矩阵
60
61 //y_transform1(x_trans1);//使用自定义矩阵，用这个函数，去底部这个函数里 修改16个数值
62
63 std::cout << "旋转矩阵为：" << endl;
64 cout << x_trans1 << endl;
65
66 //使用创建的变换对未过滤的输入点云进行变换
67
68 //pcl::transformPointCloud(*pcd_src, *pcd_src, x_trans);
69 pcl::transformPointCloud(*cloud_src, *pcd_src, x_trans1);
70 pcl::io::savePCDFileASCII("test.pcd", *pcd_src); //旋转后保存的文件
71
72 pcl::visualization::PCLVisualizer viewer("registration Viewer");
73 viewer.setBackgroundColor(255, 255, 255);
74 viewer.addPointCloud(pcd_src, src_h, "source cloud");
75 viewer.addPointCloud(cloud_src, cloud_srch, "tgt cloud");
76
77
78 viewer.addCoordinateSystem(0.05);//坐标轴显示
79 while (!viewer.wasStopped())
80 {
81 viewer.spinOnce(100);
82 boost::this_thread::sleep(boost::posix_time::microseconds(100000));
83 }
84 return (0);
85 }
86 void y_transform(Eigen::Matrix4d &y_trans, float theta)
87 {
88 y_trans(0, 0) = cos(theta);
89 y_trans(0, 1) = 0;
90 y_trans(0, 2) = -sin(theta);
91 y_trans(1, 0) = 0;
92 y_trans(1, 1) = 1;
93 y_trans(1, 2) = 0;
94 y_trans(2, 0) = sin(theta);
95 y_trans(2, 1) = 0;
96 y_trans(2, 2) = cos(theta);
97 y_trans(0, 3) = 0.03;
98 y_trans(1, 3) = 0.0;
99 y_trans(2, 3) = 0.04;
100 y_trans(3, 0) = 0.000;
101 y_trans(3, 1) = 0.000;
102 y_trans(3, 2) = 0.0;
103 y_trans(3, 3) = 1;
104 }
105
106 //实验用，自己定义矩阵每个值
107 void y_transform1(Eigen::Matrix4d &y_trans)
108 {
109
110
111
112
113 y_trans(0, 0) = 0.840702;
114 y_trans(0, 1) = 0.0118127;
115 y_trans(0, 2) = - 0.541376;
116 y_trans(0, 3) = 0.0365537;
117 y_trans(1, 0) = - 0.0140057 ;
118 y_trans(1, 1) = 0.999906;
119 y_trans(1, 2) = 0.0000651455;
120 y_trans(1, 3) = - 0.000152141;
121 y_trans(2, 0) = 0.541323;
122 y_trans(2, 1) = 0.00752753;
123 y_trans(2, 2) = 0.840784;
124 y_trans(2, 3) = 0.0387605;
125 y_trans(3, 0) = 0.000;
126 y_trans(3, 1) = 0.000;
127 y_trans(3, 2) = 0.0;
128 y_trans(3, 3) = 1;
129 }
130
131
132 void z_transform(Eigen::Matrix4d &z_trans, float theta)
133 {
134 z_trans(0, 0) = cos(theta);
135 z_trans(0, 1) = sin(theta);
136 z_trans(0, 2) = 0;
137 z_trans(1, 0) = -sin(theta);
138 z_trans(1, 1) = cos(theta);
139 z_trans(1, 2) = 0;
140 z_trans(2, 0) = 0;
141 z_trans(2, 1) = 0;
142 z_trans(2, 2) = 1;
143 z_trans(0, 3) = 0;
144 z_trans(1, 3) = 0;
145 z_trans(2, 3) = 0;
146 z_trans(3, 0) = 0;
147 z_trans(3, 1) = 0;
148 z_trans(3, 2) = 0;
149 z_trans(3, 3) = 1;
150 }
151 void x_transform(Eigen::Matrix4d &x_trans, float theta)
152 {
153 x_trans(0, 0) = 1;
154 x_trans(0, 1) = 0;
155 x_trans(0, 2) = 0;
156 x_trans(1, 0) = 0;
157 x_trans(1, 1) = cos(theta);
158 x_trans(1, 2) = sin(theta);
159 x_trans(2, 0) = 0;
160 x_trans(2, 1) = -sin(theta);
161 x_trans(2, 2) = cos(theta);
162 x_trans(0, 3) = 0;
163 x_trans(1, 3) = 0;
164 x_trans(2, 3) = 0;
165 x_trans(3, 0) = 0;
166 x_trans(3, 1) = 0;
167 x_trans(3, 2) = 0;
168 x_trans(3, 3) = 1;
169 }
170 //放大缩小矩阵用
171 void transform(Eigen::Matrix4d &x_trans, float theta)
172 {
173 x_trans(0, 0) = theta;
174 x_trans(0, 1) = 0;
175 x_trans(0, 2) = 0;
176 x_trans(1, 0) = 0;
177 x_trans(1, 1) = theta;
178 x_trans(1, 2) = 0;
179 x_trans(2, 0) = 0;
180 x_trans(2, 1) = 0;
181 x_trans(2, 2) = theta;
182 x_trans(0, 3) = 0;
183 x_trans(1, 3) = 0;
184 x_trans(2, 3) = 0;
185 x_trans(3, 0) = 0;
186 x_trans(3, 1) = 0;
187 x_trans(3, 2) = 0;
188 x_trans(3, 3) = 1;
189 }
190 void xyz_transform(Eigen::Matrix4f &x_trans, float t1, float t2, float t3, float a, float b, float c)
191 {
192 x_trans(0, 0) = cos(t2)*cos(t3);
193 x_trans(0, 1) = -1*cos(t1)*sin(t3)+sin(t1)*sin(t2)*cos(t3);
194 x_trans(0, 2) = sin(t1)*sin(t3)+cos(t1)*sin(t2)*cos(t3);
195 x_trans(1, 0) = cos(t2)*sin(t3);
196 x_trans(1, 1) = cos(t1)*cos(t3)+sin(t1)*sin(t2)*sin(t3);
197 x_trans(1, 2) = -1*sin(t1)*cos(t3)+cos(t1)*sin(t2)*sin(t3);
198 x_trans(2, 0) = -sin(t2);
199 x_trans(2, 1) = sin(t1)*cos(t2);
200 x_trans(2, 2) = cos(t1)*cos(t2);
201 x_trans(0, 3) = a;
202 x_trans(1, 3) = b;
203 x_trans(2, 3) = c;
204 x_trans(3, 0) = 0;
205 x_trans(3, 1) = 0;
206 x_trans(3, 2) = 0;
207 x_trans(3, 3) = 1;
208 }