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 headerwritePLYHeader(ply_file, num_points);// Write the point datafloat x, y, z;for (int i = 0; i < num_points; ++i) {txt_file >> x >> y >> z;ply_file << x << " " << y << " " << z << "\n";}// Clean uptxt_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 elementswhile (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 filetxt_file << num_points << std::endl;// Read and write the point datafloat x, y, z;for (int i = 0; i < num_points; ++i) {ply_file >> x >> y >> z;txt_file << x << " " << y << " " << z << std::endl;}// Clean upply_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 fileint num_points;txt_file >> num_points;// Write the PCD headerwritePCDHeader(pcd_file, num_points);// Read and write the point datafloat x, y, z;for (int i = 0; i < num_points; ++i) {txt_file >> x >> y >> z;pcd_file << x << " " << y << " " << z << "\n";}// Clean uptxt_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 headerwhile (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, exitif (!headerPassed) {std::cerr << "PCD header not found or improper format." << std::endl;return 0;}// Write the number of points to the TXT filetxt_file << num_points << "\n";// Read and write the point datafloat x, y, z;while (pcd_file >> x >> y >> z) {txt_file << x << " " << y << " " << z << "\n";}// Clean uppcd_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 providedint num_points;txt_file >> num_points;// Write vertices to the OBJ filefloat x, y, z;while (txt_file >> x >> y >> z) {obj_file << "v " << x << " " << y << " " << z << "\n";}// Clean uptxt_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 filewhile (std::getline(obj_file, line)) {// Skip empty lines and commentsif (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 verticestxt_file << vertices.size() << std::endl;// Then write all verticesfor (const auto& vertex : vertices) {txt_file << vertex.x << " " << vertex.y << " " << vertex.z << std::endl;}// Clean upobj_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 linescontinue;}else {// Read point dataVertex point;if (ss >> point.x >> point.y >> point.z) {points.push_back(point);}}}// Write PCD headerwritePCDHeader(pcdFile, vertexCount);// Write point data to PCD filefor (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 filewhile (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 headerwritePLYHeader(plyFile, pointCount);// Write points to PLY filefor (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 headerwhile (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 datawhile (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 filefor (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 filewhile (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 headerwritePLYHeader(plyFile, vertices.size());// Write vertex data to PLY filefor (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 headerwhile (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 pointswhile (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 filefor (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 datawhile (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 headerwritePCDHeader(pcdFile, vertices.size());// Write vertices to PCD filefor (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), 2013Copyright (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 cloudsloadPointCloud(modelFName, Nm, &pModel);loadPointCloud(dataFName, Nd, &pData);goicp.pModel = pModel;goicp.Nm = Nm;goicp.pData = pData;goicp.Nd = Nd;// Build Distance Transformcout << "Building Distance Transform..." << flush;clockBegin = clock();goicp.BuildDT();clockEnd = clock();cout << (double)(clockEnd - clockBegin) / CLOCKS_PER_SEC << "s (CPU)" << endl;// Run GO-ICPif (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 valuesmodelFName = DEFAULT_MODEL_FNAME;dataFName = DEFAULT_DATA_FNAME;configFName = DEFAULT_CONFIG_FNAME;outputFName = DEFAULT_OUTPUT_FNAME;//NdDownsampled = 0; // No downsamplingNdDownsampled = 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 fileConfigMap 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 trimmingif (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.mzph.cn/news/700722.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

更简单地介绍 CUDA

更简单地介绍 CUDA

这篇文章是对 CUDA 的超级简单介绍&#xff0c;CUDA 是 NVIDIA 流行的并行计算平台和编程模型。我之前在2013年写过一篇文章《CUDA简单介绍》&#xff0c;多年来一直很受欢迎。但 CUDA 编程变得更加容易&#xff0c;GPU 也变得更快&#xff0c;所以是时候进行更新&#xff08;甚…
阅读更多...
SQL-Labs46关order by注入姿势

SQL-Labs46关order by注入姿势

君衍. 四十六关 ORDER BY数字型注入1、源码分析2、rand()盲注3、if语句盲注4、时间盲注5、报错注入6、Limit注入7、盲注脚本 四十六关 ORDER BY数字型注入 请求方式注入类型拼接方式GET报错、布尔盲注、延时盲注ORDER BY $id 我们直接可以从界面中得知传参的参数为SORT&#x…
阅读更多...
Yolo v9 “Silence”模块结构及作用!

Yolo v9 “Silence”模块结构及作用!

论文链接&#xff1a;&#x1f47f; YOLOv9: Learning What You Want to Learn Using Programmable Gradient Information 代码链接&#xff1a;&#x1f47f; https://github.com/WongKinYiu/yolov9/tree/main Silence代码 class Silence(nn.Module):def __init__(self):supe…
阅读更多...
vue2和vue3对比（语法层面）

vue2和vue3对比（语法层面）

阅读文章你将收获&#xff1a; 1 了解不使用组件化工具时&#xff0c;vue在html是如何使用的 2 知道vue2的生命周期函数有哪些 3 知道如何在组件化开发中使用vue 4 大致了解了vue2和vue3在使用上什么不同 最后&#xff1a;vue2和vue3除了下面我列出的有差异化的地方&…
阅读更多...
day41打卡

day41打卡

day41打卡 46. 携带研究材料&#xff08;第六期模拟笔试&#xff09; 状态表示 ​ 二维&#xff1a;dp[i] [j] 表示从下标为[0-i]的物品里任意取&#xff0c;放进容量为j的背包&#xff0c;价值总和最大是多少。 一维&#xff1a; ​ dp[j]表示&#xff1a;容量为j的背包&a…
阅读更多...
模型 HBG(品牌增长)

模型 HBG(品牌增长)

系列文章 分享 模型&#xff0c;了解更多&#x1f449; 模型_总纲目录。品牌增长法。 1 HBG(品牌增长)模型的应用 1.1 江小白使用HBG模型提高品牌知名度和销售额 选择受众市场&#xff1a;江小白的目标客户是年轻人&#xff0c;他们喜欢简单、时尚的产品。因此&#xff0c;江…
阅读更多...
数据结构D4作业

数据结构D4作业

1.实现单向循环链表的功能 loop.c #include "loop.h" loop_p create_loop() { loop_p H(loop_p)malloc(sizeof(loop)); if(HNULL) { printf("创建失败\n"); return NULL; } H->len0; H->nextH; ret…
阅读更多...
基于ElementUI封装省市区四级联动下拉选择

基于ElementUI封装省市区四级联动下拉选择

基于ElementUI封装的省市区下拉级联选择 效果 数据 最新省市区JSON数据获取&#xff1a;https://xiangyuecn.github.io/AreaCity-JsSpider-StatsGov/ 参数说明 参数说明inputNumShow下拉框的数量&#xff0c;最多4个defaultAddress默认显示省市区 例&#xff1a;[‘安徽’, …
阅读更多...
【C++初阶】--类和对象（下）

【C++初阶】--类和对象（下）

目录 一.const成员 1.权限放大问题 2.权限的缩小 二.再谈构造函数 1.构造函数体赋值 2.初始化列表 (1)概念 (2)使用 ①在对象实例化过程中&#xff0c;成员变量先依次进行初始化 ②再进行函数体内二次赋值 3.explicit关键字 (1)C为什么要存在自动隐式类型转换…
阅读更多...
算法打卡day1|数组篇|Leetcode 704.二分查找、27.移除元素

算法打卡day1|数组篇|Leetcode 704.二分查找、27.移除元素

数组理论基础 数组是存放在连续内存空间上的相同类型数据的集合&#xff0c;可以方便的通过下标索引的方式获取到下标下对应的数据。 1.数组下标都是从0开始的。 2.数组内存空间的地址是连续的。 正是因为数组的在内存空间的地址是连续的&#xff0c;所以我们在删除或者增添…
阅读更多...
【深度学习笔记】3_5 图像分类数据集fashion-mnist

【深度学习笔记】3_5 图像分类数据集fashion-mnist

注&#xff1a;本文为《动手学深度学习》开源内容&#xff0c;仅为个人学习记录&#xff0c;无抄袭搬运意图 3.5 图像分类数据集&#xff08;Fashion-MNIST&#xff09; 在介绍softmax回归的实现前我们先引入一个多类图像分类数据集。它将在后面的章节中被多次使用&#xff0c…
阅读更多...
《Docker 简易速速上手小册》第1章 Docker 基础入门（2024 最新版）

《Docker 简易速速上手小册》第1章 Docker 基础入门（2024 最新版）

文章目录 1.1 Docker 简介与历史1.1.1 Docker 基础知识1.1.2 重点案例&#xff1a;Python Web 应用的 Docker 化1.1.3 拓展案例 1&#xff1a;使用 Docker 进行 Python 数据分析1.1.4 拓展案例 2&#xff1a;Docker 中的 Python 机器学习环境 1.2 安装与配置 Docker1.2.1 重点基…
阅读更多...
消息队列-RabbitMQ：发布确认—发布确认逻辑和发布确认的策略

消息队列-RabbitMQ：发布确认—发布确认逻辑和发布确认的策略

九、发布确认 1、发布确认逻辑 生产者将信道设置成 confirm 模式&#xff0c;一旦信道进入 confirm 模式&#xff0c;所有在该信道上面发布的消息都将会被指派一个唯一的 ID (从 1 开始)&#xff0c;一旦消息被投递到所有匹配的队列之后&#xff0c;broker 就会发送一个确认给…
阅读更多...
Python基础教程——17个工作必备的Python自动化代码

Python基础教程——17个工作必备的Python自动化代码

您是否厌倦了在日常工作中做那些重复性的任务&#xff1f;简单但多功能的Python脚本可以解决您的问题。 引言 Python是一种流行的编程语言&#xff0c;以其简单性和可读性而闻名。因其能够提供大量的库和模块&#xff0c;它成为了自动化各种任务的绝佳选择。让我们进入自动化…
阅读更多...
K8s环境搭建

K8s环境搭建

一、基础环境准备 VMware虚拟机&#xff0c;安装三台CentOS&#xff0c;网络环境选择NAT模式&#xff0c;推荐配置如下&#xff08;具体安装步骤省略&#xff0c;网上很多虚拟机安装CentOS7的教程&#xff09; 二、网络环境说明 使用NAT模式&#xff0c;我的IP分别是&#xf…
阅读更多...
Promise相关理解记录

Promise相关理解记录

一、Promise基础定义相关 Promise是一个构造函数&#xff0c;调用时需要使用new关键字 Promise是解决回调地狱的一种异步解决方式 Promise有三个状态&#xff1a;pending(进行中)、fulfilled(成功)、rejected(失败) Promise的状态只会从 pending→fulfilled 或者 pending→…
阅读更多...
300分钟吃透分布式缓存-13讲：如何完整学习MC协议及优化client访问？

300分钟吃透分布式缓存-13讲：如何完整学习MC协议及优化client访问？

协议分析 异常错误响应 接下来&#xff0c;我们来完整学习 Mc 协议。在学习 Mc 协议之前&#xff0c;首先来看看 Mc 处理协议指令&#xff0c;如果发现异常&#xff0c;如何进行异常错误响应的。Mc 在处理所有 client 端指令时&#xff0c;如果遇到错误&#xff0c;就会返回 …
阅读更多...
信号系统之线性图像处理

信号系统之线性图像处理

1 卷积 图像卷积的工作原理与一维卷积相同。例如&#xff0c;图像可以被视为脉冲的总和&#xff0c;即缩放和移位的delta函数。同样&#xff0c;线性系统的特征在于它们如何响应脉冲。也就是说&#xff0c;通过它们的脉冲响应。系统的输出图像等于输入图像与系统脉冲响应的卷积…
阅读更多...
pclpy 半径滤波实现

pclpy 半径滤波实现

pclpy 半径滤波实现 一、算法原理背景 二、代码1.pclpy 官方给与RadiusOutlierRemoval2.手写的半径滤波&#xff08;速度太慢了&#xff0c;用官方的吧&#xff09; 三、结果1.左边为原始点云&#xff0c;右边为半径滤波后点云 四、相关数据 一、算法原理 背景 RadiusOutlier…
阅读更多...
Linux——进程概念

Linux——进程概念

目录 冯诺依曼体系结构 操作系统 管理 系统调用和库函数 进程的概念 进程控制块——PCB 查看进程 通过系统调用获取进程标示符 通过系统调用创建进程 进程状态 运行状态-R ​编辑 浅度睡眠状态-S 深度睡眠状态-D 暂停状态-T 死亡状态-X 僵尸状态-Z 僵尸进程…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023