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.

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.

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