The main purpose is to crop the point cloud based on the known ROI area. Either leave the point cloud ROI area or remove it.
The ROI area is generally a rectangle, that is (x, y, width, height).
Then the encapsulated function form is generally as follows:
pcl::PointCloud<pcl::PointXYZ>::Ptr CloudClipper(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,double x,double y, double width, double height)
{
// 实现点云滤波
// 创建滤波后点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
// 调用filter方法得到滤波后点云
return cloud_filtered;
}
A simpler, more direct and crude method is to use straight-through filtering
#include <pcl/filters/passthrough.h>
pcl::PassThrough<pcl::PointXYZ> pass; //创建直通滤波器对象
pass.setInputCloud(pointCloud_raw); //设置输入的点云
pass.setFilterFieldName("z"); //设置过滤时所需要点云类型为Z字段
pass.setFilterLimits(-0.1, 10); //设置在过滤字段的范围
pass.setFilterLimitsNegative(true); //设置保留还是过滤掉字段范围内的点，设置为true表示过滤掉字段范围内的点
pass.filter(*cloud_filtered); //执行滤波
There is actually a conditional filter in the PCL library, and it sounded really similar to the filter I wanted to use, so I tried it.
#include <pcl/filters/conditional_removal.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr ConditionFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double x, double y, double width, double height)
{
//创建条件限定下的滤波器
//pcl::ConditionBase
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());
//创建条件定义对象
//为条件定义对象添加比较算子: 使用大于0.0和小于0.8这两个条件用于建立滤波器。
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, x)));
//添加在x字段上大于0的比较算子
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, x + width)));
//添加在x字段上小于0.8的比较算子
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GT, y)));
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::LT, y + height)));
//创建滤波器并用条件定义对象初始化
pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud(cloud); //设置输入点云
condrem.setKeepOrganized(false); //设置保持点云的结构:为true时被剔除的点为NAN
//condrem.setUserFilterValue(0.1f);
//condrem.getIndices
pcl::IndicesConstPtr inliers = condrem.getRemovedIndices();
if (inliers != nullptr)
{
std::cout << "indice number: " << inliers->size() << std::endl;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
condrem.filter(*cloud_filtered); //执行条件滤波，存储结果到cloud_filtered
std::cout << "filter size: " << cloud_filtered->points.size() << std::endl;
/* pcl::IndicesConstPtr inliers = condrem.getRemovedIndices();
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, *inliers, *cloud_filtered);*/
return cloud_filtered;
}
Line-by-line explanation:
1. First, you need to create a condition definition object for setting conditions. In fact, there are two main types of conditions: pcl::ConditionAndpcl::PointT::Ptr and pcl::ConditionOrpcl::PointT::Ptr. The former is the condition, which means that all conditions must be met. In fact, it is the point cloud obtained by each condition. To find the intersection, the latter is an OR condition, then the union of the filtering results of each condition is found. Can be used as needed.
2. Set the field, that is, range_cond->addComparison(pcl::FieldComparisonpcl::PointXYZ::ConstPtr(new
pcl::FieldComparisonpcl::PointXYZ(“x”, pcl::ComparisonOps::GT, x))); "x" actually refers to the x coordinate value of the point cloud. If "r" can filter out the value of the R channel in RGB, then the data structure of the point cloud must be PointXYZRGB instead of PointXYZ. This should be noted. In addition, for pcl::ComparisonOps::GT and pcl::ComparisonOps::LT, in fact, GT is greater than, which is greater than, and LT is less than, which is less than.
3.//Create the filter and initialize it with the condition definition object
pcl::ConditionalRemovalpcl::PointXYZ condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud(cloud); //Set the input point cloud.
The above sentences are nothing more than instantiating the filter. After selecting the object, enter the conditions and point clouds set above.
4.setKeepOrganized is used to maintain the order of the point cloud after removing conditions. However, the point clouds generally processed are unordered point clouds. In most cases, this place is set to false, but it must be determined according to the actual situation. Depends. If there is no clearly defined topological relationship between points in an unordered point cloud, such as no clear connection relationship or boundary relationship, then after removing some points in the point cloud, the ordered properties of the point cloud will also be destroyed. . At this time, even if setKeepOrganized is set to true, the output point cloud is still unordered.
5.setUserFilterValue: This function is used to set the threshold parameter for conditional removal. For some conditions, such as Euclidean distance, a threshold needs to be specified for removal. The setUserFilterValue function can set this threshold. This function needs to pass a template parameter, indicating the type of threshold, which can be float, double, int, etc. When using the setUserFilterValue function, you should set an appropriate threshold according to the actual situation to avoid removing too many or too few points.
6. The conditional filter can also get
condrem.getIndices();
condrem.getRemovedIndices(); that is, get the index of the point and the index of the removed point. Some filters have a setNegative method. When set to true, you can get the points filtered out by the filter. When set to false, you can get the points left by the filter. But there is no such method in conditional filters. So I want to get the index of the removed points, and then extract the points removed by the filter through pcl::copyPointCloudpcl::PointXYZ(*cloud, *inliers, *cloud_filtered);. It turns out that the size in the index obtained by the filter is 0, that is, there is no index. There may be several reasons:
在执行条件滤波操作之前，没有设置条件对象。条件滤波器必须先设置条件对象，才能根据条件对点云进行筛选。如果没有设置条件对象，则条件滤波器会将输入点云中的所有点都保留下来，因此“已移除索引”列表中就没有任何点。
设置的条件不满足任何点。如果设置的条件不满足输入点云中的任何点，则条件滤波器不会移除任何点，因此“已移除索引”列表中也就没有任何点。
使用的数据类型不正确。条件滤波器的输入点云和条件对象都必须是相同的数据类型，否则条件滤波器会出现异常，导致“已移除索引”列表为空。例如，如果输入点云是 XYZRGB 类型的，而条件对象是 XYZ 类型的，则条件滤波器会出现异常。
In fact, there is no problem in the above three situations. But why is size 0? Is it because there is no index in the point cloud data structure itself? ? ?
And using condrem.getIndices(); returns a nullptr null pointer for the following reasons:
1. The input point cloud is not set before performing the conditional filtering operation. The conditional filter must set the input point cloud to filter the point cloud based on conditions. If the input point cloud is not set, the pointer returned by the getIndices() function is nullptr.
2. The conditional filter does not remove any points. If the set condition does not meet any point in the input point cloud, or the input point cloud itself already meets the condition, the conditional filter will not remove any points, so the pointer returned by the getIndices() function is nullptr.
3. Index output is not enabled. Conditional filters by default do not output the indices of removed points. If you want to output the index of the removed point, you need to enable index output on the conditional filter, that is, pcl::ConditionalRemovalpcl::PointXYZ condrem;
condrem.setKeepOrganized(true); // Enable index output
After setting setKeepOrganized to true, getIndices The returned pointer is not empty, but size is the number of input point clouds. and! ! ! At this point, the point cloud cannot be filtered out.
Later, I discovered that in fact, after pcl::IndicesPtr outliers = condrem.getIndices(); is placed after condrem.filter(*cloud_filtered);, the returned pointer is not empty, but its size is still consistent with the size of the input point cloud midpoint. .
Therefore, currently, the point cloud clipped from the point cloud can be obtained through conditional filtering, but points other than the clipped points cannot be obtained.
Cropping using crop_box
The code is as follows and can be used directly
pcl::PointCloud<pcl::PointXYZ>::Ptr cropclipper3D(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double x, double y, double width, double height)
{
pcl::PointXYZ min_point(x, y, -100);
pcl::PointXYZ max_point(x + width, y + height, 100);
Eigen::Vector4f minpt(x, y, -100, 1);
Eigen::Vector4f maxpt(x + width, y + height, 100, 1);
pcl::CropBox<pcl::PointXYZ> crop_box;
crop_box.setMin(minpt);
crop_box.setMax(maxpt);
// 将点云限制在 3D 盒子内部或者外部，并保存输出点云
pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud(new pcl::PointCloud<pcl::PointXYZ>);
crop_box.setInputCloud(cloud);
crop_box.setNegative(true);
crop_box.filter(*clipped_cloud);
return clipped_cloud;
}
This filter needs to first determine the two coordinate points of the box, which must be the two points on the diagonal (the two furthest points of a cuboid). Then this filter has the setNegative method we mentioned before, which allows you to obtain points within the ROI or outside the area according to your wishes. The effect is as shown in the picture below. It feels good. The
running time is about 44ms. This speed feels good. It would be better if it could be faster. If there are many areas on a plane point cloud that need to be extracted or cropped, multi-threading can be considered.
Next, I wanted to study how to use BoxClipper3D, but after looking for it for a long time, I finally found it on github. Click here to check it out! This should be PCL's official test code.
#include <pcl/test/gtest.h>
#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/pcl_base.h> // for pcl::Indices
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/filters/box_clipper3D.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/eigen.h>
using namespace pcl;
using namespace Eigen;
//
TEST (BoxClipper3D, Filters)
{
// PointCloud
// -------------------------------------------------------------------------
// Create cloud with center point and corner points
PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
input->push_back (PointXYZ (0.0f, 0.0f, 0.0f));
input->push_back (PointXYZ (0.9f, 0.9f, 0.9f));
input->push_back (PointXYZ (0.9f, 0.9f, -0.9f));
input->push_back (PointXYZ (0.9f, -0.9f, 0.9f));
input->push_back (PointXYZ (-0.9f, 0.9f, 0.9f));
input->push_back (PointXYZ (0.9f, -0.9f, -0.9f));
input->push_back (PointXYZ (-0.9f, -0.9f, 0.9f));
input->push_back (PointXYZ (-0.9f, 0.9f, -0.9f));
input->push_back (PointXYZ (-0.9f, -0.9f, -0.9f));
ExtractIndices<PointXYZ> extract_indices;
pcl::Indices indices;
BoxClipper3D<PointXYZ> boxClipper3D (Affine3f::Identity ());
boxClipper3D.clipPointCloud3D (*input, indices);
PointCloud<PointXYZ> cloud_out;
extract_indices.setInputCloud (input);
extract_indices.setIndices (pcl::make_shared<pcl::Indices> (indices));
extract_indices.filter (cloud_out);
EXPECT_EQ (int (indices.size ()), 9);
EXPECT_EQ (int (cloud_out.size ()), 9);
EXPECT_EQ (int (cloud_out.width), 9);
EXPECT_EQ (int (cloud_out.height), 1);
// Translate points by 1 in Y-axis ...
Affine3f t (Translation3f (0.0f, 1.0f, 0.0f));
boxClipper3D.setTransformation (t);
boxClipper3D.clipPointCloud3D (*input, indices);
EXPECT_EQ (int (indices.size ()), 5);
// ... then rotate points +45 in Y-Axis
t.rotate (AngleAxisf (45.0f * static_cast<float>(M_PI) / 180.0f, Vector3f::UnitY ()));
boxClipper3D.setTransformation (t);
boxClipper3D.clipPointCloud3D (*input, indices);
EXPECT_EQ (int (indices.size ()), 1);
// ... then rotate points -45 in Z-axis
t.rotate (AngleAxisf (-45.0f * static_cast<float>(M_PI) / 180.0f, Vector3f::UnitZ ()));
boxClipper3D.setTransformation (t);
boxClipper3D.clipPointCloud3D (*input, indices);
EXPECT_EQ (int (indices.size ()), 3);
// ... then scale points by 2
t.scale (2.0f);
boxClipper3D.setTransformation (t);
boxClipper3D.clipPointCloud3D (*input, indices);
EXPECT_EQ (int (indices.size ()), 1);
}
In this official test code, EXPECT_EQ
EXPECT_EQ is a macro in Google Test, used to compare whether two values are equal. When two values are not equal, it produces a failed test result. The usage format of the EXPECT_EQ macro is as follows:
EXPECT_EQ(expected_value, actual_value);
Among them, expected_value represents the expected value, and actual_value represents the actual value. If the values of expected_value and actual_value are equal, the test will pass, otherwise the test will fail and the values of expected_value and actual_value will be output.
The EXPECT_EQ macro is suitable for comparing basic data types such as integers, floating point, characters, and strings. If you want to compare values of custom types, you can do so by overloading the operator== operator.
You can customize a macro MY_ASSERT_EQ according to this function, and the effect will be similar.
#include <iostream>
// 自定义宏
#define MY_ASSERT_EQ(expected, actual) \
if ((expected) != (actual)) {
\
std::cerr << "Assertion failed: " #expected " == " #actual \
<< ", expected " << (expected) << ", but got " << (actual) \
<< " at " << __FILE__ << ":" << __LINE__ << std::endl; \
}
This BoxClipper3D class mainly implements the use of a Box (a cube in space, imagined as a small box) to clip point clouds. The points that fall into this box are kept and the rest are removed. We don’t need to add this box ourselves, but we can change it according to our needs. The center of mass of the box is at the origin of the coordinate system without artificial change, and each edge length is 2. In fact, these eight vertices can be written:
// 盒子的质心
pcl::PointXYZ(0.0f, 0.0f, 0.0f);
// 其余的八个顶点
pcl::PointXYZ(1, 1, 1);
pcl::PointXYZ(1, 1, -1);
pcl::PointXYZ(1, -1, 1);
pcl::PointXYZ(-1, 1, 1);
pcl::PointXYZ(1, -1, -1);
pcl::PointXYZ(-1, -1, 1);
pcl::PointXYZ(-1, 1, -1);
pcl::PointXYZ(-1, -1, -1);
If you can't imagine it, you can draw it on paper and it will appear.
Since the box is given, how can we use it to flexibly tailor the point cloud according to our ideas. First, you need to define an affine transformation matrix, which is 3x3 and is declared directly through Eigen::Affine3f T; in PCL. There is a member function void setTransformation (const Eigen::Affine3f &transformation) in the BoxClipper3D class that can set the affine transformation matrix we gave. Then this box will be transformed by the affine transformation matrix we set. Finally, the points where the point cloud falls within this box are retained. We all know that the affine transformation matrix can perform scaling, translation, rotation and other transformations. That is to say, through an affine transformation matrix, the box can be scaled, translated, rotated and other operations, and the box will be transformed to the shape we want to crop. place.
#include <pcl/filters/box_clipper3D.h>
BoxClipper3D (const Eigen::Affine3f &transformation)
// 利用仿射变换的矩阵transformation来构造滤波对象，最终输出点云为落在通过仿射变换矩阵变换后的立方体内的点集,立方体默认为以原点为中心.XYZ方向.尺度为2的立方体。
BoxClipper3D (const Eigen::Vector3f &rodrigues, const Eigen::Vector3f &translation, const Eigen::Vector3f &box_size)
// 功能同上,利用三个向量构造滤波对象,其中rodrigues用来指定变换的立方体的姿态,translation用来指定变换立方体的位置,box_size用来指定变换立方体各个方向的缩放系数,最终由三个向量指定-一个完整的放射变换矩阵。
void setTransformation (const Eigen::Affine3f &transformation)
// 设置仿射变换矩阵transformation。
void setTransformation (const Eigen::Vector3f &rodrigues, const Eigen::Vector3f &translation, const Eigen::Vector3f &box_size)
// 通过三个向量设置仿射变换,参数同上。
virtual ~BoxClipper3D () throw ()
// 析构函数
virtual bool clipPoint3D (const PointT &point) const
// 裁剪掉一个单点point的接口:如果点被裁剪掉则返回true,点不在裁剪空间则返回false.
virtual bool clipLineSegment3D (PointT &from, PointT &to) const
// 通过输入线段起始点from和终点to,裁剪掉一.条线段的接口:若线段被裁剪掉则返回true,若线段在剪贴空间外则返回false.
virtual void clipPlanarPolygon3D (std::vector< PointT, Eigen::aligned_allocator< PointT > > &polygon) const
// 通过一个有序点列表,提供裁剪一个平面多边形的接口。其中,polygon是指定的任何方向(ccw or cw).上的多边形,注意有序点,才能保证两个相邻的点定义多边形的边界。
virtual void clipPlanarPolygon3D (const std::vector< PointT, Eigen::aligned_allocator< PointT > > &polygon, std::vector< PointT, Eigen::aligned_allocator< PointT > > &clipped_polygon) const
// 通过一个有序点列表,提供裁剪一个平面多边形的接口:输入多边形polygon,输出裁剪后多边形clipped_ polygon。
virtual void clipPointCloud3D (const pcl::PointCloud< PointT > &cloud_in, std::vector< int > &clipped, const std::vector< int > &indices=std::vector< int >()) const
// 裁剪点云的接口
virtual Clipper3D< PointT > * clone () const
// 使用其参数克隆基础剪贴器的多态方法。
Generally speaking, we use the above clipPointCloud3D for clipping. The first parameter is passed in the point cloud to be clipped. The second parameter directly declares a pcl::Indices indices; and is passed in. In the end, this algorithm will The indices of points falling within the box are entered into indices.
The point cloud can be extracted using the following code:
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::Indices indices;
extract_indices.setInputCloud(input);
extract_indices.setIndices(pcl::make_shared<pcl::Indices>(indices));
extract_indices.filter(cloud_out);
// 下述代码应该也可以实现，效果一样
//pcl::copyPointCloud<pcl::PointXYZ>(*input, indices, cloud_out);
During the use, I found that this thing is very mysterious. I don’t know if it is my personal understanding. I tried to affine transform this Box in the way I wanted. I first created an affine transformation matrix based on my idea, the code as follows
Eigen::Affine3f t = Eigen::Affine3f::Identity();
t.scale(Eigen::Vector3f( 0.5f, 1.0f, 1.0f));
//t.translate(Eigen::Vector3f(-x * 1.0f, -y * 1.0f, -11.7 * 1.0f));
t.translation() << -x * 1.0f, -y * 1.0f, -11.7 * 1.0f; //x = -16，y = 58
std::cout << "t : " << t.matrix() << std::endl;
There is nothing wrong with the obtained affine transformation matrix
: 0.5 0 0 16
0 1 0 -58
0 0 1 -11.7
0 0 0 1.
But the magic is that no points can be cropped. So I created 8 points, representing the 8 vertices of the box respectively, and then performed affine transformation on the point cloud. The result was then visualized and I found that the box was actually on the point cloud. As for the box that can be clipped, I also visualized it and found that the box was far away from the point cloud.
// 进行仿射变换
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*Box, *transformed_cloud, t);
As can be seen from the picture below, the box can frame part of the point cloud, but in this case no points can be obtained.
As can be seen from the picture below, the box does not frame any points, but it can crop the points.
At present, it is very doubtful whether the object of affine transformation is a box or a point cloud.
What is mentioned here is to perform affine transformation on the box