使用RANSAC算法寻找3D空间中最多点通过的平面

//points：空间中的点，x,y,z
//C：平面方程标准式参数：Ax+By+Cz+D=0,C.at<float>(0,0)x+C.at<float>(1,0)y+C.at<float>(2,0)z+C.at<float>(3,0)=0

bool RansacPanel(std::vector<cv::Point3f>& points,cv::Mat& C)
{
if (points.size() == 0)
return false;
int distance_thres = 6;
int loop_count = 0;
int iner_count = 0;
int max_iner_count = 0;
float distance = 0;
float sqrt_ = 0;
int rand_k1 = 0, rand_k2 = 0, rand_k3 = 0;
cv::Point3f point1, point2, point3;//理论上空间中3点可确定一个平面，可考虑多点组成超定方程组求解。
cv::Mat A(3,3,CV_32FC1,cv::Scalar(1));
float max_dis = 0, min_dis = 1000000, average_dis = 0;
while (loop_count++ < 10000)
{
srand((unsigned int)time(0));    //以当前系统时间来初始化srand种子
rand_k1 = rand() % points.size();
point1 = points[rand_k1];
rand_k2 = rand() % points.size();
point2 = points[rand_k2];
rand_k3 = rand() % points.size();
point3 = points[rand_k3];
//判断三点是否共线，以三点组成两向量，计算向量夹角，以夹角来判定是否共线
cv::Point3f v1(point1.x - point2.x, point1.y - point2.y, point1.z - point2.z), v2(point1.x - point3.x, point1.y - point3.y, point1.z - point3.z);
float cos_theta = abs((v1.x * v2.x + v1.y * v2.y + v1.z * v2.z) / (sqrtf(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z) * sqrtf(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z)));
while (abs(cos_theta - 1) < 0.05)//夹角小于0.05弧度时认定为共线