vector subscript out of range怎么解决？
#include <iostream>
#include<string>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/centroid.h>
#include <Eigen/Dense>
using Eigen::MatrixXf;
using Eigen::JacobiSVD;
using Eigen::VectorXf;
int num_iter_=10;
int num_lpr_=5000;
double th_seeds_=-0.5;
double th_dist_=1;
namespace scan_line_run
{
/** Euclidean Velodyne coordinate, including intensity and ring number, and label. */
struct PointXYZIRL
{
PCL_ADD_POINT4D; // quad-word XYZ
float intensity; ///< laser intensity reading
uint16_t ring; ///< laser ring number
uint16_t label; ///< point label
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment
};
}; // namespace scan_line_run
#define SLRPointXYZIRL scan_line_run::PointXYZIRL
pcl::PointCloud<pcl::PointXYZ>::Ptr g_seeds_pc(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr g_ground_pc(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr g_not_ground_pc(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<SLRPointXYZIRL>::Ptr g_all_pc(new pcl::PointCloud<SLRPointXYZIRL>());
void extract_initial_seeds_(pcl::PointCloud<pcl::PointXYZ> p_sorted) {
// LPR is the mean of low point representative
double sum = 0;
int cnt = 0;
// Calculate the mean height value.
for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; i++) {
sum += p_sorted.points[i].z;
cnt++;
}
double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0
g_seeds_pc->clear();
// iterate pointcloud, filter those height is less than lpr.height+th_seeds_
for (int i = 0; i <p_sorted.size(); i++) {
if (p_sorted.points[i].z < lpr_height + th_seeds_) {
g_seeds_pc->points.push_back(p_sorted.points[i]);
}
}
/*
g_seeds_pc->height = 1;
g_seeds_pc->width = g_seeds_pc->points.size();
g_seeds_pc->is_dense = true;
pcl::PCDWriter writer;
writer.write("testnew3.pcd", *g_seeds_pc, false);
*/
// return seeds points
}
int d_;
MatrixXf normal_;
double th_dist_d_;//阈值
void estimate_plane_(void) {
// Create covarian matrix in single pass.
// TODO: compare the efficiency.
Eigen::Matrix3f cov;
Eigen::Vector4f pc_mean;
pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
// Singular Value Decomposition: SVD
JacobiSVD<MatrixXf> svd(cov, Eigen::DecompositionOptions::ComputeFullU);
// use the least singular vector as normal
normal_ = (svd.matrixU().col(2));
// mean ground seeds value
Eigen::Vector3f seeds_mean = pc_mean.head<3>();
// according to normal.T*[x,y,z] = -d
d_ = -(normal_.transpose() * seeds_mean)(0, 0);
// set distance threhold to `th_dist - d`
th_dist_d_ = th_dist_ - d_;
// return the equation parameters
}
bool point_cmp(pcl::PointXYZ a, pcl::PointXYZ b) {
return a.z < b.z;
}
int main()
{
//读取pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//打开点云
if (pcl::io::loadPCDFile <pcl::PointXYZ>("testnew3.pcd", *cloud) == -1)
{
std::cout << "Cloud reading failed." << std::endl;
return (-1);
}
pcl::PCDWriter writer;
//采用体素滤波器来实现降采样
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFilterVoxelGrid(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::ApproximateVoxelGrid<pcl::PointXYZ> sor;
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setLeafSize(0.05f, 0.05f, 0.05f);
sor.filter(*cloudFilterVoxelGrid);
//平移
Eigen::Affine3f transformM = Eigen::Affine3f::Identity();
transformM.translation() << 5.0, 0.0, 0.0;
transformM.rotate(Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitZ()));
pcl::PointCloud<pcl::PointXYZ>::Ptr pingyi(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pingyi2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloudFilterVoxelGrid, *pingyi, transformM); //pcl::transformPointCloud(*source_cloud, *target_cloud, transform) transform是转换矩阵
pingyi2 = pingyi;
sort(pingyi->points.begin(), pingyi->end(), point_cmp);
//提取种子点
extract_initial_seeds_(*pingyi);
g_ground_pc = g_seeds_pc;
// 5. Ground plane fitter mainloop进行迭代拟合，拟合次数为num_iter_
for (int i = 0; i < num_iter_; i++) {
//拟合平面
estimate_plane_();
//g_ground_pc->clear();
//g_not_ground_pc->clear();
//pointcloud to matrix基于拟合的平面重新选择地面点，再进行拟合平面
MatrixXf points(pingyi2->points.size(), 3);
int j = 0;
for (auto p : pingyi2->points) {
points.row(j++) << p.x, p.y, p.z;
}
// ground plane model
VectorXf result = points * normal_;//计算所有点云到拟合后平面的距离d result=Ax+By+Cz
// threshold filter以阈值进行过滤
for (int r = 0; r < result.rows(); r++) {
if (result[r] < th_dist_d_) {
g_all_pc->points[r].label = 1u;// means ground 如果d小于阈值th_dist_d_，则认为该点是地面，存入地面点中，作为下次地面拟合的参考点
g_ground_pc->points.push_back(pingyi2->points[r]);
//writer.write<pcl::PointXYZ>("test" + std::to_string(i) + ".pcd", *g_ground_pc, false);
}
else {
g_all_pc->points[r].label = 0u;// means not ground and non clusterred
g_not_ground_pc->points.push_back(pingyi2->points[r]);
}
}
g_ground_pc->height = 1;
g_ground_pc->width = g_ground_pc->points.size();
g_ground_pc->is_dense = true;
writer.write("testnew.pcd", *g_ground_pc, false);
}
}
vector subscript out of range
