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再进行配准，不过要注意的是我的点云文件转换只包含顶点信息，如果一份点云你只关注顶点信息，你可以使用我的代码进行文件转换，如果你想要更多信息，那么这份代码可能满足不了你的要求。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mfbz.cn/a/408159.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

计算机组成原理（13）-----硬件多线程

计算机组成原理（13）-----硬件多线程

目录 1.细粒度多线程 2.粗粒度多线程 3.同时多线程&#xff08;SMT&#xff09; 在不支持硬件多线程的处理器中&#xff0c;若要进行线程的切换&#xff0c;就需要保存和恢复线程的运行环境&#xff08;否则会出现数据覆盖引起的错误&#xff09;。 但在支持硬件多线程的处…
阅读更多...
五篇保姆级分类诊断教程，数据特征提取+优化算法+机器学习

五篇保姆级分类诊断教程，数据特征提取+优化算法+机器学习

今天水一期&#xff0c;总结一下以前写过的几篇保姆级故障诊断。学会这几篇&#xff0c;机器学习的故障诊断你就基本合格了&#xff01; 本期内容&#xff1a;基于SABO-VMD-CNN-SVM的分类诊断。 依旧是采用经典的西储大学轴承数据。基本流程如下&#xff1a; 首先是以最小包络熵…
阅读更多...
Github代码仓库SSH配置流程

Github代码仓库SSH配置流程

作者&#xff1a; Herman Ye Auromix 测试环境&#xff1a; Ubuntu20.04 更新日期&#xff1a; 2024/02/21 注1&#xff1a; Auromix 是一个机器人爱好者开源组织。 注2&#xff1a; 由于笔者水平有限&#xff0c;以下内容可能存在事实性错误。 相关背景 在为Github代码仓库配…
阅读更多...
RocketMQ快速实战以及集群架构原理详解

RocketMQ快速实战以及集群架构原理详解

RocketMQ快速实战以及集群架构原理详解 组成部分 启动Rocket服务之前要先启动NameServer NameServer 提供轻量级Broker路由服务&#xff0c;主要是提供服务注册 Broker 实际处理消息存储、转发等服务的核心组件 Producer 消息生产者集群&#xff0c;通常为业务系统中的一个功…
阅读更多...
基础光学系列：（四）机器视觉中的光圈、焦距与景深调整技巧

基础光学系列：（四）机器视觉中的光圈、焦距与景深调整技巧

控制景深和成像质量是摄影和机器视觉领域的关键技能。通过调整光圈、焦距、和感光元件&#xff08;如数码相机的图像传感器&#xff09;的位置&#xff0c;可以精细地控制图像的外观。下面是如何通过这些参数来控制景深和提高成像质量的一些建议&#xff1a; 调整光圈来控制景…
阅读更多...
学生管理系统简易版（C语言）

学生管理系统简易版（C语言）

简介&#xff1a; 有登录功能&#xff0c;这个功能是利用文本文件来保存用户名和密码彩色的菜单页面多种功能较好的排版含有文件夹图标 目录展示&#xff1a; 页面展示&#xff1a; 下载链接&#xff1a;https://download.csdn.net/download/liyankang/88873436 度盘链接&…
阅读更多...
BUU [GWCTF 2019]mypassword

BUU [GWCTF 2019]mypassword

BUU [GWCTF 2019]mypassword 开题&#xff0c;是个登录界面&#xff0c;也有注册功能 首先我们注册个号看看。然后登录 提示不是SQL注入 有反馈界面&#xff0c;反馈应该管理员会看&#xff0c;可能存在XSS。 查看源码&#xff0c;注释给出了后端源码&#xff0c;对XSS进行了…
阅读更多...
GEE数据集——GLANCE 全球土地覆被训练数据集

GEE数据集——GLANCE 全球土地覆被训练数据集

GLANCE 全球土地覆被训练数据集 GLanCE 培训数据集向公众开放&#xff0c;专为区域到全球土地覆被和土地覆被变化分析而设计。该数据集的中等空间分辨率为 30 米&#xff0c;时间跨度为 1984 年至 2020 年&#xff0c;在地理和光谱上代表了全球所有生态区域。每个训练单元提供多…
阅读更多...
极狐GitLab 使用指南：如何使用极狐GitLab 进行第一次 git commit

极狐GitLab 使用指南：如何使用极狐GitLab 进行第一次 git commit

GitLab 是一个全球知名的一体化 DevOps 平台&#xff0c;很多人都通过私有化部署 GitLab 来进行源代码托管。极狐GitLab 是 GitLab 在中国的发行版&#xff0c;专门为中国程序员服务。可以一键式部署极狐GitLab。 进行第一次 Git 提交 本教程包含一些关于 Git 的工作原理&…
阅读更多...
Escalate_Linux-环境变量劫持提权(5)

Escalate_Linux-环境变量劫持提权(5)

环境变量劫持提权 在Shll输入命令时&#xff0c;Shel会按PAH环境变量中的路径依次搜索命令&#xff0c;若是存在同名的命令&#xff0c;则执行最先找到的&#xff0c;若是PATH中加入了当前目录&#xff0c;也就是“”这个符号&#xff0c;则可能会被黑客利用&#xff0c;例如在…
阅读更多...
RM电控讲义【HAL库篇】（二）

RM电控讲义【HAL库篇】（二）

8080并口模式是一种常见的计算机接口模式&#xff0c;主要用于LCD&#xff08;液晶显示屏&#xff09;模块。 在8080并口模式中&#xff0c;通信端口包括多种信号线&#xff0c;用于实现数据的读写和控制功能。主要的信号线包括&#xff1a; CS&#xff08;片选信号&#xff…
阅读更多...
ArcgisForJS如何将ArcGIS Server发布的点要素渲染为热力图？

ArcgisForJS如何将ArcGIS Server发布的点要素渲染为热力图？

文章目录 0.引言1.ArcGIS创建点要素2.ArcGIS Server发布点要素3.ArcgisForJS将ArcGIS创建的点要素渲染为热力图 0.引言 ArcGIS For JS 是一个强大的地理信息系统&#xff08;GIS&#xff09;工具&#xff0c;它允许开发者使用 JavaScript 语言来创建各种 GIS 应用。ArcGIS Ser…
阅读更多...
Java——防止SQL注入的几种策略

Java——防止SQL注入的几种策略

一、什么是SQL注入 SQL注入&#xff08;SQL Injection&#xff09;是一种常见的网络安全漏洞&#xff0c;它允许攻击者通过操纵应用程序的输入来执行恶意的SQL查询。这种漏洞发生在应用程序没有正确验证、过滤或转义用户提供的输入数据时。攻击者可以利用这个漏洞来执行未经授…
阅读更多...
Flask基础学习3

Flask基础学习3

参考视频&#xff1a;41-【实战】答案列表的渲染_哔哩哔哩_bilibili flask 实现发送短信功能 pip install flask-mail # 安装依赖 我这里用登录的网易邮箱获取的授权码&#xff08;登录QQ邮箱的授权码总是断开收不到邮件&#xff09;&#xff0c; # config # config mail MAI…
阅读更多...
Python入门必学：单引号、双引号与三引号的差异与应用

Python入门必学：单引号、双引号与三引号的差异与应用

Python入门必学&#xff1a;单引号、双引号与三引号的差异与应用 &#x1f308; 个人主页&#xff1a;高斯小哥 &#x1f525; 高质量专栏&#xff1a;Matplotlib之旅&#xff1a;零基础精通数据可视化、Python基础【高质量合集】、PyTorch零基础入门教程 &#x1f448; 希望得…
阅读更多...
HC595级联原理及实例 - STM32

HC595级联原理及实例 - STM32

74HC595的最重要的功能就是&#xff1a;串行输入&#xff0c;并行输出。其次&#xff0c;74HC595里面有2个8位寄存器&#xff1a;移位寄存器、存储寄存器。74HC595的数据来源只有一个口&#xff0c;一次只能输入一个位&#xff0c;那么连续输入8次&#xff0c;就可以积攒为一个…
阅读更多...
MiKTeX安装后，Latex编译后PDF无法预览，是灰色的

MiKTeX安装后，Latex编译后PDF无法预览，是灰色的

解决方式删掉编译器就可以&#xff0c; 即删掉MiKTeX MiKTeX安装后会将编译器默认修改为MiKTeX&#xff0c;这个时候会显示报错&#xff0c;简单粗暴的方式是删掉MiKTeX软件
阅读更多...
python统计分析——单因素方差分析

python统计分析——单因素方差分析

参考资料&#xff1a;用python动手学统计学 方差分析&#xff1a;analysis of variance&#xff0c;缩写为ANOVA 1、背景知识 1.1 要使用方差分析&#xff0c;数据的总体必须服从正态分布&#xff0c;而且各个水平内部的方差必须相等。 1.2 反复检验导致显著性结果更易出现…
阅读更多...
相机选型介绍

相机选型介绍

摄影测量中&#xff0c;相机是非常重要的角色&#xff0c;合适的相机产出合适的图像&#xff0c;得到合适的重建精度&#xff0c;这是相机的重要性。 您也许第一反应是&#xff0c;摄影测量所需的理想相机&#xff0c;是有着超高分辨率的相机&#xff0c;但事实可能并非如此&a…
阅读更多...
太阳能光伏模型的参数确定及模型应用介绍

太阳能光伏模型的参数确定及模型应用介绍

一、太阳能光伏模型介绍 ​ 太阳能通过光伏&#xff08;PV&#xff09;发电系统转化为电能。通过使用新材料技术&#xff0c;一直致力于提高光伏系统中太阳能电池的功率转换效率。基于钙钛矿太阳能电池的冠军器件具有24.8%的认证功率转换效率&#xff0c;仍有很大的改进空间。…
阅读更多...
最新文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发 尧图建网站