ros开发增加clion常用模板及初始化配置(二)

python模板

py_cov协方差

import numpy as np

import matplotlib.pyplot as plt

plt.style.use("ggplot")
plt.rcParams['figure.figsize'] = (12, 8)

# 生成随即向量（x， y方向），均值mean为0， 标准差stdandard deviation 为1
x = np.random.normal(0.0, 1.0, 200)
y = np.random.normal(0.0, 1.0, 200)

X = np.vstack((x, y))

# 绘制到坐标系中
plt.scatter(X[0], X[1])
# plt.scatter(x, y)
plt.title("Generated Data")
# 确保坐标系x，y轴正负方向范围一致
plt.axis("equal")
plt.show()

# 计算这些点集的协方差矩阵

# 计算两组数据的协方差
def cov(x, y):
    x_bar, y_bar = np.mean(x), np.mean(y)
    return np.sum((x - x_bar) * (y - y_bar)) / (len(x) - 1)


# print(cov(x, x))
# print(cov(x, y))
# print(cov(y, y))
def cov_mat(X):
    return np.array([
        [cov(X[0], X[0]), cov(X[0], X[1])],
        [cov(X[1], X[0]), cov(X[1], X[1])]
    ])

print(cov_mat(X))

---------------------------

c++模板

cpp_pcl_NormalEstimation计算PFH_main

#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

/**
 * 加载点云
 * 计算法向量
 * 计算PFH
 */
int main(int argc, char *argv[]) {

    // 加载点云
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    pcl::io::loadPCDFile("./data/bunny.pcd", *cloud);

    pcl::PointCloud::Ptr normals(new  pcl::PointCloud);

    // 估算法向量
    pcl::NormalEstimation normalEstimation;
    normalEstimation.setInputCloud(cloud);
    // 法向量邻域参考范围
    normalEstimation.setRadiusSearch(0.03);
    // 设置邻域查找方式
    pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree);
    normalEstimation.setSearchMethod(kdtree);
    // 得到法向量
    normalEstimation.compute(*normals);

    // 创建PFH估算对象，把数据集和法线作为参数
    pcl::PFHEstimation pfh;
    // 设置输入数据
    pfh.setInputCloud(cloud);
    pfh.setInputNormals(normals);
    pfh.setSearchMethod(kdtree);
    // 使用半径为8cm的球体，作为搜索的邻域。必须要比用于估算法向量的半径要大！！！！！！！！
    pfh.setRadiusSearch(0.08);
    // 计算特征
    pcl::PointCloud::Ptr pfh_output(new  pcl::PointCloud);
    pfh.compute(*pfh_output);
// 打印输出结果
    unsigned long size = pfh_output->points.size();
    for (int i = 0; i < size; ++i) {
        pcl::PFHSignature125 &signature125 = pfh_output->points[i];
        float* hh = signature125.histogram;

        printf("%d: %f, %f, %f, %f, %f \n", i, hh[1], hh[2], hh[3], hh[4], hh[5]);
    }

    std::cout << "PFH直方图个数：" << size << std::endl;
    std::cout << "cloud点云个数：" << cloud->points.size() << std::endl;

    // 可以显示法线
    pcl::visualization::PCLVisualizer viewer("viewer");
    viewer.addPointCloudNormals(cloud, normals, 1, 0.01, "normals");

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }

    return 0;

}