PCL&&VTK点击屏幕选最近点

在网上搜索的大多点选方法都是在回调函数中使用event.getPoint()获取点，但该方法实现的点选经常存在选不到点或选到的点不是鼠标最近点等问题，因此写了其他方法，特记录如下。

正文

方法描述：根据屏幕坐标得到鼠标光标在世界坐标系的射线，然后寻找点云中距离该射线最近的点。

方法优点：鼠标即使没有确切在点上，也可以选到点云中离鼠标最近的点，交互体验更好。

1. 通过屏幕坐标得到射线

首先注册回调函数，在移动鼠标时得到屏幕坐标

//注册鼠标移动回调函数
m_viewer->registerMouseCallback(mouseMove_callback, (void*)&callbackPoint);

// 得到屏幕坐标
void mouseMove_callback(const pcl::visualization::MouseEvent& e, void* args) {
	struct callback_args* data = (struct callback_args*)args; 
	
	// 屏幕坐标
	data->screen_x = e.getX();
	data->screen_y = e.getY();
} 

其中callback_args是用于回调函数传参的结构体，具体如下：

struct callback_args{
	pcl::PointXYZRGB clicked_points_3d;    // 得到的点
	pcl::visualization::PCLVisualizer::Ptr viewerPtr;    // PCLVisualizer
	int screen_x, screen_y;                // 屏幕坐标
	pcl::PointXYZRGB world_points_near;    // 射线起点
	pcl::PointXYZRGB world_points_far;     // 射线终点
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr *cloud;    // 点云

    // 得到射线的两点
	void getWorldPoint();
    // 点云中距射线最近的点
	void getNearestPointOfRay();
}

通过函数getWorldPoint()可以得到射线的两个点，具体定义如下：

// 得到射线的两个点
void callback_args::getWorldPoint() {
	vtkRenderer* renderer{ viewerPtr->getRenderWindow()->GetRenderers()->GetFirstRenderer() };
	// 射线点1 
	renderer->SetDisplayPoint(screen_x, screen_y, 0);
	renderer->DisplayToWorld();
	world_points_near.x = (renderer->GetWorldPoint())[0];
	world_points_near.y = (renderer->GetWorldPoint())[1];
	world_points_near.z = (renderer->GetWorldPoint())[2];
	// 射线点2
	renderer->SetDisplayPoint(screen_x, screen_y, 1);
	renderer->DisplayToWorld();
	world_points_far.x = (renderer->GetWorldPoint())[0];
	world_points_far.y = (renderer->GetWorldPoint())[1];
	world_points_far.z = (renderer->GetWorldPoint())[2];
}

2. 寻找拒射线最近点

同样注册回调函数，在按shift+鼠标左键时触发

// 注册屏幕选点回调函数
m_viewer->registerPointPickingCallback(choosePoint_callback, (void*)&callbackPoint);

// shift+鼠标左键 选点回调函数
void choosePoint_callback(const pcl::visualization::PointPickingEvent& event, void* args){
	// 创建结构体对象
	struct callback_args* data = (struct callback_args*)args;

	// 从屏幕坐标得到射线的两个点
	data->getWorldPoint(); 
	// 画射线
	data->viewerPtr->removeShape("clicked_line");
	data->viewerPtr->addLine(data->world_points_near, data->world_points_far, "clicked_line");

	// 找点到射线的最近点
	data->getNearestPointOfRay();

	// 添加到渲染点云中  
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud->push_back(data->clicked_points_3d);

	// 将选中点用红色标记
	pcl::visualization::PointCloudColorHandlerCustom<PointT> red(cloud, 255, 0, 0);
	data->viewerPtr->removePointCloud("clicked_points");
	data->viewerPtr->addPointCloud(cloud, red, "clicked_points");
	data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points"); 
}

其中getNearestPointOfRay()函数如下：

// 点云中距射线最近的点
void callback_args::getNearestPointOfRay() {
	// 定义直线上的两个点
	Eigen::Vector3f point1(world_points_near.x, world_points_near.y, world_points_near.z);
	Eigen::Vector3f point2(world_points_far.x, world_points_far.y, world_points_far.z);

	// 初始化距离和最近点索引
	float minDistance = std::numeric_limits<float>::max();
	size_t closestPointIndex = 0;

	// 遍历点云中的每个点 
	for (size_t i = 0; i < (*cloud) ->size(); ++i) {
		Eigen::Vector3f point((*cloud)->points[i].x, (*cloud)->points[i].y, (*cloud)->points[i].z);

		// 计算点到直线的距离
		float distance = abs((point - point1).cross(point2 - point1).squaredNorm()); 

		// 更新最近点的距离和索引
		if (distance < minDistance) {
			minDistance = distance;
			closestPointIndex = i;
		}
	}

	// 添加最近点
	clicked_points_3d = (*cloud)->points[closestPointIndex];
}

注意， callback_args结构体中的cloud点云需要事先指向需要寻找的点云。如果要在多个点云中寻找，可以将其改成vector<PointCloudT>，再做修改。

3. 效果 

效果如下，黑色点为点云，蓝色线为屏幕坐标转换后的射线，红色点为点云中距射线的最近点

即使射线不在点云上，也可以找到最近点