Skip to content

Instantly share code, notes, and snippets.

@adioshun
Last active June 5, 2019 02:32
Show Gist options
  • Save adioshun/319d6a1326d33fa42cdd56833c3ef560 to your computer and use it in GitHub Desktop.
Save adioshun/319d6a1326d33fa42cdd56833c3ef560 to your computer and use it in GitHub Desktop.
PCL test code
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(pcl-test)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcl-test main.cpp)
target_link_libraries(pcl-test ${PCL_LIBRARIES})
SET(COMPILE_FLAGS "-std=c++11")
add_definitions(${COMPILE_FLAGS})
#include <iostream>
int main() {
std::cout << "hello, world!" << std::endl;
return (0);
}
//PCD 헤더 정보 출력
#include <iostream>
#include <fstream>
#include <string>
#include <iomanip>
#include <vector>
struct PointXYZ
{
double x, y, z;
};
/*
Author:chd_ayj
Date:2018-6-8
Description: read .PCD file
*/
//读取pcd点云文件
void readPCDfile(const std::string finname, std::vector<PointXYZ>& points, const std::string foutname)
{
std::ifstream fin(finname);
if (fin.bad()){
std::cout << "打开文件失败!" << std::endl;
return;
}
char s[11][1024]; //存储Header
int Points_Num; //点数量
std::string data_columns_type; //列数: X Y Z
std::string data_type; //点坐标存储方式(ascii或者binary)
std::vector<PointXYZ> cloud;
//连续读取Header行数
std::cout << "start to read file header....." << std::endl;
std::cout << "file header: " << std::endl;
for (int i = 0; i < 11; ++i){
fin.getline(s[i], 1024);
//std::cout << "第" << i + 1 << "行:" << s[i] << std::endl;
std::cout << s[i] << std::endl;
//FIELDS x y z rgb
if (i == 2){
std::string s2 = s[2];
size_t pos = s2.find("FIELDS");
size_t size = s2.size();
data_columns_type = s2.substr(pos + 7, size);
//std::cout << "data_columns_type:" << data_columns_type << std::endl;
}
//POINTS xxx
if (i == 9){
std::string s9 = s[9], Points_Str;
size_t pos = s9.find("POINTS");
size_t size = s9.size();
Points_Str = s9.substr(pos + 7, size);
Points_Num = atoi(Points_Str.c_str());
//std::cout << "Points:" << std::Points_Num << endl;
}
//DATA ascii或者binary
if (i == 10){
std::string s10 = s[10], DATA_SIZE;
size_t pos = s10.find("DATA");
size_t size = s10.size();
data_type = s10.substr(pos + 5, size);
//std::cout << "data_type:" << data_type << std::endl;
}
}
std::cout << std::endl;
std::cout << "start to read point ....." << std::endl;
PointXYZ p;
if ((data_columns_type == "x y z") && (data_type == "ascii")){
//读取点坐标记录
while (!fin.eof()){
fin >> p.x >> p.y >> p.z;
if (fin.peek() == EOF){
break;
}
cloud.push_back(p);
}
}else{
std::cout << "data_type = binary, read failed!" << std::endl;
}
////////////////////////////////////////////////////////////
//点坐标输出txt文本
std::cout << "start to write point to txt....." << std::endl;
std::ofstream out(foutname);
for (size_t i = 0; i < points.size(); ++i)
{
out << std::setiosflags(std::ios::fixed) << std::setprecision(7)
<< points.at(i).x << " "
<< points.at(i).y << " "
<< points.at(i).z << std::endl;
}
std::cout << "write point to txt finished!" << std::endl;
}
int main()
{
std::string finname = "table_scene_lms400.pcd";
std::string foutame = "table_scene_lms400.txt";
std::vector<PointXYZ> points;
readPCDfile(finname, points, foutame);
system("pause");
return 0;
}
#-*-coding:utf-8-*-
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def readXYZfile(filename, Separator):
data = [[], [], []]
f = open(filename,'r')
line = f.readline()
num = 0
while line: #按行读入点云
c,d,e = line.split(Separator)
data[0].append(c) #X坐标
data[1].append(d) #Y坐标
data[2].append(e) #Z坐标
num = num + 1
line = f.readline()
f.close()
#string型转float型
x = [ float(data[0] ) for data[0] in data[0] ]
z = [ float(data[1] ) for data[1] in data[1] ]
y = [ float(data[2] ) for data[2] in data[2] ]
print("读入点的个数为:{}个。".format(num))
point = [x,y,z]
return point
#三维离散点图显示点云
def displayPoint(data,title):
#解决中文显示问题
plt.rcParams['font.sans-serif']=['SimHei']
plt.rcParams['axes.unicode_minus'] = False
#点数量太多不予显示
while len(data[0]) > 20000:
print("点太多了!")
exit()
#散点图参数设置
fig=plt.figure()
ax=Axes3D(fig)
ax.set_title(title)
ax.scatter3D(data[0], data[1],data[2], c = 'r', marker = '.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
if __name__ == "__main__":
data = readXYZfile("demo.txt",',')
displayPoint(data, "兔子")
---------------------
作者:chd_ayj
来源:CSDN
原文:https://blog.csdn.net/qq_22170875/article/details/84728384
版权声明:本文为博主原创文章,转载请附上博文链接!
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment