PCL计算点到直线距离

最近做东西的时候需要几算pcl点云中点到另两点连线的距离，pcl中没有现成的函数，所以自己写了一个
这篇博客代码的内容包含在linux下运行的所有代码：一个头文件一个源文件和一个test,相应的CMakeLists的内容
应用示例main.cpp
示例里的直线为点（0，0，0）和（1，1，1）确定的，然后计算了直线上的点（2，2，2），直线外的两点（2，1，1），（9，7，16）到直线的距离，与手动计算的值进行比较来验证算法的正确性

#include"distance.h"

int main()
{
    PointCloud::Ptr cloud(new PointCloud);

    // 创建点云数据
    cloud->width =5;
    cloud->height =1;
    cloud->points.resize (cloud->width * cloud->height);
    for (size_t i=0; i< 3; ++i){
        cloud->points[i].x =i;
        cloud->points[i].y =i;
        cloud->points[i].z =i;
    }
    cloud->points[3].x =2;
    cloud->points[3].y =1;
    cloud->points[3].z =1;
    cloud->points[4].x =9;
    cloud->points[4].y =7;
    cloud->points[4].z =16;

    cout<<"distance of point (2,2,2) = "<<distance(cloud->points[0],cloud->points[1],cloud->points[2])<<endl;
    cout<<"distance of point (2,1,1) = "<<distance(cloud->points[0],cloud->points[1],cloud->points[3])<<endl;
    cout<<"distance of point (9,7,16) = "<<distance(cloud->points[0],cloud->points[1],cloud->points[4])<<endl;
}

程序运行的结果如下

这三个点与手工计算的结果符合，可以初步验证算法的正确性，下面是需要的其他代码

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(main)
# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization filters )
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # use this in Ubuntu 16.04

# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

add_executable(main main.cpp distance.cpp)
target_link_libraries(main ${PCL_LIBRARIES})

**distance.h**

#ifndef DISTANCE_H
#define DISTANCE_H
//C++标准库
#include<fstream>
#include<vector>
#include<map>
using namespace std;

//PCL
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/common/transforms.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/filters/voxel_grid.h>

//类型定义
typedef pcl::PointXYZRGBA PointT;	
typedef pcl::PointCloud<PointT> PointCloud;

//定义三维空间向量
struct VECTOR3D
{
	float x,y,z;
};

//计算两个向量的叉乘的模
double product(VECTOR3D vector1,VECTOR3D vector2);

//计算点云中点到另两点组成的直线的距离
double distance(PointT fixed,PointT pose,PointT pointForDistance);
#endif

distance.cpp

#include"distance.h"
//计算两个向量的叉乘的模
double product(VECTOR3D vector1,VECTOR3D vector2)
{
	double x,y,z,value;
	x=vector1.y*vector2.z-vector1.z*vector2.y;
	y=vector1.z*vector2.x-vector1.x*vector2.z;
	z=vector1.x*vector2.y-vector1.y*vector2.x;
	value=sqrt(x*x+y*y+z*z);
	return value;
}

//计算点云中点到另两点组成的直线的距离
double distance(PointT fixed,PointT pose,PointT pointForDistance)
{
	double distance;
	VECTOR3D v1,v2,v3;
	v1.x=fixed.x-pose.x;
	v1.y=fixed.y-pose.y;
	v1.z=fixed.z-pose.z;
	v2.x=fixed.x-pointForDistance.x;
	v2.y=fixed.y-pointForDistance.y;
	v2.z=fixed.z-pointForDistance.z;
	distance=product(v1,v2)/sqrt(v1.x*v1.x+v1.y*v1.y+v1.z*v1.z);
	return distance;
}