[IterativeClosestPoint] Viewpoint information not handled during registration

[carlsberg03]

Describe the bug

Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output) return wrong registration result when the input 2 point cloud have different viewpoint.

Context

Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output) does not take into account the viewpoint differences between the two input point clouds. The returned point cloud's data field is usually consistent with the reference cloud. However, differing viewpoints can lead to significant positional discrepancies when visualized using pcl::visualizer, since the visualizer considers the transformation from the camera coordinate system to the world coordinate system based on the viewpoint, whereas icp.align() does not.

Expected behavior

(though not perfectly matched,it seems like a right behaviour)

Current Behavior

To Reproduce

I provide a simple example to reproduce my question

#include <iostream>
#include <fstream>
#include <thread>

#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>



int main() {
	std::string sFileName_mov = R"(E:\bunny.pcd)";
	std::string sFileName_ref = R"(E:\bunnyT.pcd)";

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_mov(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref(new pcl::PointCloud<pcl::PointXYZ>);

	if (pcl::io::loadPCDFile(sFileName_mov, *cloud_mov) < 0 ||
		pcl::io::loadPCDFile(sFileName_ref, *cloud_ref) < 0) {
		std::cerr << "Failed to load point clouds." << std::endl;
		return -1;
	}

	// if set viewpoint to zero, program run as expected
	//cloud_mov->sensor_origin_.setZero();
	//cloud_mov->sensor_orientation_ = Eigen::Quaternionf::Identity();
	//cloud_ref->sensor_origin_.setZero();
	//cloud_ref->sensor_orientation_ = Eigen::Quaternionf::Identity();

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_mov);
	icp.setInputTarget(cloud_ref);
	icp.setMaximumIterations(100);
	icp.setMaxCorrespondenceDistance(0.05);
	icp.setTransformationEpsilon(1e-8);
	icp.setEuclideanFitnessEpsilon(1e-6);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp_result(new pcl::PointCloud<pcl::PointXYZ>);
	icp.align(*cloud_icp_result);

	if (icp.hasConverged()) {
		std::cout << "ICP converged. Score: " << icp.getFitnessScore() << std::endl;
	}
	else {
		std::cerr << "ICP did not converge." << std::endl;
	}

	// visualization
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("ICP Result Viewer"));
	viewer->setBackgroundColor(0, 0, 0);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ref_color(cloud_ref, 0, 0, 255);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_ref, ref_color, "reference cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> mov_color(cloud_icp_result, 255, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_icp_result, mov_color, "aligned cloud");

	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "reference cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned cloud");

	viewer->addCoordinateSystem(0.1);
	while (!viewer->wasStopped()) {
		viewer->spinOnce(100);
	}
	return 0;
}

the main part to reproduce the result is the pcd file "bunny.pcd" and "bunnyT.pcd".I cannot directly upload this to github.So I provide google drive link here:https://drive.google.com/file/d/1JxzXIKyi-IC3wjB3d7cJMqlBQNV_3gle/view?usp=drive_link, https://drive.google.com/file/d/1IRVhzo26caWpQKDPQZ2jaR7Y5IrnyKpU/view?usp=drive_link.The key point is that the two PCD files have different "VIEWPOINT" fields. Since they are in ASCII format, you can easily inspect them using any text editor.

Your Environment (please complete the following information):

• OS: Windows 10
• Compiler: MSVC 1940
• PCL Version 1.15.1

Possible Solution

1.When performing point cloud registration, if the viewpoints of the two input clouds are different, a warning should be issued to the user.
2. Transform the data to the world coordinate system for both point clouds before performing the registration.
or align one point cloud's viewpoint with the other by transforming the data of one cloud to match the viewpoint of the other.

[mvieth]

Hi, thank you for reporting this, and thank you for the pull request. The viewpoint information (that is, sensor_origin_ and sensor_orientation_) says from where the points have been recorded (see for example https://pcl.readthedocs.io/projects/tutorials/en/master/basic_structures.html#basic-structures and https://pointclouds.org/documentation/classpcl_1_1_point_cloud.html#a5de17e88bdf15e1c4fd1bcc6b85b1941 ). Only few classes/algorithms in PCL use this information, I can currently only think of NormalEstimation, where this is used to resolve an ambiguity in the normals (by orientating them towards the viewpoint), and VoxelGridOcclusionEstimation, where the viewpoint is used to estimate which parts of the space may not be visible for the sensor.
I think there might actually be a bug in PCLVisualizer: Let's say we have two point clouds where the points are exactly the same in the two clouds, only sensor_origin_ and sensor_orientation_ differ (this is basically the case in your example, after successfully aligning the two clouds). Then, I think, PCLVisualizer should show the two clouds perfectly aligned/overlapping (the behaviour that you also expect). I will take a closer look at PCLVisualizer and what it does with sensor_origin_ and sensor_orientation_.
Additionally, in pcl::transformPointCloud, sensor_origin_ and sensor_orientation_ seem to be handled incorrectly (see #4164 ). pcl::transformPointCloud is also used inside IterativeClosestPoint. Perhaps you are interested in creating a pull request to fix that issue? So far, no one else has created one.
Regarding your pull request ( #6280 ): I am not sure if those changes make sense. For example, let's say someone has two scans/recordings of the same scene (or object), but taken from different angles. Then, after aligning, it would not make sense that output and target_ cloud have the same viewpoint information -- they should have different values because the position/orientation of the sensor relative to the object was different. But of course output should also have a different viewpoint value than input_ .
So, to summarize, I will check whether PCLVisualizer handles sensor_origin_ and sensor_orientation_ correctly, but there is at least a bug in pcl::transformPointCloud.

[carlsberg03]

Hi, thank you for reporting this, and thank you for the pull request. The viewpoint information (that is, sensor_origin_ and sensor_orientation_) says from where the points have been recorded (see for example https://pcl.readthedocs.io/projects/tutorials/en/master/basic_structures.html#basic-structures and https://pointclouds.org/documentation/classpcl_1_1_point_cloud.html#a5de17e88bdf15e1c4fd1bcc6b85b1941 ). Only few classes/algorithms in PCL use this information, I can currently only think of NormalEstimation, where this is used to resolve an ambiguity in the normals (by orientating them towards the viewpoint), and VoxelGridOcclusionEstimation, where the viewpoint is used to estimate which parts of the space may not be visible for the sensor. I think there might actually be a bug in PCLVisualizer: Let's say we have two point clouds where the points are exactly the same in the two clouds, only sensor_origin_ and sensor_orientation_ differ (this is basically the case in your example, after successfully aligning the two clouds). Then, I think, PCLVisualizer should show the two clouds perfectly aligned/overlapping (the behaviour that you also expect). I will take a closer look at PCLVisualizer and what it does with sensor_origin_ and sensor_orientation_. Additionally, in pcl::transformPointCloud, sensor_origin_ and sensor_orientation_ seem to be handled incorrectly (see #4164 ). pcl::transformPointCloud is also used inside IterativeClosestPoint. Perhaps you are interested in creating a pull request to fix that issue? So far, no one else has created one. Regarding your pull request ( #6280 ): I am not sure if those changes make sense. For example, let's say someone has two scans/recordings of the same scene (or object), but taken from different angles. Then, after aligning, it would not make sense that output and target_ cloud have the same viewpoint information -- they should have different values because the position/orientation of the sensor relative to the object was different. But of course output should also have a different viewpoint value than input_ . So, to summarize, I will check whether PCLVisualizer handles sensor_origin_ and sensor_orientation_ correctly, but there is at least a bug in pcl::transformPointCloud.

Thank you for your detailed and patient reply. After reconsidering, I agree that PR #6280 was indeed ill-considered on my part. I will continue thinking and exploring further.

Besides that, I still have some questions. Regarding the potential bug you mentioned in pcl::visualizer, do you think the data in a PCD file (i.e., the point cloud data) is represented in world coordinates? I'm not quite sure whether the data represents coordinates in the world coordinate system or in the camera coordinate system (i.e., relative to the viewpoint), and I couldn't find any clear standard on this.

The reason I assumed that data is in camera coordinates is because another open-source software, CloudCompare, displays point clouds in a way consistent with pcl::visualizer—that is, it seems to treat the data as relative to the viewpoint, then transforms and displays it accordingly.This is also why I thought the issue was a bug in ICP rather than in the visualizer.

I would really appreciate it if you could help clarify this doubt. Thank you!