一些点云笔记

最新推荐文章于 2024-05-17 18:09:12 发布

小巫山云子

最新推荐文章于 2024-05-17 18:09:12 发布

阅读量118

点赞数

文章标签：笔记

本文链接：https://blog.csdn.net/SmallPink0713/article/details/134079011

版权

旋转点云计算正余弦

Q:在计算平面旋转时会计算旋转角度的正弦和余弦值，问为什么要这么计算？

A:计算旋转角度的正弦和余弦值是为了确定平面上一个点绕着交线旋转的角度。

在三维空间中，旋转可以由旋转矩阵或四元数来表示。但在二维平面（例如计算机图形学）中，我们通常使用三角函数（正弦和余弦）来表示旋转。这是因为平面上的旋转可以通过两个坐标轴上的旋转来表示。

例如，假设我们有一个平面，并且我们知道一个点P在平面上。如果我们想要围绕交线R"§旋转这个点，我们可以使用正弦和余弦值来确定旋转的方向和角度。这样，我们就可以计算出新的点的坐标。

点云的点积计算

Q:计算线面/面面之间的关系时，为什么会计算点积？

A:点积用于计算平面的法向量和平面上一个点的连线与平面的法向量之间的角度。这是通过将平面的一个点与平面的法向量相乘，然后将结果与平面的法向量的长度相乘，最后除以这个点积得到的。

如果点积等于0，那么这两个向量就是正交的，也就是说它们之间的角度是90度。如果点积大于0，那么这两个向量就是正交的，也就是说它们之间的角度小于90度。如果点积小于0，那么这两个向量就是负交的，也就是说它们之间的角度大于90度。

pcl中数据类型和数据结构

在Point Cloud Library（PCL）中，数据类型和数据结构是两个不同的概念。pcl::PointCloudpcl::PointXYZ表示一个点云数据结构，其中每个点都是pcl::PointXYZ类型的。它可以用于存储、处理和可视化三维点云数据。

数据类型指的是在PCL中用于表示点云数据的类型。例如，pcl::PointXYZ是一种数据类型，用于表示三维空间中的点，包含x、y和z三个坐标值。数据类型定义了点的属性和结构。

数据结构指的是在PCL中用于组织和存储点云数据的方式。例如，pcl::PointCloud是一种数据结构，用于表示一个点云，可以包含多个点。数据结构定义了点云的组织方式和操作方式。

因此，在PCL中，数据类型用于定义点的属性，而数据结构用于组织点云数据。通过选择合适的数据类型和数据结构，可以有效地处理和操作点云数据。

点云中的平面表示

在PCL库中，点到平面的距离计算公式为：
distance = |Ax + By + Cz + D| / sqrt(A^2 + B^2 + C^2)
其中，(x, y, z)为点的坐标，(A, B, C)为平面的法向量，D为平面参数方程中的常数项。

这里的mo并没有明确的物理意义，它可能是点到平面距离计算公式中的除数，用来平衡平面参数方程中的常数项D。这个公式是一种点到平面距离的近似计算方法，它的精度受到mo取值的影响，取值过大或过小都可能影响计算结果的准确性。

评估点云配准精度

输入两个点云首先建立kdtree 找到源点云和目标点云中距离最近的对应点然后求两个点之间的距离和方向向量将方向向量转换为单位向量然后记录两个点的距离评估所有向量的长度分布计算向量分布的半高宽给出所有向量长度的标准差估计值

#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

// 计算两个点之间的距离和方向向量的函数
//void compute_distance_and_direction(const pcl::PointXYZ& point1, const pcl::PointXYZ& point2, double& distance, Eigen::Vector3f& direction)
//{
//    distance = (point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y) + (point1.z - point2.z) * (point1.z - point2.z);
//    direction = (point2.x - point1.x) / distance;
//}

// 将方向向量转换为单位向量的函数
Eigen::Vector3f convert_to_unit_vector(const Eigen::Vector3f& direction)
{
    float length = std::sqrt(direction.x() * direction.x() + direction.y() * direction.y() + direction.z() * direction.z());
    return Eigen::Vector3f(direction.x() / length, direction.y() / length, direction.z() / length);
}

// 计算向量长度
double vector_length(const std::vector<float>& vec) {
    double sum = 0.0;
    for (auto& val : vec) {
        sum += val * val;
    }
    return std::sqrt(sum);
}

// 评估所有向量的长度分布
std::vector<double> length_distribution(const std::vector<std::vector<float>>& point_cloud) {
    std::vector<double> lengths;
    for (auto& val : point_cloud) 
    {
         lengths.push_back(vector_length(val));
    }
    return lengths;
}

// 计算向量分布的半高宽
double half_max_width(const std::vector<double>& lengths, double max_length) {
    double half_max = 0.5 * max_length;
    auto it = std::lower_bound(lengths.begin(), lengths.end(), half_max);
    if (it == lengths.end()) {
        return 0.0; // 没有超过半高宽的向量
    }
    else if (it == lengths.begin()) {
        return -1.0; // 只有小于半高宽的向量
    }
    else {
        return *(it - 1); // 前一个元素就是半高宽
    }
}
// 计算所有向量长度的标准差估计值
double standard_deviation(const std::vector<double>& lengths) {
    double mean = std::accumulate(lengths.begin(), lengths.end(), 0.0) / lengths.size();
    double sq_sum = std::inner_product(lengths.begin(), lengths.end(), lengths.begin(), 0.0);
    return std::sqrt(sq_sum / lengths.size() - mean * mean);
}

int main(int argc, char** argv)
{
     // 读取点云文件
    pcl::PointCloud<pcl::PointXYZ> source_cloud_;
    pcl::PointCloud<pcl::PointXYZ> target_cloud_;

    std::string file_name_one = "";
    std::string file_name_two = "";

    pcl::io::loadPLYFile<pcl::PointXYZ >(file_name_one, target_cloud_);
    pcl::io::loadPLYFile<pcl::PointXYZ >(file_name_two, source_cloud_);

    pcl::KdTreeFLANN<pcl::PointXYZ > tree;
    tree.setInputCloud(target_cloud_.makeShared());

    pcl::Indices nn_indices(1);
    std::vector<float>nn_dists(1);

    int cloud_size = source_cloud_.size();
    pcl::PointXYZ cluster_indices;
    std::vector<std::vector<float>> direction;

    for (size_t i = 0;i < cloud_size;i++)
    {
        cluster_indices = source_cloud_.at(i);
        tree.nearestKSearch(cluster_indices,1, nn_indices, nn_dists);
        cluster_indices = target_cloud_.at(nn_indices[0]);

        float dx = source_cloud_[i].x - cluster_indices.x;
        float dy = source_cloud_[i].y - cluster_indices.y;
        float dz = source_cloud_[i].z - cluster_indices.z;
        direction.push_back({ dx, dy, dz });

    }

        // 计算所有点云向量长度分布
        std::vector<double> lengths = length_distribution(direction);
        // 计算向量分布的半高宽
        double half_max = half_max_width(lengths, *std::max_element(lengths.begin(), lengths.end()));
        // 计算所有向量长度的标准差估计值
        double std_dev = standard_deviation(lengths);

        std::cout << half_max << endl;
        std::cout << std_dev << endl;

    return 0;
}

计算点云的最大最小特征值

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// 计算点云的均值向量
Eigen::Vector3d computeMeanVector(const std::vector<Eigen::Vector3d>& vector) {
    double sum_x = 0, sum_y = 0, sum_z = 0;
    for (const auto& vec : vector) { //遍历vector中的每个元素,并将值赋给vec
        sum_x += vec[0]; //将当前向量vec的第一个分量（索引为0）的值加到sum_x上。
        sum_y += vec[1]; 
        sum_z += vec[2];
    }
    return Eigen::Vector3d(sum_x / vector.size(), sum_y / vector.size(), sum_z / vector.size()); //计算了向量集合的平均向量。
}

// 计算协方差矩阵
//函数computeCovarianceMatrix，它接受一个类型为vector<Eigen::Vector3d>的参数vector。该函数的作用是计算给定向量集合的协方差矩阵。
Eigen::Matrix3d computeCovarianceMatrix(const vector<Eigen::Vector3d>& vector) {
   Eigen::Vector3d mean = computeMeanVector(vector);
   Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero(); //建了一个3x3的零矩阵，用于存储协方差矩阵。
    for (const auto& vec : vector) {
        Vector3d diff = vec - mean; //计算当前向量与均值向量之间的差异，并将结果存储在变量diff中。
        covariance += diff * diff.transpose(); //将差异向量的转置与差异向量相乘，然后将结果累加到协方差矩阵中。
    }
    covariance /= vector.size() - 1; //将协方差矩阵除以向量集合的大小减一，以进行归一化处理。
    return covariance;
}

// 计算特征值
pair<double, double> computeEigenvalues(const Eigen::Matrix3d& matrix) {
//创建了一个Eigen::SelfAdjointEigenSolver对象，用于求解自伴矩阵的特征值。自伴矩阵是指满足其共轭转置等于其本身的方阵。这里将输入的matrix作为参数传递给eigensolver对象。
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(matrix);

    return make_pair(eigensolver.eigenvalues()[0], eigensolver.eigenvalues()[1]);
}

iint main(int argc, char** argv)
{
     // 读取点云文件
    pcl::PointCloud<pcl::PointXYZ> source_cloud_;
    pcl::PointCloud<pcl::PointXYZ> target_cloud_;

    std::string file_name_one = "";
    std::string file_name_two = "";

    pcl::io::loadPLYFile<pcl::PointXYZ >(file_name_one, target_cloud_);
    pcl::io::loadPLYFile<pcl::PointXYZ >(file_name_two, source_cloud_);

    pcl::KdTreeFLANN<pcl::PointXYZ > tree;
    tree.setInputCloud(target_cloud_.makeShared());

    pcl::Indices nn_indices(1);
    std::vector<float>nn_dists(1);

    int cloud_size = source_cloud_.size();
    pcl::PointXYZ cluster_indices;
    std::vector<std::vector<float>> direction;
    std::vector<Eigen::Vector3d> vector;

    for (size_t i = 0;i < cloud_size;i++)
    {
        cluster_indices = source_cloud_.at(i);
        tree.nearestKSearch(cluster_indices,1, nn_indices, nn_dists);
        cluster_indices = target_cloud_.at(nn_indices[0]);

        float dx = source_cloud_[i].x - cluster_indices.x;
        float dy = source_cloud_[i].y - cluster_indices.y;
        float dz = source_cloud_[i].z - cluster_indices.z;
        direction.push_back({ dx, dy, dz });  //计算点云方向向量
        
        float length = std::sqrt(pow(dx,2)+pow(dy,2)+pow(dz,2));
        vector.push_back({ dx/length, dy/length, dz/length }); //计算点云单位向量
        
        //输出所有vector
        std::cout << "component " << i + 1 << ": " << vector[i] << std::endl;
        //输出vector的size
        std::cout << "The size of the vector is: " << vector.size() << std::endl;
        
    }

    // 计算均值向量
    Eigen::Vector3d mean = computeMeanVector(vector);
    cout << "Mean vector: " << mean.transpose() << endl;

    // 计算协方差矩阵
    Eigen::Matrix3d covariance = computeCovarianceMatrix(vectors);
    cout << "Covariance matrix: " << covariance << endl;

    // 计算特征值
    pair<double, double> eigenvalues = computeEigenvalues(covariance);
    cout << "Eigenvalues: " << eigenvalues.first << ", " << eigenvalues.second << endl;

    // 找到最大和最小特征值
    double min_eigenvalue = eigenvalues.first;
    double max_eigenvalue = eigenvalues.second;
    for (int i = 0; i < 3; ++i) {
        for (int j = i + 1; j < 3; ++j) {
            if (eigenvalues.first < min_eigenvalue || (eigenvalues.first == min_eigenvalue && eigenvalues.second < min_eigenvalue)) {
                min_eigenvalue = eigenvalues.first;
                max_eigenvalue = eigenvalues.second;
            } else if (eigenvalues.first > max_eigenvalue || (eigenvalues.first == max_eigenvalue && eigenvalues.second > max_eigenvalue)) {
                min_eigenvalue = eigenvalues.first;
                max_eigenvalue = eigenvalues.second;
            }
        }
    }
    cout << "Minimum eigenvalue: " << min_eigenvalue << ", Maximum eigenvalue: " << max_eigenvalue << endl;

    return 0;
}