Filtering a Point Cloud to Match the Field of View of the Camera

In a previous post, I described why and how I was collecting a Point Clouds dataset. My setup is depicted in the image above, where a 360°, 32-beam LiDAR is placed above a stereo camera. One of the steps mentioned in the article was to crop (or filter) the Point Cloud to only show points approximately within the Field of View (FoV) of the camera of my robot. The reason is that the camera frames provide the necessary context to understand what the LiDAR is seeing, making things like annotating the clouds easier.

In this post, I will describe how to process a full Point Cloud to only retain the points within the camera FoV. There are multiple libraries that could be used to achieve this but I used a popular and powerful C++ library called Point Cloud Library (PCL). I am an Ubuntu user so as usual, my instructions are for Ubuntu and may work without many modifications for other Linux distros.

Here are the steps:

Step 1: Install the PCL library

sudo apt install libpcl-dev

Step 2: Clone my PCL experiments repo

git clone https://github.com/carlos-argueta/pcl_experiments.git

The two folders that interest us are filter_camera_fov which contains the C++ code and pcds which contains a small dataset composed of full 360°LiDAR scans and JPG images captured by the front-facing camera of my robot. The idea is to filter the scans to only retain content that can be seen by the camera.

Step 3: Go to the filter_camera_fov folder and create the build folder

cd pcl_experiments/filter_camera_fov
mkdir build
cd build

Step 4: Build the filter

cmake ..
make

Step 5: Run the filter

Running the filter will override the PCD files, if you intend to do something with the original files, please make a copy of the pcds folder before running the code below.

./filter ../../pcds

That’s it, the program will load all the files in the pcds folder (or any other folder with PCD files that you specify), and then filter them one by one, overwriting the original files with the filtered Point Clouds. The following video shows the entire process.

If you want to quickly visualize the PCDs (before or after filtering), you can use the pcl_viewer utility. To install and use it you can follow these simple steps:

Step 1: Install pcl_tools

sudo apt-get install pcl-tools

Step 2: Visualize PCD file with pcl_viewer

Assuming you are at the root of the repo’s directory, run the following code.

Change the name of the file to view another one of the provided Point Clouds.

pcl_viewer pcds/lidar_2022_05_05_16_04_21.2595.pcd

For details about how the code works please see the filter.cpp file inside the filter_camera_fov folder. In this article, I will just briefly describe how the filter function of the program works.

void filter(std::vector<PCD, Eigen::aligned_allocator<PCD> > &data){
  cout<<endl<<endl<<"Applying Filter"<<endl;

  PointCloud::Ptr cloud_filtered (new PointCloud);

  // Create the filter
  pcl::FrustumCulling<PointT> fc;
  // The following parameters were defined by trial and error. 
  // You can modify them to better match your expected results
  fc.setVerticalFOV (100);
  fc.setHorizontalFOV (100);
  fc.setNearPlaneDistance (0.0);
  fc.setFarPlaneDistance (150);
   
  // Define the camera pose as a rotation and translation with respect to the LiDAR pose.
  Eigen::Matrix4f camera_pose = Eigen::Matrix4f::Identity();
  Eigen::Matrix3f rotation = Eigen::Quaternionf(0.9969173, 0, 0, 0.0784591  ).toRotationMatrix();
  Eigen::RowVector3f translation(0, 0, 0);
  // This is the most important part, it tells you in which direction to look in the Point Cloud
  camera_pose.block(0,0,3,3) = rotation; 
  camera_pose.block(3,0,1,3) = translation;
  cout<<"Camera Pose "<<endl<<camera_pose<<endl<<endl;
  fc.setCameraPose (camera_pose);
   
  // Go over each Point Cloud and filter it
  for (auto & d : data){
    // Run the filter on the cloud
    PointCloud::Ptr cloud_filtered (new PointCloud);
    fc.setInputCloud (d.cloud);
    fc.filter(*cloud_filtered);
    // Update the cloud 
    d.cloud = cloud_filtered;
    // Replace the PCD file with the filtered data
    pcl::io::savePCDFileASCII (d.f_name, *d.cloud);
    
  }

}

The filter function receives a vector (like a list) of loaded PCD structures. The PCD structure is a C struct that has a file name f_name and a Point Cloud cloud field. In lines 7 to 13 above, a FrustumCulling filter is created. The provided parameters of the filter are good defaults that I found by trial and error, but feel free to experiment with different values. The parameters define a frustum and only points falling within it will be kept. This is great as it is related to something called the viewing frustum, which is in turn related to the fields of view of cameras etc. Feel free to follow the provided links to read more about it if you want.

Next, in lines 15–23 we define a transformation matrix representing the camera pose, which is basically a rotation and translation of the frustum. Think of it as moving the frustum to point in the direction in which we want to see, and hence keep points within that view. The provided values for the rotation and translation matrices roughly correspond to the transformation necessary to align my camera’s coordinate system with my LiDAR’s coordinate system. If you capture your own cloud-image pairs, you may need to adjust this to match your hardware setup.

Finally, the loop in lines 25–36, picks one Point Cloud at a time from the input vector, applies the filter, replaces the old cloud with the filtered cloud in the vector, and overrides the older PCD file with the filtered content. That is essentially the heart of the program. Other functions in the program deal with loading all PCD files into the vector, visualizing a cloud before and after filtering, etc. Please consult the source code for more details.