How can I rotate the pcl::CropBox wrt its own particular axis rather than global axis? or how can I apply affine transform to pcl::CropBox?

Question

I have a cube-shaped point cloud whose x, y, & z-coordinates range from -1.0 to 1.0 unit.

#include <pcl/ModelCoefficients.h>
#include <pcl/common/common.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <iostream>
#include <string>
#include <vector>

int main() {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr
        main_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>),
        cropped_cloud_ptr1(new pcl::PointCloud<pcl::PointXYZRGB>),
        cropped_cloud_ptr2(new pcl::PointCloud<pcl::PointXYZRGB>);

    float x_start = -1.0, x_end = 1.0, x_resolution = 0.05;
    float y_start = -1.0, y_end = 1.0, y_resolution = 0.05;
    float z_start = -1.0, z_end = 1.0, z_resolution = 0.05;

    float rgb_color_mul = 5.0;
    float rgb_color_add = 10.0;

    pcl::PointXYZRGB pt;

    for (float i = x_start; i < x_end; i += x_resolution) {
        for (float j = y_start; j < y_end; j += y_resolution) {
            for (float k = z_start; k < z_end; k += z_resolution) {
                pt.x = i;
                pt.y = j;
                pt.z = k;
                pt.r = i * 100 + 100;
                pt.g = 200;
                pt.b = j * 100 + 50;
                main_cloud_ptr->points.push_back(pt);
            }
        }
    }

    std::cout << "main_cloud_ptr->points.size() = " << main_cloud_ptr->points.size() << "\n";

    pcl::visualization::PCLVisualizer::Ptr visualizer1(new pcl::visualization::PCLVisualizer("visualizer1 -- main_cloud_ptr"));
    visualizer1->setBackgroundColor(0, 0, 0);
    visualizer1->addPointCloud<pcl::PointXYZRGB>(main_cloud_ptr, "main_cloud_ptr");
    visualizer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "main_cloud_ptr");
    visualizer1->addCoordinateSystem(0.5);
    visualizer1->addCube(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0, 1.0, 1.0, "cube1");
    visualizer1->setRepresentationToWireframeForAllActors();
    while (!visualizer1->wasStopped()) {
        visualizer1->spinOnce();
    }

Now, using pcl::CropBox filter, I want to accomplish two different tasks and get two different types of slices from my cube-shaped point cloud:

1. take out a thin slice of the point cloud which is

   • parallel to XY plane
   • perpendicular to z-axis
   • at a certain distance on z-axis from the origin e.g. 0.3-0.5

2. take out a thin slice of the point cloud

   • which is tilted 45° based on the x-axis of the box itself rather than 45° rotation wrt global x-axis
   • whose centroid is at 0.4 unit distance on z-axis

For task 1, I did something like this and it's perfectly OK:

    pcl::CropBox<pcl::PointXYZRGB> box_filter1;

    box_filter1.setMin(Eigen::Vector4f(-1.0, -1.0, 0.3, 1.0));
    box_filter1.setMax(Eigen::Vector4f(1.0, 1.0, 0.5, 1.0));

    box_filter1.setInputCloud(main_cloud_ptr);
    box_filter1.filter(*cropped_cloud_ptr1);

    pcl::visualization::PCLVisualizer::Ptr visualizer2(new pcl::visualization::PCLVisualizer("visualizer2 -- cropped_cloud_ptr1"));
    visualizer2->setBackgroundColor(0, 0, 0);
    visualizer2->addPointCloud<pcl::PointXYZRGB>(cropped_cloud_ptr1, "cropped_cloud_ptr1");
    visualizer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cropped_cloud_ptr1");
    visualizer2->addCoordinateSystem(0.5);

    visualizer2->addCube(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0, 1.0, 1.0, "cube2");

    visualizer2->setRepresentationToWireframeForAllActors();

    while (!visualizer2->wasStopped()) {
        visualizer2->spinOnce();
    }

For task 2, I almost followed same procedure at task 1 except for setting rotation of pcl::CropBox using setRotation method and did something like this:

    pcl::CropBox<pcl::PointXYZRGB> box_filter2;

    box_filter2.setMin(Eigen::Vector4f(-1.0, -1.0, 0.3, 1.0));
    box_filter2.setMax(Eigen::Vector4f(1.0, 1.0, 0.5, 1.0));

    box_filter2.setRotation(Eigen::Vector3f(M_PI / 4, 0, 0));

    box_filter2.setInputCloud(main_cloud_ptr);
    box_filter2.filter(*cropped_cloud_ptr2);

    pcl::visualization::PCLVisualizer::Ptr visualizer3(new pcl::visualization::PCLVisualizer("visualizer2 -- cropped_cloud_ptr2"));
    visualizer3->setBackgroundColor(0, 0, 0);
    visualizer3->addPointCloud<pcl::PointXYZRGB>(cropped_cloud_ptr2, "cropped_cloud_ptr2");
    visualizer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cropped_cloud_ptr2");
    visualizer3->addCoordinateSystem(0.5);

    visualizer3->addCube(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0, 1.0, 1.0, "cube3");

    visualizer3->setRepresentationToWireframeForAllActors();

    while (!visualizer3->wasStopped()) {
        visualizer3->spinOnce();
    }

    return 0;
}

However, in that case, the box gets rotated by global X-axis.

This not what I want. I want to rotate/tilt the box wrt its own x-axis only (i.e. imagine a local coordinate system of the box, origin located at the centroid of box, and all the local x, y, & z-axis are parallel to global x, y, & z-axis.)

I know there is a setTransform method in pcl::CropBox which takes an object of Eigen::Affine3f as input argument. But, how can I use it in my case to rotate/title the cropBox wrt its own x-axis only?

If there are any other ways to solve this problem then also please share your thoughts/solution.

Rodmur

Today, I came across pcl::BoxClipper3D< PointT > class and for it, it's mentioned that "Implementation of a box clipper in 3D. Actually it allows affine transformations, thus any parallelepiped in general pose. The affine transformation is used to transform the point before clipping it using the unit cube centered at origin and with an extend of -1 to +1 in each dimension." But, how to apply affine transformation using Eigen? pointclouds.org/documentation/classpcl_1_1_box_clipper3_d.html – Rodmur

Answer 1

I had to solve this problem during my ROS project so might as well share my approach.

In order to achieve your desired result, you have to define a crop-box / Min / Max which is symmetric over the global 0,0,0 and then rotate and translate it where your want it to be.

In other words your box-filters coordinate system is 0,0,0 by default and does not change by .setMin() /.setMax()

You can change your filters coordinate system by .setTranslation()

`

box_filter2.setMin(Eigen::Vector4f(-0.5, -0.5, -0.1, 1.0));
box_filter2.setMax(Eigen::Vector4f(0.5, 0.5, 0.1, 1.0));

box_filter2.setTranslation(Eigen::Vector3f(0, 0, 0.4));

float angle = 45 * (M_PI / 180);
box_filter2.setRotation(Eigen::Vector3f ((float)(angle * M_PI / 180),0.0f, 0.0f));

Translation only

With Rotation

Note:

You can also use setTransform which combines translation and rotation into one big affine transformation. Putting in the same values for both approaches will therefore not yield the same result.

// This will produce a different result then the above snippet. 
//transform_matrix in x
transform_matrix(1, 1) = cos(angle);
transform_matrix(1, 2) = sin(angle);
transform_matrix(2, 1) = -sin(angle);
transform_matrix(2, 2) = cos(angle);

//translation by z
transform_matrix(2, 3) = 0.4;

Eigen::Affine3f transform;
transform = transform_matrix;
box_filter1.setTransform(transform);

I find this approach more error prone and harder to use. Affine Transformation

Some Helpful links:

https://en.wikipedia.org/wiki/Transformation_matrix

https://www.andre-gaschler.com/rotationconverter/

How to convert Eigen::Matrix4f to Eigen::Affine3f

I hope this helped.

Havildar

Answer 2

I came to the solution as Joachim Dunkel above and it worked for me. Although, I was also using cropbox to solve a Ros problem.

I feel that the pcl docmentation should mention the purpose of the set translation function more clearly

https://pointclouds.org/documentation/classpcl_1_1_crop_box_3_01pcl_1_1_p_c_l_point_cloud2_01_4.html#a83666462180c91cd175b4118f71200d7

"In translation , the (tx,ty,tz) values that the box should be translated by" it also means the translation of the coordinate axis of the cropbox

Cioban