GO-ICP的使用(一)

一、代码下载以、修改以及使用

下载：

链接：yangjiaolong/Go-ICP: Implementation of the Go-ICP algorithm for globally optimal 3D pointset registration (github.com)

解压之后：

首先visual studio项目，配置好PCL环境；首先把上述图片中看得到的以cpp、h、hpp结尾的文件全都放入你的项目中就行了。

其中pointTypeTrans.h和pointTypeTrans.cpp是我为了可视化新建的文件，源文件里面并没有。

修改：

由于该代码运行时可能会出现重定义的情况，你需要修改定义，源码有可视化程序，不过是用matlab进行可视化，代码就在demo文件夹下，我这里修改了下，使得程序可以直接进行可视化

1、重定义修改

matrix.h把这行注释掉，或者重定义成另一个名字，不过重定义成另一个新名字之后，要把matrix.h和matrix.cpp的所有引用原来的名字的地方都改成新名字

matrix.h把这两行重定义成另一个名字FLOAT2，不过重定义成FLOAT2之后，要把matrix.h和matrix.cpp的所有引用FLOAT的地方都改成FLOAT2

2、可视化的修改

pointTypeTrans.h

#ifndef POINTTYPETRANS_H
#define POINTTYPETRANS_H
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <sstream>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PloudCloud;
struct Vertex {
	float x, y, z;
};
class PointTypeTrans
{
public:
	PointTypeTrans();
	void writePLYHeader(std::ofstream& ply_file, int num_points);
	void writePCDHeader(std::ofstream& pcdFile, int vertexCount);
	int txtToPly(const std::string txt_filename, const std::string ply_filename);
	int plyTotxt(const std::string ply_filename, const std::string txt_filename);
	int txtToPcd(const std::string txt_filename, const std::string pcd_filename);
	int pcdTotxt(const std::string pcd_filename, const std::string txt_filename);
	int txtToObj(const std::string txt_filename, const std::string obj_filename);
	int objTotxt(const std::string obj_filename, const std::string txt_filename);
	int plyToPcd(const std::string plyFilename, const std::string pcdFilename);
	int pcdToPly(const std::string pcdFilename, const std::string plyFilename);
	int plyToObj(const std::string plyFilename, const std::string objFilename);
	int objToPly(const std::string objFilename, const std::string plyFilename);
	int pcdToObj(const std::string pcdFilename, const std::string objFilename);
	int objToPcd(const std::string objFilename, const std::string pcdFilename);
	void view_display(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target, pcl::PointCloud<pcl::PointXYZ>::Ptr result);
	void format_selection(std::string& suffix, int flag, std::string& file);
	~PointTypeTrans();

};







#endif // !POINTTYPETRANS_H

pointTypeTrans.cpp

#include "pointTypeTrans.h"

PointTypeTrans::PointTypeTrans()
{

}

void PointTypeTrans::writePLYHeader(std::ofstream& ply_file, int num_points)
{
    ply_file << "ply\n";
    ply_file << "format ascii 1.0\n";
    ply_file << "element vertex " << num_points << "\n";
    ply_file << "property float x\n";
    ply_file << "property float y\n";
    ply_file << "property float z\n";
    ply_file << "end_header\n";
}

void PointTypeTrans::writePCDHeader(std::ofstream& pcdFile, int vertexCount)
{
    pcdFile << "# .PCD v0.7 - Point Cloud Data file format\n";
    pcdFile << "VERSION 0.7\n";
    pcdFile << "FIELDS x y z\n";
    pcdFile << "SIZE 4 4 4\n";
    pcdFile << "TYPE F F F\n";
    pcdFile << "COUNT 1 1 1\n";
    pcdFile << "WIDTH " << vertexCount << "\n";
    pcdFile << "HEIGHT 1\n";
    pcdFile << "VIEWPOINT 0 0 0 1 0 0 0\n";
    pcdFile << "POINTS " << vertexCount << "\n";
    pcdFile << "DATA ascii\n";
}
//1
int PointTypeTrans::txtToPly(const std::string txt_filename, const std::string ply_filename)
{
    if (strcmp(txt_filename.substr(txt_filename.size() - 4, 4).c_str(), ".txt") != 0)
        return 0;
    if (strcmp(ply_filename.substr(ply_filename.size() - 4, 4).c_str(), ".ply") != 0)
        return 0;
    std::ifstream txt_file(txt_filename);
    std::ofstream ply_file(ply_filename);

    if (!txt_file.is_open() || !ply_file.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    int num_points;
    txt_file >> num_points;

    // Write the PLY header
    writePLYHeader(ply_file, num_points);

    // Write the point data
    float x, y, z;
    for (int i = 0; i < num_points; ++i) {
        txt_file >> x >> y >> z;
        ply_file << x << " " << y << " " << z << "\n";
    }

    // Clean up
    txt_file.close();
    ply_file.close();

    std::cout << "Conversion from TXT to PLY completed successfully." << std::endl;
    return 1;
}

//2
int PointTypeTrans::plyTotxt(const std::string ply_filename, const std::string txt_filename)
{
    if (strcmp(txt_filename.substr(txt_filename.size() - 4, 4).c_str(), ".txt") != 0)
        return 0;
    if (strcmp(ply_filename.substr(ply_filename.size() - 4, 4).c_str(), ".ply") != 0)
        return 0;
    std::ifstream ply_file(ply_filename);
    std::ofstream txt_file(txt_filename);

    if (!ply_file.is_open() || !txt_file.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::string line;
    int num_points = 0;
    bool header_ended = false;

    // Read the PLY header to find the number of vertex elements
    while (getline(ply_file, line)) {
        std::istringstream iss(line);
        std::string token;
        iss >> token;

        if (token == "element") {
            iss >> token; // This should be "vertex"
            if (token == "vertex") {
                iss >> num_points;
            }
        }
        else if (token == "end_header") {
            header_ended = true;
            break;
        }
    }

    if (!header_ended) {
        std::cerr << "Could not find the end of the header." << std::endl;
        return 0;
    }

    // Write the number of points to the TXT file
    txt_file << num_points << std::endl;

    // Read and write the point data
    float x, y, z;
    for (int i = 0; i < num_points; ++i) {
        ply_file >> x >> y >> z;
        txt_file << x << " " << y << " " << z << std::endl;
    }

    // Clean up
    ply_file.close();
    txt_file.close();

    std::cout << "Conversion from PLY to TXT completed successfully." << std::endl;

    return 1;
}

//3
int PointTypeTrans::txtToPcd(const std::string txt_filename, const std::string pcd_filename)
{
    if (strcmp(txt_filename.substr(txt_filename.size() - 4, 4).c_str(), ".txt") != 0)
        return 0;
    if (strcmp(pcd_filename.substr(pcd_filename.size() - 4, 4).c_str(), ".pcd") != 0)
        return 0;
    std::ifstream txt_file(txt_filename);
    std::ofstream pcd_file(pcd_filename);

    if (!txt_file.is_open() || !pcd_file.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    // Read the number of points from the first line of the TXT file
    int num_points;
    txt_file >> num_points;

    // Write the PCD header
    writePCDHeader(pcd_file, num_points);

    // Read and write the point data
    float x, y, z;
    for (int i = 0; i < num_points; ++i) {
        txt_file >> x >> y >> z;
        pcd_file << x << " " << y << " " << z << "\n";
    }

    // Clean up
    txt_file.close();
    pcd_file.close();

    std::cout << "Conversion from TXT to PCD completed successfully." << std::endl;

    return 1;
}
//4
int PointTypeTrans::pcdTotxt(const std::string pcd_filename, const std::string txt_filename)
{
    if (strcmp(txt_filename.substr(txt_filename.size() - 4, 4).c_str(), ".txt") != 0)
        return 0;
    if (strcmp(pcd_filename.substr(pcd_filename.size() - 4, 4).c_str(), ".pcd") != 0)
        return 0;
    std::ifstream pcd_file(pcd_filename);
    std::ofstream txt_file(txt_filename);

    if (!pcd_file.is_open() || !txt_file.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::string line;
    int num_points = 0;
    bool headerPassed = false;

    // Skip the PCD header
    while (std::getline(pcd_file, line)) {
        std::stringstream ss(line);
        std::string firstWord;
        ss >> firstWord;

        // Check for lines starting with "POINTS"
        if (firstWord == "POINTS") {
            ss >> num_points;
        }

        // Check for the end of the header, which is marked as "DATA"
        if (firstWord == "DATA") {
            headerPassed = true;
            break;
        }
    }

    // If we didn't reach the end of the header, exit
    if (!headerPassed) {
        std::cerr << "PCD header not found or improper format." << std::endl;
        return 0;
    }

    // Write the number of points to the TXT file
    txt_file << num_points << "\n";

    // Read and write the point data
    float x, y, z;
    while (pcd_file >> x >> y >> z) {
        txt_file << x << " " << y << " " << z << "\n";
    }

    // Clean up
    pcd_file.close();
    txt_file.close();

    std::cout << "Conversion from PCD to TXT completed successfully." << std::endl;

    return 1;
}
//5
int PointTypeTrans::txtToObj(const std::string txt_filename, const std::string obj_filename)
{
    if (strcmp(txt_filename.substr(txt_filename.size() - 4, 4).c_str(), ".txt") != 0)
        return 0;
    if (strcmp(obj_filename.substr(obj_filename.size() - 4, 4).c_str(), ".obj") != 0)
        return 0;
    std::ifstream txt_file(txt_filename);
    std::ofstream obj_file(obj_filename);

    if (!txt_file.is_open() || !obj_file.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    // Optional: Read the number of points if it's provided
    int num_points;
    txt_file >> num_points;

    // Write vertices to the OBJ file
    float x, y, z;
    while (txt_file >> x >> y >> z) {
        obj_file << "v " << x << " " << y << " " << z << "\n";
    }

    // Clean up
    txt_file.close();
    obj_file.close();

    std::cout << "Conversion from TXT to OBJ completed successfully." << std::endl;

    return 1;
}
//6
int PointTypeTrans::objTotxt(const std::string obj_filename, const std::string txt_filename)
{
    if (strcmp(txt_filename.substr(txt_filename.size() - 4, 4).c_str(), ".txt") != 0)
        return 0;
    if (strcmp(obj_filename.substr(obj_filename.size() - 4, 4).c_str(), ".obj") != 0)
        return 0;
    std::ifstream obj_file(obj_filename);
    std::ofstream txt_file(txt_filename);

    if (!obj_file.is_open() || !txt_file.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> vertices;
    std::string line;

    // Process each line of the OBJ file
    while (std::getline(obj_file, line)) {
        // Skip empty lines and comments
        if (line.empty() || line[0] == '#') continue;

        std::stringstream ss(line);
        std::string prefix;
        ss >> prefix;

        // Store vertex data if the line starts with 'v'
        if (prefix == "v") {
            Vertex vertex;
            ss >> vertex.x >> vertex.y >> vertex.z;
            vertices.push_back(vertex);
        }
    }

    // First write the total number of vertices
    txt_file << vertices.size() << std::endl;

    // Then write all vertices
    for (const auto& vertex : vertices) {
        txt_file << vertex.x << " " << vertex.y << " " << vertex.z << std::endl;
    }

    // Clean up
    obj_file.close();
    txt_file.close();

    std::cout << "Conversion from OBJ to TXT completed successfully." << std::endl;

    return 1;
}
//7
int PointTypeTrans::plyToPcd(const std::string plyFilename, const std::string pcdFilename)
{
    if (strcmp(plyFilename.substr(plyFilename.size() - 4, 4).c_str(), ".ply") != 0)
        return 0;
    if (strcmp(pcdFilename.substr(pcdFilename.size() - 4, 4).c_str(), ".pcd") != 0)
        return 0;
    std::ifstream plyFile(plyFilename);
    std::ofstream pcdFile(pcdFilename);

    if (!plyFile.is_open() || !pcdFile.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> points;
    std::string line;
    bool headerPassed = false;
    int vertexCount = 0;
    while (std::getline(plyFile, line)) {
        std::stringstream ss(line);
        if (line == "end_header") {
            headerPassed = true;
            continue;
        }
        if (!headerPassed) {
            if (line.substr(0, 14) == "element vertex") {
                ss >> line >> line >> vertexCount; // Read vertex count
            }
            // Skip header lines
            continue;
        }
        else {
            // Read point data
            Vertex point;
            if (ss >> point.x >> point.y >> point.z) {
                points.push_back(point);
            }
        }
    }

    // Write PCD header
    writePCDHeader(pcdFile, vertexCount);

    // Write point data to PCD file
    for (const auto& point : points) {
        pcdFile << point.x << " " << point.y << " " << point.z << "\n";
    }

    plyFile.close();
    pcdFile.close();

    std::cout << "Conversion from PLY to PCD completed successfully." << std::endl;

    return 1;
}
//8
int PointTypeTrans::pcdToPly(const std::string pcdFilename, const std::string plyFilename)
{
    if (strcmp(plyFilename.substr(plyFilename.size() - 4, 4).c_str(), ".ply") != 0)
        return 0;
    if (strcmp(pcdFilename.substr(pcdFilename.size() - 4, 4).c_str(), ".pcd") != 0)
        return 0;
    std::ifstream pcdFile(pcdFilename);
    std::ofstream plyFile(plyFilename);

    if (!pcdFile.is_open() || !plyFile.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> points;
    std::string line;
    int pointCount = 0;
    bool dataSection = false;

    // Parse PCD file
    while (std::getline(pcdFile, line)) {
        std::stringstream ss(line);
        if (line.substr(0, 6) == "POINTS") {
            ss >> line >> pointCount;
        }
        else if (line.substr(0, 4) == "DATA") {
            dataSection = true;
            continue;
        }

        if (dataSection) {
            Vertex point;
            if (ss >> point.x >> point.y >> point.z) {
                points.push_back(point);
            }
        }
    }

    // Write PLY header
    writePLYHeader(plyFile, pointCount);

    // Write points to PLY file
    for (const auto& point : points) {
        plyFile << point.x << " " << point.y << " " << point.z << "\n";
    }

    pcdFile.close();
    plyFile.close();

    std::cout << "Conversion from PCD to PLY completed successfully." << std::endl;

    return 1;
}
//9
int PointTypeTrans::plyToObj(const std::string plyFilename, const std::string objFilename)
{
    if (strcmp(plyFilename.substr(plyFilename.size() - 4, 4).c_str(), ".ply") != 0)
        return 0;
    if (strcmp(objFilename.substr(objFilename.size() - 4, 4).c_str(), ".obj") != 0)
        return 0;
    std::ifstream plyFile(plyFilename);
    std::ofstream objFile(objFilename);

    if (!plyFile.is_open() || !objFile.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> vertices;
    std::string line;
    int vertexCount = 0;
    bool inHeader = true;

    // Process PLY header
    while (inHeader && std::getline(plyFile, line)) {
        std::istringstream iss(line);
        std::string token;
        iss >> token;

        if (token == "element") {
            iss >> token;
            if (token == "vertex") {
                iss >> vertexCount;
            }
        }
        else if (token == "end_header") {
            inHeader = false;
        }
    }

    // Process vertex data
    while (vertexCount > 0 && std::getline(plyFile, line)) {
        std::istringstream iss(line);
        Vertex vertex;
        iss >> vertex.x >> vertex.y >> vertex.z;
        vertices.push_back(vertex);
        --vertexCount;
    }

    // Write to OBJ file
    for (const Vertex& vertex : vertices) {
        objFile << "v " << vertex.x << " " << vertex.y << " " << vertex.z << std::endl;
    }

    plyFile.close();
    objFile.close();
    std::cout << "Conversion from PLY to OBJ completed successfully." << std::endl;

    return 1;
}

//10
int PointTypeTrans::objToPly(const std::string objFilename, const std::string plyFilename)
{
    if (strcmp(plyFilename.substr(plyFilename.size() - 4, 4).c_str(), ".ply") != 0)
        return 0;
    if (strcmp(objFilename.substr(objFilename.size() - 4, 4).c_str(), ".obj") != 0)
        return 0;
    std::ifstream objFile(objFilename);
    std::ofstream plyFile(plyFilename);

    if (!objFile.is_open() || !plyFile.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> vertices;
    std::string line;

    // Process OBJ file
    while (std::getline(objFile, line)) {
        if (line.empty() || line[0] == '#') continue;
        std::istringstream iss(line);
        std::string prefix;
        iss >> prefix;
        if (prefix == "v") {
            Vertex vertex;
            iss >> vertex.x >> vertex.y >> vertex.z;
            vertices.push_back(vertex);
        }
    }

    // Write PLY header
    writePLYHeader(plyFile, vertices.size());

    // Write vertex data to PLY file
    for (const Vertex& vertex : vertices) {
        plyFile << vertex.x << " " << vertex.y << " " << vertex.z << "\n";
    }

    objFile.close();
    plyFile.close();
    std::cout << "Conversion from OBJ to PLY completed successfully." << std::endl;

    return 1;
}

//11
int PointTypeTrans::pcdToObj(const std::string pcdFilename, const std::string objFilename)
{
    if (strcmp(pcdFilename.substr(pcdFilename.size() - 4, 4).c_str(), ".pcd") != 0)
        return 0;
    if (strcmp(objFilename.substr(objFilename.size() - 4, 4).c_str(), ".obj") != 0)
        return 0;
    std::ifstream pcdFile(pcdFilename);
    std::ofstream objFile(objFilename);

    if (!pcdFile.is_open() || !objFile.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> vertices;
    std::string line;
    int pointsCount = 0;
    bool isDataSection = false;

    // Read PCD header
    while (std::getline(pcdFile, line)) {
        std::stringstream ss(line);
        std::string keyword;
        ss >> keyword;

        if (keyword == "POINTS") {
            ss >> pointsCount;
        }
        else if (keyword == "DATA" && line.find("ascii") != std::string::npos) {
            isDataSection = true;
            break; // Stop reading header lines after "DATA"
        }
    }

    // Read PCD data points
    while (isDataSection && pointsCount > 0 && std::getline(pcdFile, line)) {
        std::stringstream ss(line);
        Vertex vertex;
        ss >> vertex.x >> vertex.y >> vertex.z;
        vertices.push_back(vertex);
        --pointsCount;
    }

    // Write vertices as points to OBJ file
    for (const Vertex& vertex : vertices) {
        objFile << "v " << vertex.x << " " << vertex.y << " " << vertex.z << "\n";
    }

    pcdFile.close();
    objFile.close();

    std::cout << "Conversion from PCD to OBJ completed successfully." << std::endl;

    return 1;
}

//12
int PointTypeTrans::objToPcd(const std::string objFilename, const std::string pcdFilename)
{
    if (strcmp(pcdFilename.substr(pcdFilename.size() - 4, 4).c_str(), ".pcd") != 0)
        return 0;
    if (strcmp(objFilename.substr(objFilename.size() - 4, 4).c_str(), ".obj") != 0)
        return 0;
    std::ifstream objFile(objFilename);
    std::ofstream pcdFile(pcdFilename);

    if (!objFile.is_open() || !pcdFile.is_open()) {
        std::cerr << "Error opening files!" << std::endl;
        return 0;
    }

    std::vector<Vertex> vertices;
    std::string line;

    // Read OBJ vertex data
    while (std::getline(objFile, line)) {
        if (line.empty() || line[0] == '#') continue;
        std::stringstream ss(line);
        std::string prefix;
        ss >> prefix;

        if (prefix == "v") {
            Vertex vertex;
            ss >> vertex.x >> vertex.y >> vertex.z;
            vertices.push_back(vertex);
        }
    }

    // Write PCD header
    writePCDHeader(pcdFile, vertices.size());

    // Write vertices to PCD file
    for (const Vertex& vertex : vertices) {
        pcdFile << vertex.x << " " << vertex.y << " " << vertex.z << "\n";
    }

    objFile.close();
    pcdFile.close();

    std::cout << "Conversion from OBJ to PCD completed successfully." << std::endl;

    return 1;
}

void PointTypeTrans::view_display(PloudCloud::Ptr cloud_target, PloudCloud::Ptr result)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("PCL Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
     对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(cloud_target, 255, 0, 0);//红色
    viewer->addPointCloud<pcl::PointXYZ>(cloud_target, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    // 对配准点云着色可视化 (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>output_color(result, 0, 255, 0);//绿色
    viewer->addPointCloud<pcl::PointXYZ>(result, output_color, "output_color");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "output_color");


    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    }
}

void PointTypeTrans::format_selection(std::string &suffix, int flag, std::string& file)
{
    std::string filef = file;
    int pos = filef.find(suffix);
    switch (flag)
    {
    case 1:
    {
        file = filef.substr(0, pos) + ".ply";
        int res = pcdToPly(filef, file);
        suffix = ".ply";
        if (res)
        {
            std::cout << "A pcd file was successfully imported" << std::endl;
        }
        break;
    }
        
    case 2:
    {
        file = filef.substr(0, pos) + ".ply";
        int res = txtToPly(filef, file);
        suffix = ".ply";
        if (res)
        {
            std::cout << "A pcd file was successfully imported" << std::endl;
        }
        break;
    }
        
    }

}
PointTypeTrans::~PointTypeTrans()
{

}

main.cpp，在main.cpp增加了调用，同时取消了从命令行读取参数，改成固定的参数，方便测试，当然你想用也可以，直接把if(argc > 5)这段代码解开注释就行

/********************************************************************
Main Function for point cloud registration with Go-ICP Algorithm
Last modified: Feb 13, 2014

"Go-ICP: Solving 3D Registration Efficiently and Globally Optimally"
Jiaolong Yang, Hongdong Li, Yunde Jia
International Conference on Computer Vision (ICCV), 2013

Copyright (C) 2013 Jiaolong Yang (BIT and ANU)

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*********************************************************************/

#include <time.h>
#include <iostream>
#include <fstream>
#include "pointTypeTrans.h"
#include "jly_goicp.h"
#include "ConfigMap.hpp"
#include <io.h>
#include <direct.h>
using namespace std;

#define DEFAULT_OUTPUT_FNAME "./data/output.txt"
#define DEFAULT_CONFIG_FNAME "./data/config.txt"
#define DEFAULT_MODEL_FNAME "./data/model_bunny.txt"
#define DEFAULT_DATA_FNAME "./data/data_bunny.txt"

void parseInput(int argc, char** argv, string& modelFName, string& dataFName, int& NdDownsampled, string& configFName, string& outputFName);
void readConfig(string FName, GoICP& goicp);
int loadPointCloud(string FName, int& N, POINT3D** p);

int main(int argc, char** argv)
{
	string data_file = "./data";
	if (_access(data_file.c_str(), 0) == -1)	//如果文件夹不存在
		_mkdir(data_file.c_str());
	int Nm, Nd, NdDownsampled;
	clock_t  clockBegin, clockEnd;
	string modelFName, dataFName, configFName, outputFname;
	POINT3D* pModel, * pData;
	GoICP goicp;

	parseInput(argc, argv, modelFName, dataFName, NdDownsampled, configFName, outputFname);
	readConfig(configFName, goicp);

	// Load model and data point clouds
	loadPointCloud(modelFName, Nm, &pModel);
	loadPointCloud(dataFName, Nd, &pData);

	goicp.pModel = pModel;
	goicp.Nm = Nm;
	goicp.pData = pData;
	goicp.Nd = Nd;

	// Build Distance Transform
	cout << "Building Distance Transform..." << flush;
	clockBegin = clock();
	goicp.BuildDT();
	clockEnd = clock();
	cout << (double)(clockEnd - clockBegin) / CLOCKS_PER_SEC << "s (CPU)" << endl;

	// Run GO-ICP
	if (NdDownsampled > 0)
	{
		goicp.Nd = NdDownsampled; // Only use first NdDownsampled data points (assumes data points are randomly ordered)
	}
	cout << "Model ID: " << modelFName << " (" << goicp.Nm << "), Data ID: " << dataFName << " (" << goicp.Nd << ")" << endl;
	cout << "Registering..." << endl;
	clockBegin = clock();
	goicp.Register();
	clockEnd = clock();
	double time = (double)(clockEnd - clockBegin) / CLOCKS_PER_SEC;
	cout << "Optimal Rotation Matrix:" << endl;
	cout << goicp.optR << endl;
	cout << "Optimal Translation Vector:" << endl;
	cout << goicp.optT << endl;
	cout << "Finished in " << time << endl;

	ofstream ofile;
	ofile.open(outputFname.c_str(), ofstream::out);
	ofile << time << endl;
	ofile << goicp.optR << endl;
	ofile << goicp.optT << endl;
	ofile.close();

	delete(pModel);
	delete(pData);









	//可视化
	std::ifstream transformFile(DEFAULT_OUTPUT_FNAME);
	if (!transformFile.is_open()) {
		std::cerr << "无法打开变换文件。" << std::endl;
		return 1;
	}

	float value;

	// 跳过第一行
	std::string line;
	std::getline(transformFile, line);

	// 读取旋转矩阵
	Eigen::Matrix3f rotationMatrix;
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			transformFile >> rotationMatrix(i, j);
		}
	}

	// 读取平移向量
	Eigen::Vector3f translationVector;
	for (int i = 0; i < 3; ++i) {
		transformFile >> translationVector(i);
	}

	transformFile.close();



	// 构造齐次变换矩阵
	Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
	transform.block<3, 3>(0, 0) = rotationMatrix; // 逆转置，因为Eigen默认是行主序的
	transform.block<3, 1>(0, 3) = translationVector;

	// 变换点云
	PloudCloud::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
	PloudCloud::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	PloudCloud::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);
	

	PointTypeTrans v;
	//std::string resultF = data_file + "/Result" + ".ply";
	std::string sourceF = data_file + "/Source" + ".ply";
	std::string targetF = data_file + "/Target" + ".ply";
	pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
	
	//int res1 = v.txtToPly(DEFAULT_OUTPUT_FNAME, resultF);
	int res2 = v.txtToPly(DEFAULT_MODEL_FNAME, targetF);
	int res3 = v.txtToPly(DEFAULT_DATA_FNAME, sourceF);
	bool trans_or_no =  res2 && res3;
	if (!trans_or_no)
	{
		cout << "falled!!!" << endl;
	}
	if (pcl::io::loadPLYFile<pcl::PointXYZ>(sourceF, *cloud) == -1)
	{
		PCL_ERROR("加载点云失败\n");
	}
	if (pcl::io::loadPLYFile<pcl::PointXYZ>(targetF, *cloud_target) == -1)
	{
		PCL_ERROR("加载点云失败\n");
	}
	pcl::transformPointCloud(*cloud, *result, transform);
	v.view_display(cloud_target, cloud);
	v.view_display(cloud_target, result);
	return 0;
}

void parseInput(int argc, char** argv, string& modelFName, string& dataFName, int& NdDownsampled, string& configFName, string& outputFName)
{
	// Set default values
	modelFName = DEFAULT_MODEL_FNAME;
	dataFName = DEFAULT_DATA_FNAME;
	configFName = DEFAULT_CONFIG_FNAME;
	outputFName = DEFAULT_OUTPUT_FNAME;
	//NdDownsampled = 0; // No downsampling
	NdDownsampled = 500; // No downsampling


	/*if (argc > 5)
	{
		outputFName = argv[5];
	}
	if (argc > 4)
	{
		configFName = argv[4];
	}
	if (argc > 3)
	{
		NdDownsampled = atoi(argv[3]);
	}
	if (argc > 2)
	{
		dataFName = argv[2];
	}
	if (argc > 1)
	{
		modelFName = argv[1];
	}*/

	cout << "INPUT:" << endl;
	cout << "(modelFName)->(" << modelFName << ")" << endl;
	cout << "(dataFName)->(" << dataFName << ")" << endl;
	cout << "(NdDownsampled)->(" << NdDownsampled << ")" << endl;
	cout << "(configFName)->(" << configFName << ")" << endl;
	cout << "(outputFName)->(" << outputFName << ")" << endl;
	cout << endl;
}

void readConfig(string FName, GoICP& goicp)
{
	// Open and parse the associated config file
	ConfigMap config(FName.c_str());

	goicp.MSEThresh = config.getF("MSEThresh");
	goicp.initNodeRot.a = config.getF("rotMinX");
	goicp.initNodeRot.b = config.getF("rotMinY");
	goicp.initNodeRot.c = config.getF("rotMinZ");
	goicp.initNodeRot.w = config.getF("rotWidth");
	goicp.initNodeTrans.x = config.getF("transMinX");
	goicp.initNodeTrans.y = config.getF("transMinY");
	goicp.initNodeTrans.z = config.getF("transMinZ");
	goicp.initNodeTrans.w = config.getF("transWidth");
	goicp.trimFraction = config.getF("trimFraction");
	// If < 0.1% trimming specified, do no trimming
	if (goicp.trimFraction < 0.001)
	{
		goicp.doTrim = false;
	}
	goicp.dt.SIZE = config.getI("distTransSize");
	goicp.dt.expandFactor = config.getF("distTransExpandFactor");

	cout << "CONFIG:" << endl;
	config.print();
	//cout << "(doTrim)->(" << goicp.doTrim << ")" << endl;
	cout << endl;
}

int loadPointCloud(string FName, int& N, POINT3D** p)
{
	int i;
	ifstream ifile;

	ifile.open(FName.c_str(), ifstream::in);
	if (!ifile.is_open())
	{
		cout << "Unable to open point file '" << FName << "'" << endl;
		exit(-1);
	}
	ifile >> N; // First line has number of points to follow
	*p = (POINT3D*)malloc(sizeof(POINT3D) * N);
	for (i = 0; i < N; i++)
	{
		ifile >> (*p)[i].x >> (*p)[i].y >> (*p)[i].z;
	}

	ifile.close();

	return 0;
}

使用：

main.cpp下图第一行是输出文件路径，这份文件并不是配准后的点云文件，里面是旋转矩阵和位移矩阵；第二行是配准需要的参数文件路径，里面都是GO-ICP配准所需参数，如果配准效果不满意，可以在这个文件里面调整参数；第三第四行分别是目标点云文件路径和输入点云文件路径。

结果：

这是我修改程序之后的可视化

配准前

配准后

这是用源码自带的matlab程序实现的可视化，直接运行demo文件夹的demo.m文件即可，不过我是把demo文件放到自己的项目文件夹里了，并改名为data文件夹，所以我之前的文件路径都是./data开头的。

配准前

配准后

其实我的pointTypeTrans.h和pointTypeTrans.cpp还包含了几种点云文件转换代码，有兴趣的话可以试试利用代码把ply\pcd\obj点云文件转换成txt再进行配准，不过要注意的是我的点云文件转换只包含顶点信息，如果一份点云你只关注顶点信息，你可以使用我的代码进行文件转换，如果你想要更多信息，那么这份代码可能满足不了你的要求。