Project1,简化了提取索引点云,点云合并
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include<fstream>
#include <string>
#include <vector>
#include <LasOperator.h>
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
using namespace std;
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB> &cloud);
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB> &cloud) {
std::ifstream ifs(s, std::ios::in | std::ios::binary);
liblas::ReaderFactory f;
liblas::Reader reader = f.CreateWithStream(ifs);
unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();
cloud.width = nbPoints;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize(cloud.width * cloud.height);
int i = 0;
uint16_t r1, g1, b1;
int r2, g2, b2;
uint32_t rgb;
while (reader.ReadNextPoint()) {
cloud.points[i].x = (reader.GetPoint().GetX());
cloud.points[i].y = (reader.GetPoint().GetY());
cloud.points[i].z = (reader.GetPoint().GetZ());
r1 = (reader.GetPoint().GetColor().GetRed());
g1 = (reader.GetPoint().GetColor().GetGreen());
b1 = (reader.GetPoint().GetColor().GetBlue());
r2 = ceil(((float)r1 / 65536)*(float)256);
g2 = ceil(((float)g1 / 65536)*(float)256);
b2 = ceil(((float)b1 / 65536)*(float)256);
rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);
i++;
}
}
int main() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr seed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB tempPnt;
float x1, y1, x2, y2, a1, a2, a3, b11, b22, b3;
x1 = y1 = a1 = a2 = a3 = b11 = b22 = b3 = 0;
x2 = 0; y2 = 1;
float r = 1;
float SmoothnessThreshold = 6;
float CurvatureThreshold = 0.01;
cout << "读取点云" << endl;
loadLasFile("sample100.las", *cloud);
cout << "读取轨迹线" << endl;
pcl::io::loadPCDFile("trail.pcd", *trail);
cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;
cout << "构建八叉树" << endl;
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
std::vector<int>pointIdx;
std::vector<int>pointIdx1;
for (size_t i = 0; i < trail->points.size() + 500; i += 20) {
cout << "-------------------------------------------------------------" << endl;
a1 = trail->points[i].x;
a2 = trail->points[i].y;
a3 = trail->points[i].z;
b11 = trail->points[i + 20].x;
b22 = trail->points[i + 20].y;
b3 = trail->points[i + 20].z;
x1 = a1 - b11;
y1 = a2 - b22;
float cosa = (x1*x2 + y1 * y2) / (sqrt(x1*x1 + y1 * y1) + sqrt(x2*x2 + y2 * y2));
float sina = sqrt(1 - cosa * cosa);
float range = r / sina;
Eigen::Vector3f n(-100000, min(a2, b22), -100000);
Eigen::Vector3f m(100000, max(a2, b22) - 0.001, min(a3, b3));
octree.boxSearch(n, m, pointIdx);
cout << i << " 这段有点数:" << pointIdx.size() << "\t";
pcl::copyPointCloud(*cloud, pointIdx, *part);
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree1(resolution);
octree1.setInputCloud(part);
octree1.addPointsFromInputCloud();
Eigen::Vector3f p(a1 - range, min(trail->points[i + 8].y, trail->points[i + 12].y), -100000);
Eigen::Vector3f q(a1 + range, max(trail->points[i + 8].y, trail->points[i + 12].y) - 0.000001, min(a3, b3));
octree1.boxSearch(p, q, pointIdx1);
cout << "这段种子数:" << pointIdx1.size() << endl;
pcl::copyPointCloud(*part, pointIdx1, *partSeed);
*seed += *partSeed;
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> >(new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(part);
normal_estimator.setKSearch(500);
normal_estimator.compute(*normals);
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setMinClusterSize(50);
reg.setMaxClusterSize(30000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(30);
reg.setInputCloud(part);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(SmoothnessThreshold / 180.0 * M_PI);
reg.setCurvatureThreshold(CurvatureThreshold);
std::vector <pcl::PointIndices> clusters;
reg.extract(clusters);
pcl::PointIndices cluster;
reg.getSegmentFromPoint(pointIdx1[0], cluster);
cout << "取得的种子是:" << pointIdx1[0] << "\t生长后点数:" << cluster.indices.size() << endl;
cout << "种子点" << part->points[pointIdx1[0]].x << "\t" << part->points[pointIdx1[0]].y << "\t" << part->points[pointIdx1[0]].z << "\t" << endl;
cout << "-------------------------------------------------------------" << endl;
pcl::copyPointCloud(*part, cluster, *tempCloud);
*result += *tempCloud;
}
cout << "生长后的点云保存到result.pcd,0result.pcd" << endl;
pcl::io::savePCDFileASCII("0result.pcd", *result);
cout << "保存成功" << endl;
cout << "切割后的点云保存到seedCloud,seedCloud.pcd" << endl;
pcl::io::savePCDFileASCII("0seedCloud.pcd", *seed);
cout << "保存成功" << endl;
return 0;
}
Project1,添加输入las文件的函数
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include<fstream>
#include <string>
#include <vector>
#include <LasOperator.h>
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
using namespace std;
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB> &cloud);
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB> &cloud) {
std::ifstream ifs(s, std::ios::in | std::ios::binary);
liblas::ReaderFactory f;
liblas::Reader reader = f.CreateWithStream(ifs);
unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();
cloud.width = nbPoints;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize(cloud.width * cloud.height);
int i = 0;
uint16_t r1, g1, b1;
int r2, g2, b2;
uint32_t rgb;
while (reader.ReadNextPoint()) {
cloud.points[i].x = (reader.GetPoint().GetX());
cloud.points[i].y = (reader.GetPoint().GetY());
cloud.points[i].z = (reader.GetPoint().GetZ());
r1 = (reader.GetPoint().GetColor().GetRed());
g1 = (reader.GetPoint().GetColor().GetGreen());
b1 = (reader.GetPoint().GetColor().GetBlue());
r2 = ceil(((float)r1 / 65536)*(float)256);
g2 = ceil(((float)g1 / 65536)*(float)256);
b2 = ceil(((float)b1 / 65536)*(float)256);
rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);
i++;
}
}
int main() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr seed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB tempPnt;
float x1, y1, x2, y2, a1, a2, a3, b11, b22, b3;
x1 = y1 = a1 = a2 = a3 = b11 = b22 = b3 = 0;
x2 = 0; y2 = 1;
float r = 1;
float SmoothnessThreshold =6;
float CurvatureThreshold = 0.01;
cout << "读取点云" << endl;
loadLasFile("sample10.las", *cloud);
cout << "读取轨迹线" << endl;
pcl::io::loadPCDFile("trail.pcd", *trail);
cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;
cout << "构建八叉树" << endl;
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
std::vector<int>pointIdxRadiusSearch;
std::vector<int>pointIdxRadiusSearch1;
ofstream ofs;
ofs.open("seed.txt");
for (size_t i = 0; i < trail->points.size() + 500; i += 20) {
cout << "-------------------------------------------------------------" << endl;
a1 = trail->points[i].x;
a2 = trail->points[i].y;
a3 = trail->points[i].z;
b11 = trail->points[i + 20].x;
b22 = trail->points[i + 20].y;
b3 = trail->points[i + 20].z;
x1 = a1 - b11;
y1 = a2 - b22;
float cosa = (x1*x2 + y1 * y2) / (sqrt(x1*x1 + y1 * y1) + sqrt(x2*x2 + y2 * y2));
float sina = sqrt(1 - cosa * cosa);
float range = r / sina;
Eigen::Vector3f n(-100000, min(a2, b22), -100000);
Eigen::Vector3f m(100000, max(a2, b22) - 0.001, min(a3, b3));
octree.boxSearch(n, m, pointIdxRadiusSearch);
cout << i<<" 这段有点数:" << pointIdxRadiusSearch.size() << "\t";
for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j) {
tempPnt.x = cloud->points[pointIdxRadiusSearch[j]].x;
tempPnt.y = cloud->points[pointIdxRadiusSearch[j]].y;
tempPnt.z = cloud->points[pointIdxRadiusSearch[j]].z;
tempPnt.rgb = cloud->points[pointIdxRadiusSearch[j]].rgb;
part->push_back(tempPnt);
}
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree1(resolution);
octree1.setInputCloud(part);
octree1.addPointsFromInputCloud();
Eigen::Vector3f p(a1 - range, min(trail->points[i + 8].y, trail->points[i + 12].y), -100000);
Eigen::Vector3f q(a1 + range, max(trail->points[i + 8].y, trail->points[i + 12].y)-0.000001, min(a3, b3));
octree1.boxSearch(p, q, pointIdxRadiusSearch1);
cout << "这段种子数:" << pointIdxRadiusSearch1.size() << endl;
for (size_t j = 0; j < pointIdxRadiusSearch1.size(); ++j) {
tempPnt.x = part->points[pointIdxRadiusSearch1[j]].x;
tempPnt.y = part->points[pointIdxRadiusSearch1[j]].y;
tempPnt.z = part->points[pointIdxRadiusSearch1[j]].z;
tempPnt.rgb = part->points[pointIdxRadiusSearch1[j]].rgb;
seed->push_back(tempPnt);
}
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> >(new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(part);
normal_estimator.setKSearch(500);
normal_estimator.compute(*normals);
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setMinClusterSize(50);
reg.setMaxClusterSize(30000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(30);
reg.setInputCloud(part);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(SmoothnessThreshold / 180.0 * M_PI);
reg.setCurvatureThreshold(CurvatureThreshold);
std::vector <pcl::PointIndices> clusters;
reg.extract(clusters);
pcl::PointIndices cluster;
reg.getSegmentFromPoint(pointIdxRadiusSearch1[0], cluster);
cout << "取得的种子是:" << pointIdxRadiusSearch1[0] << "\t生长后点数:" << cluster.indices.size() << endl;
cout << "种子点" << part->points[pointIdxRadiusSearch1[0]].x << "\t" << part->points[pointIdxRadiusSearch1[0]].y << "\t" << part->points[pointIdxRadiusSearch1[0]].z << "\t" << endl;
cout << "-------------------------------------------------------------" << endl;
for (int j = 0; j < cluster.indices.size(); ++j) {
tempPnt.x = part->points[cluster.indices[j]].x;
tempPnt.y = part->points[cluster.indices[j]].y;
tempPnt.z = part->points[cluster.indices[j]].z;
tempPnt.rgb = part->points[cluster.indices[j]].rgb;
result->push_back(tempPnt);
}
part->clear();
partSeed->clear();
}
ofs.close();
cout << "生长后的点云保存到result.pcd,0result.pcd" << endl;
pcl::io::savePCDFileASCII("0result.pcd", *result);
cout << "保存成功" << endl;
cout << "切割后的点云保存到seedCloud,seedCloud.pcd" << endl;
pcl::io::savePCDFileASCII("0seedCloud.pcd", *seed);
cout << "保存成功" << endl;
return 0;
}
Project1,pcd输入文件,修改点云变量名
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include<fstream>
#include <string>
#include <vector>
#include <LasOperator.h>
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
using namespace std;
int main() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr seed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB tempPnt;
float x1, y1, x2, y2, a1, a2, a3, b11, b22, b3;
x1 = y1 = a1 = a2 = a3 = b11 = b22 = b3 = 0;
x2 = 0; y2 = 1;
float r = 1;
float SmoothnessThreshold =6;
float CurvatureThreshold = 0.01;
cout << "读取点云" << endl;
pcl::io::loadPCDFile("o.pcd", *cloud);
cout << "读取轨迹线" << endl;
pcl::io::loadPCDFile("trail.pcd", *trail);
cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;
cout << "构建八叉树" << endl;
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
std::vector<int>pointIdxRadiusSearch;
std::vector<int>pointIdxRadiusSearch1;
ofstream ofs;
ofs.open("seed.txt");
for (size_t i = 0; i < trail->points.size() + 500; i += 20) {
cout << "-------------------------------------------------------------" << endl;
a1 = trail->points[i].x;
a2 = trail->points[i].y;
a3 = trail->points[i].z;
b11 = trail->points[i + 20].x;
b22 = trail->points[i + 20].y;
b3 = trail->points[i + 20].z;
x1 = a1 - b11;
y1 = a2 - b22;
float cosa = (x1*x2 + y1 * y2) / (sqrt(x1*x1 + y1 * y1) + sqrt(x2*x2 + y2 * y2));
float sina = sqrt(1 - cosa * cosa);
float range = r / sina;
Eigen::Vector3f n(-100000, min(a2, b22), -100000);
Eigen::Vector3f m(100000, max(a2, b22) - 0.001, min(a3, b3));
octree.boxSearch(n, m, pointIdxRadiusSearch);
cout << i<<" 这段有点数:" << pointIdxRadiusSearch.size() << "\t";
for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j) {
tempPnt.x = cloud->points[pointIdxRadiusSearch[j]].x;
tempPnt.y = cloud->points[pointIdxRadiusSearch[j]].y;
tempPnt.z = cloud->points[pointIdxRadiusSearch[j]].z;
tempPnt.rgb = cloud->points[pointIdxRadiusSearch[j]].rgb;
part->push_back(tempPnt);
}
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree1(resolution);
octree1.setInputCloud(part);
octree1.addPointsFromInputCloud();
Eigen::Vector3f p(a1 - range, min(trail->points[i + 8].y, trail->points[i + 12].y), -100000);
Eigen::Vector3f q(a1 + range, max(trail->points[i + 8].y, trail->points[i + 12].y)-0.000001, min(a3, b3));
octree1.boxSearch(p, q, pointIdxRadiusSearch1);
cout << "这段种子数:" << pointIdxRadiusSearch1.size() << endl;
for (size_t j = 0; j < pointIdxRadiusSearch1.size(); ++j) {
tempPnt.x = part->points[pointIdxRadiusSearch1[j]].x;
tempPnt.y = part->points[pointIdxRadiusSearch1[j]].y;
tempPnt.z = part->points[pointIdxRadiusSearch1[j]].z;
tempPnt.rgb = part->points[pointIdxRadiusSearch1[j]].rgb;
seed->push_back(tempPnt);
}
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> >(new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(part);
normal_estimator.setKSearch(500);
normal_estimator.compute(*normals);
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setMinClusterSize(50);
reg.setMaxClusterSize(30000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(30);
reg.setInputCloud(part);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(SmoothnessThreshold / 180.0 * M_PI);
reg.setCurvatureThreshold(CurvatureThreshold);
std::vector <pcl::PointIndices> clusters;
reg.extract(clusters);
pcl::PointIndices cluster;
reg.getSegmentFromPoint(pointIdxRadiusSearch1[0], cluster);
cout << "取得的种子是:" << pointIdxRadiusSearch1[0] << "\t生长后点数:" << cluster.indices.size() << endl;
cout << "种子点" << part->points[pointIdxRadiusSearch1[0]].x << "\t" << part->points[pointIdxRadiusSearch1[0]].y << "\t" << part->points[pointIdxRadiusSearch1[0]].z << "\t" << endl;
cout << "-------------------------------------------------------------" << endl;
for (int j = 0; j < cluster.indices.size(); ++j) {
tempPnt.x = part->points[cluster.indices[j]].x;
tempPnt.y = part->points[cluster.indices[j]].y;
tempPnt.z = part->points[cluster.indices[j]].z;
tempPnt.rgb = part->points[cluster.indices[j]].rgb;
result->push_back(tempPnt);
}
part->clear();
partSeed->clear();
}
ofs.close();
cout << "生长后的点云保存到cloud3,0result.pcd" << endl;
pcl::io::savePCDFileASCII("0result.pcd", *result);
cout << "保存成功" << endl;
cout << "切割后的点云保存到seedCloud,seedCloud.pcd" << endl;
pcl::io::savePCDFileASCII("0seedCloud.pcd", *seed);
cout << "保存成功" << endl;
return 0;
}
Project1,分段切割、区域生长,sample100测试得到的结果没有边缘,但是不连续
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include<fstream>
#include <string>
#include <vector>
#include <LasOperator.h>
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
using namespace std;
int main() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud3(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB tempPnt;
float x1, y1, x2, y2, a1, a2, a3, b11, b22, b3;
x1 = y1 = a1 = a2 = a3 = b11 = b22 = b3 = 0;
x2 = 0; y2 = 1;
float r = 1;
float SmoothnessThreshold =6;
float CurvatureThreshold = 0.01;
std::ifstream ifs("sample10.las", std::ios::in | std::ios::binary);
liblas::ReaderFactory f;
liblas::Reader reader = f.CreateWithStream(ifs);
unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();
cloud->width = nbPoints;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize(cloud->width * cloud->height);
part->width = 0;
part->height = 1;
part->is_dense = false;
part->points.resize(part->width * part->height);
partSeed->width = 0;
partSeed->height = 1;
partSeed->is_dense = false;
partSeed->points.resize(partSeed->width * partSeed->height);
int i = 0;
uint16_t r1, g1, b1;
int r2, g2, b2;
uint32_t rgb;
while (reader.ReadNextPoint()) {
cloud->points[i].x = (reader.GetPoint().GetX());
cloud->points[i].y = (reader.GetPoint().GetY());
cloud->points[i].z = (reader.GetPoint().GetZ());
r1 = (reader.GetPoint().GetColor().GetRed());
g1 = (reader.GetPoint().GetColor().GetGreen());
b1 = (reader.GetPoint().GetColor().GetBlue());
r2 = ceil(((float)r1 / 65536)*(float)256);
g2 = ceil(((float)g1 / 65536)*(float)256);
b2 = ceil(((float)b1 / 65536)*(float)256);
rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
cloud->points[i].rgb = *reinterpret_cast<float*>(&rgb);
i++;
}
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
std::vector<int>pointIdxRadiusSearch;
std::vector<int>pointIdxRadiusSearch1;
cout << "原始点云: " << cloud->width << " \t" << cloud->points.size() << endl;
pcl::io::loadPCDFile("trail.pcd", *cloud1);
cout << "轨迹线: " << cloud1->width << " \t" << cloud1->points.size() << endl;
ofstream ofs;
ofs.open("seed.txt");
for (size_t i = 0; i < cloud1->points.size() + 500; i += 20) {
a1 = cloud1->points[i].x;
a2 = cloud1->points[i].y;
a3 = cloud1->points[i].z;
b11 = cloud1->points[i + 20].x;
b22 = cloud1->points[i + 20].y;
b3 = cloud1->points[i + 20].z;
x1 = a1 - b11;
y1 = a2 - b22;
float cosa = (x1*x2 + y1 * y2) / (sqrt(x1*x1 + y1 * y1) + sqrt(x2*x2 + y2 * y2));
float sina = sqrt(1 - cosa * cosa);
float range = r / sina;
Eigen::Vector3f n(-100000, min(a2, b22), -100000);
Eigen::Vector3f m(100000, max(a2, b22) - 0.001, min(a3, b3));
octree.boxSearch(n, m, pointIdxRadiusSearch);
cout << "这段有点数:" << pointIdxRadiusSearch.size() << endl;
for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j) {
tempPnt.x = cloud->points[pointIdxRadiusSearch[j]].x;
tempPnt.y = cloud->points[pointIdxRadiusSearch[j]].y;
tempPnt.z = cloud->points[pointIdxRadiusSearch[j]].z;
tempPnt.rgb = cloud->points[pointIdxRadiusSearch[j]].rgb;
part->push_back(tempPnt);
}
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree1(resolution);
octree1.setInputCloud(part);
octree1.addPointsFromInputCloud();
Eigen::Vector3f p(a1 - range, min(cloud1->points[i + 8].y, cloud1->points[i + 12].y), -100000);
Eigen::Vector3f q(a1 + range, max(cloud1->points[i + 8].y, cloud1->points[i + 12].y)-0.000001, min(a3, b3));
octree1.boxSearch(p, q, pointIdxRadiusSearch1);
cout << i<<"\t"<<"这段种子数:" << pointIdxRadiusSearch1.size() << endl;
for (size_t j = 0; j < pointIdxRadiusSearch1.size(); ++j) {
tempPnt.x = part->points[pointIdxRadiusSearch1[j]].x;
tempPnt.y = part->points[pointIdxRadiusSearch1[j]].y;
tempPnt.z = part->points[pointIdxRadiusSearch1[j]].z;
tempPnt.rgb = part->points[pointIdxRadiusSearch1[j]].rgb;
seedCloud->push_back(tempPnt);
}
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> >(new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(part);
normal_estimator.setKSearch(500);
normal_estimator.compute(*normals);
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setMinClusterSize(50);
reg.setMaxClusterSize(30000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(30);
reg.setInputCloud(part);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(SmoothnessThreshold / 180.0 * M_PI);
reg.setCurvatureThreshold(CurvatureThreshold);
std::vector <pcl::PointIndices> clusters;
reg.extract(clusters);
pcl::PointIndices cluster;
for (int j = 0;j < pointIdxRadiusSearch1.size();++j) {
reg.getSegmentFromPoint(pointIdxRadiusSearch1[0], cluster);
for (int j = 0; j < cluster.indices.size(); ++j) {
tempPnt.x = part->points[cluster.indices[j]].x;
tempPnt.y = part->points[cluster.indices[j]].y;
tempPnt.z = part->points[cluster.indices[j]].z;
tempPnt.rgb = part->points[cluster.indices[j]].rgb;
cloud3->push_back(tempPnt);
}
}
for (int j = 0; j < cluster.indices.size(); ++j) {
tempPnt.x = part->points[cluster.indices[j]].x;
tempPnt.y = part->points[cluster.indices[j]].y;
tempPnt.z = part->points[cluster.indices[j]].z;
tempPnt.rgb = part->points[cluster.indices[j]].rgb;
cloud3->push_back(tempPnt);
}
pointIdxRadiusSearch.clear();
pointIdxRadiusSearch1.clear();
part->clear();
partSeed->clear();
}
ofs.close();
cout << "生长后的点云保存到cloud3,regionGrow.pcd" << endl;
cloud3->height = 1;
cloud3->is_dense = false;
pcl::io::savePCDFileASCII("0regionGrow.pcd", *cloud3);
cout << "保存成功" << endl;
cout << "切割后的点云保存到seedCloud,seedCloud.pcd" << endl;
seedCloud->height = 1;
seedCloud->is_dense = false;
pcl::io::savePCDFileASCII("0seedCloud.pcd", *seedCloud);
cout << "保存成功" << endl;
return 0;
}