点云匹配代码

点云匹配代码，计算点云的旋转矩阵，平移矩阵，匹配精度高．

/***************************************************************************
Copyright: Huang Xiaohang / Piec
Author: Huang Xiaohang
Date: 2017-12-06
Description: Implementation of the classes and functions used in the project
****************************************************************************/

#include “PointCloudToolbox.h“


///  Implementation of Class PIPointCloud  ///

PIPointCloud::PIPointCloud（） : cloud（new PointCloudT）{}

void PIPointCloud::printTransformationMatrix （）
{
    printf （“Transformation information:\n“）;
    printf （“Rotation matrix :\n“）;
    printf （“    | %6.3f %6.3f %6.3f | \n“    transform_matrix （0 0） transform_matrix （0 1） transform_matrix （0 2））;
    printf （“R = | %6.3f %6.3f %6.3f | \n“    transform_matrix （1 0） transform_matrix （1 1） transform_matrix （1 2））;
    printf （“    | %6.3f %6.3f %6.3f | \n“    transform_matrix （2 0） transform_matrix （2 1） transform_matrix （2 2））;
    printf （“Translation vector :\n“）;
    printf （“t = < %6.3f %6.3f %6.3f >\n\n“ transform_matrix （0 3） transform_matrix （1 3） transform_matrix （2 3））;
}

void PIPointCloud::pointCloudRotate（Eigen::Matrix3f rotate_matrix）
{
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity（）;
    matrix （0 0） =  rotate_matrix （0 0）;
    matrix （0 1） =  rotate_matrix （0 1）;
    matrix （0 2） =  rotate_matrix （0 2）;
    matrix （1 0） =  rotate_matrix （1 0）;
    matrix （1 1） =  rotate_matrix （1 1）;
    matrix （1 2） =  rotate_matrix （1 2）;
    matrix （2 0） =  rotate_matrix （2 0）;
    matrix （2 1） =  rotate_matrix （2 1）;
    matrix （2 2） =  rotate_matrix （2 2）;
    pcl::transformPointCloud （*cloud *cloud matrix）;
}

void PIPointCloud::pointCloudTranslate（Eigen::Vector3f translate_matrix）
{
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity（）;
    matrix （0 3） =  translate_matrix （0 0）;
    matrix （1 3） =  translate_matrix （0 1）;
    matrix （2 3） =  translate_matrix （0 2）;
    pcl::transformPointCloud （*cloud *cloud matrix）;
}

Eigen::Matrix4f PIPointCloud::pointCloudRotateX（float theta）
{
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity（）;
    matrix （1 1） =  cos （theta * M_PI / 180）;
    matrix （1 2） = -sin （theta * M_PI / 180）;
    matrix （2 1） =  sin （theta * M_PI / 180）;
    matrix （2 2） =  cos （theta * M_PI / 180）;
    pcl::transformPointCloud （*cloud *cloud matrix）;
    return matrix;
}

Eigen::Matrix4f PIPointCloud::pointCloudRotateY（float theta）
{
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity（）;
    matrix （0 0） =  cos （theta * M_PI / 180）;
    matrix （0 2） =  sin （theta * M_PI / 180）;
    matrix （2 0） = -sin （theta * M_PI / 180）;
    matrix （2 2） =  cos （theta * M_PI / 180）;
    pcl::transformPointCloud （*cloud *cloud matrix）;
    return matrix;
}

Eigen::Matrix4f PIPointCloud::pointCloudRotateZ（float theta）
{
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity（）;
    matrix （0 0） =  cos （theta * M_PI / 180）;
    matrix （0 1） = -sin （theta * M_PI / 180）;
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-12-19 09:06  PointCloudPoseEstimation-master\
     文件         692  2017-12-19 09:06  PointCloudPoseEstimation-master\CMakeLists.txt
     目录           0  2017-12-19 09:06  PointCloudPoseEstimation-master\data\
     文件         243  2017-12-19 09:06  PointCloudPoseEstimation-master\data\Cam21RotationMatrix.xml
     文件         208  2017-12-19 09:06  PointCloudPoseEstimation-master\data\Cam21TranslationMatrix.xml
     文件         243  2017-12-19 09:06  PointCloudPoseEstimation-master\data\Cam31RotationMatrix.xml
     文件         208  2017-12-19 09:06  PointCloudPoseEstimation-master\data\Cam31TranslationMatrix.xml
     文件      951011  2017-12-19 09:06  PointCloudPoseEstimation-master\data\fraction1.pcd
     文件      984488  2017-12-19 09:06  PointCloudPoseEstimation-master\data\fraction2.pcd
     文件      942094  2017-12-19 09:06  PointCloudPoseEstimation-master\data\fraction3.pcd
     文件      840962  2017-12-19 09:06  PointCloudPoseEstimation-master\data\model.pcd
     目录           0  2017-12-19 09:06  PointCloudPoseEstimation-master\include\
     文件        1440  2017-12-19 09:06  PointCloudPoseEstimation-master\include\PointCloudPoseEstimation.h
     文件       16339  2017-12-19 09:06  PointCloudPoseEstimation-master\include\PointCloudToolbox.cpp
     文件       27335  2017-12-19 09:06  PointCloudPoseEstimation-master\include\PointCloudToolbox.h
     目录           0  2017-12-19 09:06  PointCloudPoseEstimation-master\src\
     文件        3790  2017-12-19 09:06  PointCloudPoseEstimation-master\src\PointCloudPoseEstimation.cpp