【PCL】平面拟合

• CMAKE配置

cmake_minimum_required(VERSION 3.16)
project(FridayLibrary)

set(CMAKE_CXX_STANDARD 14)


SET(BOOST_INCLUDE_DIR C:/Program\ Files/PCL\ 1.11.0/3rdParty/Boost/include)
SET(BOOST_LIBRARY_DIR C:/Program\ Files/PCL\ 1.11.0/3rdParty/Boost/lib)

set(PLC_ROOT        C:/Program\ Files/PCL\ 1.11.0)
set(PLC_INCLUDE_DIR C:/Program\ Files/PCL\ 1.11.0/include)
set(PLC_LIBRARY_DIR C:/Program\ Files/PCL\ 1.11.0/lib)

find_package(PCL REQUIRED)
if(PCL_FOUND)
    message ("PCL found")
else()
    message (FATAL_ERROR "Cannot find PCL")
endif(PCL_FOUND)
include_directories(${PLC_INCLUDE_DIR} ${BOOST_INCLUDE_DIR})
link_directories(${PLC_LIBRARY_DIR} ${BOOST_LIBRARY_DIR})


add_library(FridayLibrary SHARED library.cpp library.h)
target_link_libraries( FridayLibrary PRIVATE
        ${PCL_LIBRARIES}
        ${BOOST_LIBRARIES}
        )

• PCL + C++平面拟合

pcl::PointXYZ points[36];
points[0].x = 0.2542060f, points[0].y = 0.1991714f, points[0].z = 2.0531f;
points[1].x = 0.3039878f, points[1].y = 0.1993848f, points[1].z = 2.0553f;
points[2].x = 0.3500631f, points[2].y = 0.2034014f, points[2].z = 2.0575f;
points[3].x = 0.4000528f, points[3].y = 0.2074261f, points[3].z = 2.0597f;
points[4].x = 0.4463278f, points[4].y = 0.2076477f, points[4].z = 2.0619f;
points[5].x = 0.4954670f, points[5].y = 0.2112333f, points[5].z = 2.0597f;
points[6].x = 0.2547508f, points[6].y = 0.2604488f, points[6].z = 2.0575f;
points[7].x = 0.2995513f, points[7].y = 0.2634172f, points[7].z = 2.051f;
points[8].x = 0.3458804f, points[8].y = 0.2677685f, points[8].z = 2.0553f;
points[9].x = 0.3958130f, points[9].y = 0.2680552f, points[9].z = 2.0575f;
points[10].x = 0.4415629f, points[10].y = 0.2718583f, points[10].z = 2.0575f;
points[11].x = 0.4916504f, points[11].y = 0.2759562f, points[11].z = 2.0597f;
points[12].x = 0.2512066f, points[12].y = 0.3254502f, points[12].z = 2.0597f;
points[13].x = 0.2963710f, points[13].y = 0.3285541f, points[13].z = 2.0553f;
points[14].x = 0.3458804f, points[14].y = 0.3285541f, points[14].z = 2.0553f;
points[15].x = 0.3928388f, points[15].y = 0.3334204f, points[15].z = 2.0619f;
points[16].x = 0.4425072f, points[16].y = 0.3372317f, points[16].z = 2.0619f;
points[17].x = 0.4883550f, points[17].y = 0.3372317f, points[17].z = 2.0619f;
points[18].x = 0.2471258f, points[18].y = 0.3897563f, points[18].z = 2.0575f;
points[19].x = 0.2922494f, points[19].y = 0.3889228f, points[19].z = 2.0531f;
points[20].x = 0.3420720f, points[20].y = 0.3931387f, points[20].z = 2.0553f;
points[21].x = 0.3881880f, points[21].y = 0.3973626f, points[21].z = 2.0575f;
points[22].x = 0.4391546f, points[22].y = 0.3986373f, points[22].z = 2.0641f;
points[23].x = 0.4840173f, points[23].y = 0.4015947f, points[23].z = 2.0597f;
points[24].x = 0.2430531f, points[24].y = 0.4501251f, points[24].z = 2.0553f;
points[25].x = 0.2925625f, points[25].y = 0.4539242f, points[25].z = 2.0553f;
points[26].x = 0.3386256f, points[26].y = 0.4582132f, points[26].z = 2.0575f;
points[27].x = 0.3894332f, points[27].y = 0.4596831f, points[27].z = 2.0641f;
points[28].x = 0.4353299f, points[28].y = 0.4634984f, points[28].z = 2.0641f;
points[29].x = 0.4840173f, points[29].y = 0.4663176f, points[29].z = 2.0597f;
points[30].x = 0.2395008f, points[30].y = 0.5152606f, points[30].z = 2.0575f;
points[31].x = 0.2893723f, points[31].y = 0.5196187f, points[31].z = 2.0597f;
points[32].x = 0.3344551f, points[32].y = 0.5185088f, points[32].z = 2.0553f;
points[33].x = 0.3860195f, points[33].y = 0.5251033f, points[33].z = 2.0663f;
points[34].x = 0.4315052f, points[34].y = 0.5283596f, points[34].z = 2.0641f;
points[35].x = 0.4812266f, points[35].y = 0.5283596f, points[35].z = 2.0641f;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

cloud->width = 36;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize(cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size(); ++i)
{
    cloud->points[i].x = points[i].x;
    cloud->points[i].y = points[i].y;
    cloud->points[i].z = points[i].z;
}

Eigen::VectorXf oef;
const pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.005);
ransac.computeModel();
ransac.getModelCoefficients(oef);

std::cout << "coef: " << std::endl << oef << std::endl;

• C# 封装

头文件

#ifndef FRIDAYLIBRARY_LIBRARY_H
#define FRIDAYLIBRARY_LIBRARY_H
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/point_cloud.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <vector>
#ifdef __DLL_BUILD
#define DLL_EXPORT __declspec(dllexport)
#else
#define DLL_EXPORT __declspec(dllimport)
#endif

#ifdef __cplusplus
extern "C" {
#endif


/// <summary>
/// 通过一系列坐标点拟合平面（AX + BY + CX + D = 0）
/// </summary>
/// <param name="p_src_x">X坐标指针</param>
/// <param name="p_src_y">Y坐标指针</param>
/// <param name="p_src_z">Z坐标指针</param>
/// <param name="length">坐标点个数</param>
/// <param name="threshold">用于RANSAC拟合平面时的距离系数</param>
/// <param name="plane_coefficients">返回平面方程的系数[A,B,C,D]</param>
DLL_EXPORT void fit_plane(const float* p_src_x,
                          const float* p_src_y,
                          const float* p_src_z,
                          const size_t length,
                          const double threshold, float* plane_coefficients);
#ifdef __cplusplus
}
#endif
#endif //FRIDAYLIBRARY_LIBRARY_H

源文件

#define __DLL_BUILD
#include "library.h"



/// <summary>
/// 通过一系列坐标点拟合平面（AX + BY + CX + D = 0）
/// </summary>
/// <param name="p_src_x">X坐标指针</param>
/// <param name="p_src_y">Y坐标指针</param>
/// <param name="p_src_z">Z坐标指针</param>
/// <param name="length">坐标点个数</param>
/// <param name="threshold">用于RANSAC拟合平面时的距离系数</param>
/// <param name="plane_coefficients">返回平面方程的系数[A,B,C,D]</param>
void fit_plane(const float* p_src_x,
              const float* p_src_y,
              const float* p_src_z,
              const size_t length,
              const double threshold, float* plane_coefficients)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	cloud->width = length;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width * cloud->height);
	for (size_t i = 0; i < length; ++i)
	{
		cloud->points[i].x = p_src_x[i];
		cloud->points[i].y = p_src_y[i];
		cloud->points[i].z = p_src_z[i];
	}

	const pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
	ransac.setDistanceThreshold(threshold);
	ransac.setMaxIterations(1000);
	ransac.computeModel();

	Eigen::VectorXf coefficients;
	ransac.getModelCoefficients(coefficients);
	
	plane_coefficients[0] = coefficients[0];
	plane_coefficients[1] = coefficients[1];
	plane_coefficients[2] = coefficients[2];
	plane_coefficients[3] = coefficients[3];

}

C#调用

using System;
using System.IO;
using System.Runtime.InteropServices;

namespace ConsoleApp1
{
    internal class Program
    {
        public struct Buffer
        {
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 128)]
            public byte[] DstAddr;
        }
        [DllImport("./FridayLibrary", CallingConvention = CallingConvention.Cdecl, EntryPoint = "fit_plane")]
        public static extern int FitPlane(float[] x, float[] y, float[] z, int length, double threshold, ref Buffer buf);


        static void Main(string[] args)
        {
            var x = new float[] {0.254206f ,
                        0.3039878f,
                        0.3500631f,
                        0.4000528f,
                        0.4463278f,
                        0.495467f ,
                        0.2547508f,
                        0.2995513f,
                        0.3458804f,
                        0.395813f ,
                        0.4415629f,
                        0.4916504f,
                        0.2512066f,
                        0.296371f ,
                        0.3458804f,
                        0.3928388f,
                        0.4425072f,
                        0.488355f ,
                        0.2471258f,
                        0.2922494f,
                        0.342072f ,
                        0.388188f ,
                        0.4391546f,
                        0.4840173f,
                        0.2430531f,
                        0.2925625f,
                        0.3386256f,
                        0.3894332f,
                        0.4353299f,
                        0.4840173f,
                        0.2395008f,
                        0.2893723f,
                        0.3344551f,
                        0.3860195f,
                        0.4315052f,
                        0.4812266f
                        };
            var y = new float[] {0.1991714f,
                        0.1993848f,
                        0.2034014f,
                        0.2074261f,
                        0.2076477f,
                        0.2112333f,
                        0.2604488f,
                        0.2634172f,
                        0.2677685f,
                        0.2680552f,
                        0.2718583f,
                        0.2759562f,
                        0.3254502f,
                        0.3285541f,
                        0.3285541f,
                        0.3334204f,
                        0.3372317f,
                        0.3372317f,
                        0.3897563f,
                        0.3889228f,
                        0.3931387f,
                        0.3973626f,
                        0.3986373f,
                        0.4015947f,
                        0.4501251f,
                        0.4539242f,
                        0.4582132f,
                        0.4596831f,
                        0.4634984f,
                        0.4663176f,
                        0.5152606f,
                        0.5196187f,
                        0.5185088f,
                        0.5251033f,
                        0.5283596f,
                        0.5283596f
                        };
            var z = new float[] {2.0531f,
                        2.0553f,
                        2.0575f,
                        2.0597f,
                        2.0619f,
                        2.0597f,
                        2.0575f,
                        2.051f ,
                        2.0553f,
                        2.0575f,
                        2.0575f,
                        2.0597f,
                        2.0597f,
                        2.0553f,
                        2.0553f,
                        2.0619f,
                        2.0619f,
                        2.0619f,
                        2.0575f,
                        2.0531f,
                        2.0553f,
                        2.0575f,
                        2.0641f,
                        2.0597f,
                        2.0553f,
                        2.0553f,
                        2.0575f,
                        2.0641f,
                        2.0641f,
                        2.0597f,
                        2.0575f,
                        2.0597f,
                        2.0553f,
                        2.0663f,
                        2.0641f,
                        2.0641f
                        };

            Buffer buf = new Buffer {DstAddr = new byte[128]};

            float[] coefficients = new float[4];
            var ret = FitPlane(x, y, z, z.Length, 0.005, ref buf);

            using (var stream = new MemoryStream(buf.DstAddr))
            {
                using (var reader = new BinaryReader(stream))
                {
                    var a = reader.ReadSingle();
                    var b = reader.ReadSingle();
                    var c = reader.ReadSingle();
                    var d = reader.ReadSingle();

                    Console.WriteLine($"{a}, {b}, {c}, {d}");
                }
            }

            Console.ReadKey();
        }
    }
}