平面度计算 C++实现

直接贴代码

cmake

cmake_minimum_required(VERSION 2.8.3)
project(test)

#-------------------------------------- 编译器设置 --------------------------------------
#set(CMAKE_BUILD_TYPE Release)
set(CMAKE_BUILD_TYPE Debug)

if(CMAKE_BUILD_TYPE STREQUAL "Release")
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++11 -fPIC")
else()
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -std=c++11 -fPIC")
endif()
message("*** ${PROJECT_NAME}: Build type:" ${CMAKE_BUILD_TYPE} ${CMAKE_CXX_FLAGS} "***")

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmakes)
#-------------------------------------- -添加项目- --------------------------------------

add_executable(
${PROJECT_NAME}
test.cpp )
target_link_libraries(
${PROJECT_NAME}
${catkin_LIBRARIES})

test.cpp文件

// system
#include <vector>
#include <iostream>
// eigen3
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>
using namespace Eigen;

typedef struct Pt
{
  double x;
  double y;
  double z;
}Point;
/* 最小二乘法拟合平面：Ax + By + Cz + D = 0 */
/* xjParameters：返回参数A B C D */
/* xjData：存放输入点 */
bool xjFitPlaneByLeastSquares(std::vector<double>& xjParameters, const std::vector<Point>& xjData)
{
  xjParameters.clear();
  int count = xjData.size();
  if (count < 3)
    return false;

  double meanX = 0, meanY = 0, meanZ = 0;
  double meanXX = 0, meanYY = 0, meanZZ = 0;
  double meanXY = 0, meanXZ = 0, meanYZ = 0;
  for (int i = 0; i < count; i++)
  {
    meanX += xjData[i].x;
    meanY += xjData[i].y;
    meanZ += xjData[i].z;

    meanXX += xjData[i].x * xjData[i].x;
    meanYY += xjData[i].y * xjData[i].y;
    meanZZ += xjData[i].z * xjData[i].z;

    meanXY += xjData[i].x * xjData[i].y;
    meanXZ += xjData[i].x * xjData[i].z;
    meanYZ += xjData[i].y * xjData[i].z;
  }
  meanX  /= count;
  meanY  /= count;
  meanZ  /= count;
  meanXX /= count;
  meanYY /= count;
  meanZZ /= count;
  meanXY /= count;
  meanXZ /= count;
  meanYZ /= count;

  /* eigenvector */
  Matrix3d eMat;
  eMat(0, 0) = meanXX - meanX * meanX; eMat(0, 1) = meanXY - meanX * meanY; eMat(0, 2) = meanXZ - meanX * meanZ;
  eMat(1, 0) = meanXY - meanX * meanY; eMat(1, 1) = meanYY - meanY * meanY; eMat(1, 2) = meanYZ - meanY * meanZ;
  eMat(2, 0) = meanXZ - meanX * meanZ; eMat(2, 1) = meanYZ - meanY * meanZ; eMat(2, 2) = meanZZ - meanZ * meanZ;
  Eigen::EigenSolver<Eigen::Matrix3d> xjMat(eMat);
  Matrix3d                            eValue  = xjMat.pseudoEigenvalueMatrix();
  Matrix3d                            eVector = xjMat.pseudoEigenvectors();

  /* the eigenvector corresponding to the minimum eigenvalue */
  double v1        = eValue(0, 0);
  double v2        = eValue(1, 1);
  double v3        = eValue(2, 2);
  int    minNumber = 0;
  if ((abs(v2) <= abs(v1)) && (abs(v2) <= abs(v3)))
  {
    minNumber = 1;
  }
  if ((abs(v3) <= abs(v1)) && (abs(v3) <= abs(v2)))
  {
    minNumber = 2;
  }
  double A = eVector(0, minNumber);
  double B = eVector(1, minNumber);
  double C = eVector(2, minNumber);
  double D = -(A * meanX + B * meanY + C * meanZ);

  /* result */
  if (C < 0)
  {
    A *= -1.0;
    B *= -1.0;
    C *= -1.0;
    D *= -1.0;
  }
  xjParameters.push_back(A);
  xjParameters.push_back(B);
  xjParameters.push_back(C);
  xjParameters.push_back(D);
  return true;
}
double dist(Point pt, std::vector<double> xjParameters) // Distance between point and plane
{
  double dt = 0.0;
  double mA, mB, mC, mD, mX, mY, mZ;

  mA = xjParameters[0];
  mB = xjParameters[1];
  mC = xjParameters[2];
  mD = xjParameters[3];

  mX = pt.x;
  mY = pt.y;
  mZ = pt.z;

  dt = fabs(mA*mX+mB*mY+mC*mZ+mD)/sqrt(mA*mA+mB*mB+mC*mC);
  return dt;
}

int main(int argc, char* * argv)
{
  std::vector<double> xjParameters;
  std::vector<Point>  xjData;

  double x[] = {58.8249, 149.3559, 259.9059, 273.2909, 272.2909, 272.2909,
                255.5409, 164.0009, 66.0309, 47.0559, 47.0559, 47.0559};
  double y[] = {-52.3037, -52.3037, -52.3037, -82.1139, -127.5069, -168.7469,
                -185.3669, -185.3669, -185.3669, -163.4689, -118.4089, -68.9889};
  double z[] = {0, -0.165, -0.202, -0.231, -0.163, -0.158,
                -0.098, -0.157, 0.075, 0.036, -0.012, -0.04};
  Point  tmp;
  for(int i = 0; i < 12; i++)
  {
    tmp.x = x[i];
    tmp.y = y[i];
    tmp.z = z[i];
    xjData.push_back(tmp);
  }
  if(xjFitPlaneByLeastSquares(xjParameters, xjData))
  {
    std::cout<<"A ="<<xjParameters[0]<<" B ="<<xjParameters[1]<<" C ="<<xjParameters[2]<<" D ="<<xjParameters[3]<<std::endl;
    double sum = 0.0;
    for(int j = 0; j<xjData.size(); j++)
    {
      double dis = dist(xjData[j], xjParameters);
      sum += dis;
      std::cout<<"id ="<<j+1 <<" distance ="<<dis<<std::endl;
    }
    sum = sum/xjData.size();
    std::cout<<"Average distance ="<<sum<<std::endl;
  }
  else
  {
    std::cout<<"not find plane"<<std::endl;
  }
  return 0;
}