欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

【PCL】平面拟合

程序员文章站 2024-03-25 10:45:22
...
  • 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();
        }
    }
}

 

相关标签: PCL