基于liblas 库las与pcd的rgb点云相互转换(win10 vs2013 x64环境)
希望对大家有帮助,多提建议,多交流。
#include <liblas/liblas.hpp>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <cmath>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
// 实现Unshort转为Unchar类型
//int Unshort2Unchar(uint16_t &green, uint8_t &g)
//{
// unsigned short * word;
// word = &green;
// int size = WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, NULL, 0, NULL, FALSE);
// char *AsciiBuff=new char[size];
// WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, AsciiBuff, size, NULL, FALSE);
// g = *AsciiBuff;
//
// delete[] AsciiBuff;
// AsciiBuff = NULL;
// return 0;
//}
void writePointCloudFromLas(const char* strInputLasName, const char* strOutPutPointCloudName)
{
//打开las文件
std::ifstream ifs;
ifs.open(strInputLasName, std::ios::in | std::ios::binary);
if (!ifs.is_open())
{
std::cout << “无法打开.las” << std::endl;
return;
}
liblas::ReaderFactory readerFactory;
liblas::Reader reader = readerFactory.CreateWithStream(ifs);
//写点云
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOutput(new pcl::PointCloud<pcl::PointXYZRGBA>());
cloudOutput->clear();
while (reader.ReadNextPoint())
{
double x = reader.GetPoint().GetX();
double y = reader.GetPoint().GetY();
double z = reader.GetPoint().GetZ();
uint16_t red = reader.GetPoint().GetColor()[0];
uint16_t green = reader.GetPoint().GetColor()[1];
uint16_t blue = reader.GetPoint().GetColor()[2];
/*****颜色说明
* uint16_t 范围为0-256*256 ;
* pcl 中 R G B利用的unsigned char 0-256;
* 因此这里将uint16_t 除以256 得到 三位数的0-256的值
* 从而进行rgba 值32位 的位运算。
*
******/
pcl::PointXYZRGBA thePt; //int rgba = 255<<24 | ((int)r) << 16 | ((int)g) << 8 | ((int)b);
thePt.x = x; thePt.y = y; thePt.z = z;
thePt.rgba = (uint32_t)255 << 24 | (uint32_t)(red / 256) << 16 | (uint32_t)(green / 256) << 8 | (uint32_t)(blue / 256);
//uint32_t rgb = *reinterpret_cast<int*>(&thePt.rgb); //reinterpret_cast 强制转换
cloudOutput->push_back(thePt);
}
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer(“3D Viewer”));
viewer->setBackgroundColor(0, 0, 0); //设置背景
viewer->addPointCloud(cloudOutput,”cloudssd”);
while (!viewer->wasStopped()){
viewer->spinOnce();
}
//pcl::io::savePCDFileASCII(strOutPutPointCloudName, cloudOutput);
//cloudOutput.clear();
}
//读入点云,写.las
void writeLasFromPointCloud3(const char* strInputPointCloudName, const char* strOutLasName)
{
std::ofstream ofs(strOutLasName, ios::out | ios::binary);
if (!ofs.is_open())
{
std::cout << “err to open file las…..” << std::endl;
return;
}
liblas::Header header;
header.SetVersionMajor(1);
header.SetVersionMinor(2);
header.SetDataFormatId(liblas::PointFormatName::ePointFormat3);
header.SetScale(0.001, 0.001, 0.001);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::io::loadPCDFile(strInputPointCloudName, *cloud);
std::cout << “总数:” << cloud->points.size() << std::endl;
//写liblas,
liblas::Writer writer(ofs, header);
liblas::Point point(&header);
for (size_t i = 0; i < cloud->points.size(); i++)
{
double x = cloud->points[i].x;
double y = cloud->points[i].y;
double z = cloud->points[i].z;
point.SetCoordinates(x, y, z);
uint32_t red = (uint32_t)cloud->points[i].r;
uint32_t green = (uint32_t)cloud->points[i].g;
uint32_t blue = (uint32_t)cloud->points[i].b;
liblas::Color pointColor(red, green, blue);
point.SetColor(pointColor);
writer.WritePoint(point);
//std::cout << x << “,” << y << “,” << z << std::endl;
}
double minPt[3] = { 9999999, 9999999, 9999999 };
double maxPt[3] = { 0, 0, 0 };
header.SetPointRecordsCount(cloud->points.size());
header.SetPointRecordsByReturnCount(0, cloud->points.size());
header.SetMax(maxPt[0], maxPt[1], maxPt[2]);
header.SetMin(minPt[0], minPt[1], minPt[2]);
writer.SetHeader(header);
}
int main()
{
//char strInputLasName[] = “qq.las”;//”Scan_az001.las”;
//char strOutPutPointCloudName[]=”qqqq.pcd”;
//writePointCloudFromLas(strInputLasName, strOutPutPointCloudName);
//std::string strInputPointCloudName = “eewge”;
//std::string strOutLasName = “eewge”;
//writeLasFromPointCloud(strInputPointCloudName.c_str(), strOutLasName.c_str());
char strInputPointCloudName[] = “qq.pcd”;
char strOutLasName[] = “qq.las”;
writeLasFromPointCloud3(strInputPointCloudName, strOutLasName);
return 0;
}