PCL cropBox note

cropBox的使用

CropBox 常用于点云立方体滤波

添加#include <pcl/filters/crop_box.h>

源码阅读

class CropBox : public FilterIndices<PointT>
  {
    using Filter<PointT>::getClassName;

    using PointCloud = typename Filter<PointT>::PointCloud;
    using PointCloudPtr = typename PointCloud::Ptr;
    using PointCloudConstPtr = typename PointCloud::ConstPtr;

    public:

      using Ptr = shared_ptr<CropBox<PointT> >;
      using ConstPtr = shared_ptr<const CropBox<PointT> >;

      /** \brief Constructor.
        * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
        */
      CropBox (bool extract_removed_indices = false) :
        FilterIndices<PointT> (extract_removed_indices),
        min_pt_ (Eigen::Vector4f (-1, -1, -1, 1)),
        max_pt_ (Eigen::Vector4f (1, 1, 1, 1)),
        rotation_ (Eigen::Vector3f::Zero ()),
        translation_ (Eigen::Vector3f::Zero ()),
        transform_ (Eigen::Affine3f::Identity ())
      {
        filter_name_ = "CropBox";
      }
    //设置box中最小的点
     inline void
      setMin (const Eigen::Vector4f &min_pt)
      {
        min_pt_ = min_pt;
      }
    //返回box中的最小点
     inline Eigen::Vector4f
      getMin () const
      {
        return (min_pt_);
      }
    //与box中最大点有关
     inline void
      setMax (const Eigen::Vector4f &max_pt)
      {
        max_pt_ = max_pt;
      }

      /** \brief Get the value of the maximum point of the box, as set by the user
        * \return the value of the internal \a max_pt parameter.
        */
      inline Eigen::Vector4f
      getMax () const
      {
        return (max_pt_);
      }
    //设置位移分量 (tx,ty,tz)
    inline void
      setTranslation (const Eigen::Vector3f &translation)
      {
        translation_ = translation;
      }
    inline void
      setRotation (const Eigen::Vector3f &rotation)
      {
        rotation_ = rotation;
      }
    
     //设置旋转向量(rx,ry,rz) 
      inline void
      setRotation (const Eigen::Vector3f &rotation)
      {
        rotation_ = rotation;
      }

      /** \brief Get the value of the box rotatation parameter, as set by the user. */
      inline Eigen::Vector3f
      getRotation () const
      {
        return (rotation_);
      }
    
    //通过提供变换矩阵
      inline void
      setTransform (const Eigen::Affine3f &transform)
      {
        transform_ = transform;
      }

      /** \brief Get the value of the transformation parameter, as set by the user. */
      inline Eigen::Affine3f
      getTransform () const
      {
        return (transform_);
      }
    
    protected:
      using PCLBase<PointT>::input_;
      using PCLBase<PointT>::indices_;
      using Filter<PointT>::filter_name_;
      using FilterIndices<PointT>::negative_;
      using FilterIndices<PointT>::keep_organized_;
      using FilterIndices<PointT>::user_filter_value_;
      using FilterIndices<PointT>::extract_removed_indices_;
      using FilterIndices<PointT>::removed_indices_;

      /** \brief Sample of point indices
        * \param[out] indices the resultant point cloud indices
        */
      void
      applyFilter (std::vector<int> &indices) override;
    private:
      /** \brief The minimum point of the box. */
      Eigen::Vector4f min_pt_;
      /** \brief The maximum point of the box. */
      Eigen::Vector4f max_pt_;
      /** \brief The 3D rotation for the box. */
      Eigen::Vector3f rotation_;
      /** \brief The 3D translation for the box. */
      Eigen::Vector3f translation_;
      /** \brief The affine transform applied to the cloud. */
      Eigen::Affine3f transform_;
  }


template<typename PointT> void
pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
{
  indices.resize (input_->size ());//输入点云
  removed_indices_->resize (input_->size ());//要删除的点云
  int indices_count = 0;
  int removed_indices_count = 0;

  Eigen::Affine3f transform = Eigen::Affine3f::Identity ();//RT变换矩阵
  Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity ();//RT变换矩阵的逆

    //rotation_中是欧拉角 rx ry rz 将其变换成4*4 RT矩阵，位移部分为0，0，0
  if (rotation_ != Eigen::Vector3f::Zero ())
  {
    pcl::getTransformation (0, 0, 0,
                            rotation_ (0), rotation_ (1), rotation_ (2),
                            transform);
    inverse_transform = transform.inverse ();
  }

  //RT矩阵是否是单位阵
  bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
  //位移分量是否全为0
  bool translation_is_zero = (translation_ == Eigen::Vector3f::Zero ());
  //RT矩阵的逆是不是单位阵
  bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();

  for (const auto index : *indices_)
  {
    if (!input_->is_dense)//检查该点的有效性
      // Check if the point is invalid
      if (!isFinite ((*input_)[index]))//检测点云的数据中是否包含NaN
        continue;

    // Get local point
    PointT local_pt = (*input_)[index];

    //对local_pt左乘transform_ RT矩阵 进行旋转
    // Transform point to world space 将点云转换到世界空间
    if (!transform_matrix_is_identity)
      local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
    //对local_pt的X Y Z进行调整
    if (!translation_is_zero)
    {
      local_pt.x -= translation_ (0);
      local_pt.y -= translation_ (1);
      local_pt.z -= translation_ (2);
    }
    //将点云变换到裁剪框的局部空间
    // Transform point to local space of crop box
    if (!inverse_transform_matrix_is_identity)
      local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
     //点是否在裁剪框外部
    // If outside the cropbox
    if ( (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2]) ||
         (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2]))
    {
      if (negative_)
        indices[indices_count++] = index;
      else if (extract_removed_indices_)
        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
    }
    // If inside the cropbox
    else
    {
      if (negative_ && extract_removed_indices_)
        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
      else if (!negative_) 
        indices[indices_count++] = index;
    }
  }
  indices.resize (indices_count);
  removed_indices_->resize (removed_indices_count);
}

算子作用说明

1、初始化cropBox时，默认中心在原点处，生成一个，边长为2的正方体

CropBox (bool extract_removed_indices = false) :
        FilterIndices<PointT> (extract_removed_indices),
        min_pt_ (Eigen::Vector4f (-1, -1, -1, 1)),
        max_pt_ (Eigen::Vector4f (1, 1, 1, 1)),
        rotation_ (Eigen::Vector3f::Zero ()),
        translation_ (Eigen::Vector3f::Zero ()),
        transform_ (Eigen::Affine3f::Identity ())
      {
        filter_name_ = "CropBox";
      }

pcl::CropBox<pcl::PointXYZ> box_filter2;//滤波器对象

	auto rt= box_filter2.getTransform();
	auto t = box_filter2.getTranslation();
	auto r = box_filter2.getRotation();
	std::cout << rt.matrix() << std::endl;//4*4 单位阵
	std::cout << t.matrix() << std::endl;//0 0 0
	std::cout << r.matrix() << std::endl;//0 0 0

2、将OBB的结果用于cropBox前，需要知道OBB与原点之间的转换关系，即RT矩阵，指的是先进行旋转，再平移

//绘制点云OBB
	pcl::PointXYZ min_point_OBB;
	pcl::PointXYZ max_point_OBB;
	pcl::PointXYZ position_OBB;
	Eigen::Matrix3f rotational_matrix_OBB;
	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> estimBox;
	estimBox.setInputCloud(cloud);
	estimBox.compute();
	estimBox.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);

Eigen::Vector4f minPoints, maxPoints;
	Eigen::Vector3f translasion = Eigen::Vector3f(position_OBB.x, position_OBB.y, position_OBB.z);//得到位移分量
	Eigen::Vector3f eulerAngle = rotational_matrix_OBB.eulerAngles(2, 1, 0);//将旋转矩阵转成欧拉角（Z-Y-X）z
//在cropBox使用的是（rx,ry,rz）而不是（rz,ry,rx）
	Eigen::Vector3f eulerAngle2 = Eigen::Vector3f(eulerAngle(2), eulerAngle(1), eulerAngle(0));

3、进行立方体滤波

pcl::CropBox<pcl::PointXYZ> box_filter;//滤波器对象
	minPoints = Eigen::Vector4f(min_point_OBB.x,min_point_OBB.y*0.5,min_point_OBB.z,1.0f);
	maxPoints = Eigen::Vector4f(max_point_OBB.x, max_point_OBB.y*0.5, max_point_OBB.z, 1.0f);
	box_filter.setInputCloud(cloud);
	box_filter.setMin(minPoints);//在y方向上收缩了一半
	box_filter.setMax(maxPoints);
	box_filter.setTranslation(translasion);//点在原始坐标系下的坐标
	box_filter.setRotation(eulerAngle2);//rx,ry,rz
	box_filter.setNegative(false);//保留OBB框内的点
	box_filter.filter(*flit);

4、结果