随机抽样

std::vector<int> random(int n, int N){std::vector<int> rets;for(int i=0; i<N; i++){while(true){int v = rand() % n;if(std::find(rets.begin(), rets.end(), v) == rets.end()){rets.push_back(v);break;}}}return rets;
}
bool Plane(std::vector<cv::Point3f>& cloud, std::vector<cv::Point3f>& cloudPlane){int nPlane = xxx;float thred = xxx;float P = xxx;int Iter = xxx;int S = Iter;int N = xxx;int n = cloud.size();if(n < nPlane){std::cout<<"not enough points"<<std::endl;return false;}int iter = 0;std::vector<int> inliersBest;while(true){iter++;std::vector<int> index = random(n, N);cv::Mat A = cv::Mat(3, 3, CV_32F);cv::Mat b = cv::Mat(3, 1, CV_32F);A.at<float>(0, 0) = cloud[index[0]].x;A.at<float>(0, 1) = cloud[index[0]].y;A.at<float>(0, 2) = cloud[index[0]].z;A.at<float>(1, 0) = cloud[index[1]].x;A.at<float>(1, 1) = cloud[index[1]].y;A.at<float>(1, 2) = cloud[index[1]].z;A.at<float>(2, 0) = cloud[index[2]].x;A.at<float>(2, 1) = cloud[index[2]].y;A.at<float>(2, 2) = cloud[index[2]].z;b.at<float>(0, 0) = -1;b.at<float>(1, 0) = -1;b.at<float>(2, 0) = -1;b = A.t() * b;A = A.t() * A;if(cv::determinant(A) == 0) continue;cv::Mat normal = A.inv() * b;if(cv::norm(normal) == 0) continue;std::vector<int> inliers;for(int i=0; i<n; i++){float distance = std::abs(normal.at<float>(0, 0)*cloud[i].x + normal.at<float>(1, 0)*cloud[i].y + normal.at<float>(2, 0)*cloud[i].z + 1) / cv::norm(normal);if(distance < thred) inliers.push_back(i);}if(inliers.size() > inliersBest.size()){inliersBest = inliers;float p = float(inliers.size()) / n;S = std::log(1-P)/std::log(1 - std::pow(p, N));}if(iter > Iter || iter > S) break;}cv::Mat A = cv::Mat(inliersBest.size(), 3, CV_32F);cv::Mat b = cv::Mat(inliersBest.size(), 1, CV_32F);for(int i=0; i<inliersBest.size(); i++){A.at<float>(i, 0) = cloud[inliersBest[i]].x;A.at<float>(i, 1) = cloud[inliersBest[i]].y;A.at<float>(i, 2) = cloud[inliersBest[i]].z;b.at<float>(i, 0) = -1;}b = A.t() * b;A = A.t() * A;if(cv::determinant(A) == 0){std::cout<<"zero determinant"<<std::endl;return false;}cv::Mat normal = A.inv() * b;if(cv::norm(normal) == 0){std::cout<<"zero normal"<<std::endl;return false;}for(int i=0; i<cloud.size(); i++){float distance = std::abs(normal.at<float>(0, 0)*cloud[i].x + normal.at<float>(1, 0)*cloud[i].y + normal.at<float>(2, 0)*cloud[i].z + 1) / cv::norm(normal);if(distance < thred) cloudPlane.push_back(cloud[i]);}if(cloudPlane.size() < nPlane){std::cout<<"not enough plane points"<<std::endl;return false;}return true;
}

最小二乘法：最小二乘法原理

bool Plane(vector<cv::Point3f> cloud, double& a, double& b, double& c, double& angle)
{double A11 = 0, A12 = 0, A13 = 0;double A21 = 0, A22 = 0, A23 = 0;double A31 = 0, A32 = 0, A33 = 0;double B1 = 0, B2 = 0, B3 = 0;for (int i = 0; i < cloud.size(); i++){A11 += cloud.at(i).x*cloud.at(i).x;A12 += cloud.at(i).x*cloud.at(i).y;A13 += cloud.at(i).x;A21 += cloud.at(i).x*cloud.at(i).y;A22 += cloud.at(i).y*cloud.at(i).y;A23 += cloud.at(i).y;A31 += cloud.at(i).x;A32 += cloud.at(i).y;B1 += cloud.at(i).x*cloud.at(i).z;B2 += cloud.at(i).y*cloud.at(i).z;B3 += cloud.at(i).z;}A33 = cloud.size();cv::Mat A(3, 3, CV_32FC1);A.at<float>(0, 0) = A11; A.at<float>(0, 1) = A12; A.at<float>(0, 2) = A13;A.at<float>(1, 0) = A21; A.at<float>(1, 1) = A22; A.at<float>(1, 2) = A23;A.at<float>(2, 0) = A31; A.at<float>(2, 1) = A32; A.at<float>(2, 2) = A33;cv::Mat B(3, 1, CV_32FC1);B.at<float>(0, 0) = B1; B.at<float>(1, 0) = B2; B.at<float>(2, 0) = B3;cv::Mat X;X = A.inv()*B;                                                              //X为3*1解向量，分别对应平面方程z=ax+by+c中的abca = X.at<float>(0, 0);b = X.at<float>(1, 0);c = X.at<float>(2, 0);// 计算平面的法向量Eigen::Vector3d normal_vector(a, b, c);normal_vector.normalize();// 计算相对于水平面的角度angle = std::acos(normal_vector.dot(Eigen::Vector3d(0, 0, 1))) * 180.0 / M_PI;  // Eigen::Vector3d(0, 0, 1)水平面法向量return true;
}