【激光雷达点云障碍物检测】ransac3d.h、ransac3d.cpp

ransac3d.h

//
// Created by hyin on 2020/3/25.
//

#ifndef PLAYBACK_RANSAC3D_H
#define PLAYBACK_RANSAC3D_H

#include   //基于哈希表的关联容器
#include 

namespace lidar_obstacle_detection {

	// shorthand for point cloud pointer 点云指针的简写
	template
	using PtCdtr = typename pcl::PointCloud::Ptr;//定义一个具体类型的别名， using 与 typedef 一样的效果

	template
	class Ransac //Ransac功能实现类，，并定义了函数
	{    
	private:
		int maxIterations;
		float distanceTol;
		int num_points;
	public:
		//含参构造函数
		Ransac(int maxIter, float distTol, int nPts) : maxIterations(maxIter), distanceTol(distTol), num_points(nPts) {}
		//析构函数
		~Ransac();
		// 返回值 Ransac3d函数（参数）
		std::unordered_set Ransac3d(PtCdtr cloud);
	};
}
#endif 

ransac3d.cpp

//
// Created by hyin on 2020/3/25.
//

#include "ransac3d.h"
using namespace lidar_obstacle_detection;

template //类模板
Ransac::~Ransac() {}

template
std::unordered_set Ransac::Ransac3d(PtCdtr cloud) 
{
	std::unordered_set inliersResult; // unordered_set element has been unique
										   // For max iterations
	//int maxIterations = 40;
	while (maxIterations--) 
	{
		std::unordered_set inliers;
		while (inliers.size() < 3) 
		{
			inliers.insert(rand() % num_points);   //产生 0~num_points 中的一个随机数
		}
		// TO define plane, need 3 points
		float x1, y1, z1, x2, y2, z2, x3, y3, z3;
		auto itr = inliers.begin();  //auto 自动类型
		x1 = cloud->points[*itr].x;
		y1 = cloud->points[*itr].y;
		z1 = cloud->points[*itr].z;
		itr++;
		x2 = cloud->points[*itr].x;
		y2 = cloud->points[*itr].y;
		z2 = cloud->points[*itr].z;
		itr++;
		x3 = cloud->points[*itr].x;
		y3 = cloud->points[*itr].y;
		z3 = cloud->points[*itr].z;
		// Calulate plane coefficient 计算平面系数
		float a, b, c, d, sqrt_abc;
		a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
		b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
		c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
		d = -(a * x1 + b * y1 + c * z1);
		sqrt_abc = sqrt(a * a + b * b + c * c);
		// Check distance from point to plane
		for (int ind = 0; ind < num_points; ind++) 
		{
			if (inliers.count(ind) > 0) 
			{ // that means: if the inlier in already exist, we dont need do anymore
				continue;
			}
			PointT point = cloud->points[ind];
			float x = point.x;
			float y = point.y;
			float z = point.z;
			float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and plane
			//fabs 绝对值
			if (dist < distanceTol) 
			{
				inliers.insert(ind);
			}
			if (inliers.size() > inliersResult.size()) {
				inliersResult = inliers;

			}
		}
	}
	return inliersResult;
}