1 #include <liblas/liblas.hpp>
2 #include <iomanip>
3 #include <iostream>
4 #include <sstream>
5 #include <cmath>
6 #include <pcl/point_cloud.h>
7 #include <pcl/io/pcd_io.h>
8 #include <pcl/point_types.h>
9 #include <pcl/visualization/pcl_visualizer.h>
10
11 // 实现Unshort转为Unchar类型
12 //int Unshort2Unchar(uint16_t &green, uint8_t &g)
13 //{
14 // unsigned short * word;
15 // word = &green;
16 // int size = WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, NULL, 0, NULL, FALSE);
17 // char *AsciiBuff=new char[size];
18 // WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, AsciiBuff, size, NULL, FALSE);
19 // g = *AsciiBuff;
20 //
21 // delete[] AsciiBuff;
22 // AsciiBuff = NULL;
23 // return 0;
24 //}
25
26 void writePointCloudFromLas(const char* strInputLasName, const char* strOutPutPointCloudName)
27 {
28 //打开las文件
29 std::ifstream ifs;
30 ifs.open(strInputLasName, std::ios::in | std::ios::binary);
31 if (!ifs.is_open())
32 {
33 std::cout << "无法打开.las" << std::endl;
34 return;
35 }
36 liblas::ReaderFactory readerFactory;
37 liblas::Reader reader = readerFactory.CreateWithStream(ifs);
38 //写点云
39 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOutput(new pcl::PointCloud<pcl::PointXYZRGBA>());
40 cloudOutput->clear();
41 while (reader.ReadNextPoint())
42 {
43 double x = reader.GetPoint().GetX();
44 double y = reader.GetPoint().GetY();
45 double z = reader.GetPoint().GetZ();
46 uint16_t red = reader.GetPoint().GetColor()[0];
47 uint16_t green = reader.GetPoint().GetColor()[1];
48 uint16_t blue = reader.GetPoint().GetColor()[2];
49
50 /*****颜色说明
51 * uint16_t 范围为0-256*256 ；
52 * pcl 中 R G B利用的unsigned char 0-256；
53 * 因此这里将uint16_t 除以256 得到 三位数的0-256的值
54 * 从而进行rgba 值32位 的位运算。
55 *
56 ******/
57
58 pcl::PointXYZRGBA thePt; //int rgba = 255<<24 | ((int)r) << 16 | ((int)g) << 8 | ((int)b);
59 thePt.x = x; thePt.y = y; thePt.z = z;
60 thePt.rgba = (uint32_t)255 << 24 | (uint32_t)(red / 256) << 16 | (uint32_t)(green / 256) << 8 | (uint32_t)(blue / 256);
61 //uint32_t rgb = *reinterpret_cast<int*>(&thePt.rgb); //reinterpret_cast 强制转换
62 cloudOutput->push_back(thePt);
63 }
64
65 //boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
66 //viewer->setBackgroundColor(0, 0, 0); //设置背景
67 //viewer->addPointCloud(cloudOutput,"cloudssd");
68 //while (!viewer->wasStopped()){
69 // viewer->spinOnce();
70 //}
71 pcl::io::savePCDFileASCII(strOutPutPointCloudName, *cloudOutput);
72 cloudOutput->clear();
73 }
74
75 //读入点云，写.las
76
77 void writeLasFromPointCloud3(const char* strInputPointCloudName, const char* strOutLasName)
78 {
79 std::ofstream ofs(strOutLasName, ios::out | ios::binary);
80 if (!ofs.is_open())
81 {
82 std::cout << "err to open file las....." << std::endl;
83 return;
84 }
85 liblas::Header header;
86 header.SetVersionMajor(1);
87 header.SetVersionMinor(2);
88 header.SetDataFormatId(liblas::PointFormatName::ePointFormat3);
89 header.SetScale(0.001, 0.001, 0.001);
90
91 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
92 pcl::io::loadPCDFile(strInputPointCloudName, *cloud);
93 std::cout << "总数:" << cloud->points.size() << std::endl;
94 //写liblas,
95 liblas::Writer writer(ofs, header);
96 liblas::Point point(&header);
97
98 for (size_t i = 0; i < cloud->points.size(); i++)
99 {
100 double x = cloud->points[i].x;
101 double y = cloud->points[i].y;
102 double z = cloud->points[i].z;
103 point.SetCoordinates(x, y, z);
104
105 uint32_t red = (uint32_t)cloud->points[i].r;
106 uint32_t green = (uint32_t)cloud->points[i].g;
107 uint32_t blue = (uint32_t)cloud->points[i].b;
108 liblas::Color pointColor(red, green, blue);
109 point.SetColor(pointColor);
110 writer.WritePoint(point);
111 //std::cout << x << "," << y << "," << z << std::endl;
112 }
113 double minPt[3] = { 9999999, 9999999, 9999999 };
114 double maxPt[3] = { 0, 0, 0 };
115 header.SetPointRecordsCount(cloud->points.size());
116 header.SetPointRecordsByReturnCount(0, cloud->points.size());
117 header.SetMax(maxPt[0], maxPt[1], maxPt[2]);
118 header.SetMin(minPt[0], minPt[1], minPt[2]);
119 writer.SetHeader(header);
120 }
121
122 int main()
123 {
124 //char strInputLasName[] = "qq.las";//"Scan_az001.las";
125 //char strOutPutPointCloudName[]="qqqq.pcd";
126 //writePointCloudFromLas(strInputLasName, strOutPutPointCloudName);
127
128 //std::string strInputPointCloudName = "eewge";
129 //std::string strOutLasName = "eewge";
130 //writeLasFromPointCloud(strInputPointCloudName.c_str(), strOutLasName.c_str());
131 char strInputPointCloudName[] = "qq.pcd";
132 char strOutLasName[] = "qq.las";
133 writeLasFromPointCloud3(strInputPointCloudName, strOutLasName);
134
135 return 0;
136 }
