随机抽样
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中的abc
a = 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;
}
