• 可以按绕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 }