标签：src 自定义 void float 旋转 pcl 点云 include cloud
-
可以按绕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 }
标签：src,自定义,void,float,旋转,pcl,点云,include,cloud 来源： https://www.cnblogs.com/beautifulmoonlight/p/14951741.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。