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，一经查实，立即删除！

相关文章

2024年这些半导体行业告诉我，投资迎来高光

2024年这些半导体行业告诉我，投资迎来高光

在下行的周期内&#xff0c;砍单、裁员、破产注销等关键词贯穿了2023一年。 我们挥挥手&#xff0c;告别掉过去&#xff0c;怀着对未来的美好希望奔向2024年。未来虽然代表着未知&#xff0c;也有着各种不确定性&#xff0c;但在产业的发展的过程中&#xff0c;行业趋势我们是…
阅读更多...
更简单地介绍 CUDA

更简单地介绍 CUDA

这篇文章是对 CUDA 的超级简单介绍&#xff0c;CUDA 是 NVIDIA 流行的并行计算平台和编程模型。我之前在2013年写过一篇文章《CUDA简单介绍》&#xff0c;多年来一直很受欢迎。但 CUDA 编程变得更加容易&#xff0c;GPU 也变得更快&#xff0c;所以是时候进行更新&#xff08;甚…
阅读更多...
【Vue实现参数传递：查询参数 vs. 动态路由】

【Vue实现参数传递：查询参数 vs. 动态路由】

文章目录 查询参数传递1. 什么是查询参数&#xff1f;2. 在Vue中使用查询参数步骤 1&#xff1a;在路由配置中定义查询参数步骤 2&#xff1a;在组件中使用查询参数步骤 3&#xff1a;在页面中生成链接 3. 查询参数传递的优势 动态路由传递1. 什么是动态路由&#xff1f;2. 在V…
阅读更多...
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…
阅读更多...
Linux内核处理并发与竞争的一种方法：信号量

Linux内核处理并发与竞争的一种方法：信号量

一. 简介 本文来学习Linux内核处理并发与竞争的一种方法：信号量。 本文主要对Linux内核提供的信号量进行简单的介绍。 二. Linux内核处理并发与竞争的一种方法：信号量 1. 信号量简介 大家如果有学习过 FreeRTOS 或者 UCOS 的话就应该对信号量很熟悉，因为信号量是同步…
阅读更多...
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除了下面我列出的有差异化的地方&…
阅读更多...
高防服务器的价格受到哪些因素的影响？

高防服务器的价格受到哪些因素的影响？

高防服务器可以帮助网站拒绝服务攻击&#xff0c;能够进行定时扫描网络节点&#xff0c;并且查找可能会存在的安全漏洞的服务器类型&#xff0c;对于经常遭受到DDOS攻击的大型游戏网络企业会选择高防服务器。 那面对价格多样的高防服务器我们应该怎样进行选择&#xff0c;高防服…
阅读更多...
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;江…
阅读更多...
面试前端性能优化八股文十问十答第三期

面试前端性能优化八股文十问十答第三期

面试前端性能优化八股文十问十答第三期 作者&#xff1a;程序员小白条&#xff0c;个人博客 相信看了本文后&#xff0c;对你的面试是有一定帮助的&#xff01;关注专栏后就能收到持续更新&#xff01; ⭐点赞⭐收藏⭐不迷路&#xff01;⭐ 1&#xff09;如何⽤webpack来优化…
阅读更多...
数据结构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…
阅读更多...
一文彻底搞懂SQL优化

一文彻底搞懂SQL优化

文章目录 1. 索引优化2. WHERE 子句优化3. JOIN 优化4. 查询结果优化5. 子查询优化6. 数据库统计信息和缓存优化7. 事务优化8. 数据库配置优化9. 使用适当的数据类型10. 监控和调优 SQL(Structured Query Language&#xff09;优化是指对 SQL 查询语句进行调整和改进&#xff0…
阅读更多...
关于 Reflect 的笔记

关于 Reflect 的笔记

背景&#xff1a;Reflect 为了操作对象而提供的新Api 和 Proxy对象一样 特点 将object 对象的一些明显属于语言内部的方法&#xff0c;放到Reflect 上处理&#xff1b;修改某些object返回的异常结果&#xff0c;让其变得更合理&#xff1b;让object操作都变成函数行为&#xf…
阅读更多...
基于ElementUI封装省市区四级联动下拉选择

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

基于ElementUI封装的省市区下拉级联选择 效果 数据 最新省市区JSON数据获取&#xff1a;https://xiangyuecn.github.io/AreaCity-JsSpider-StatsGov/ 参数说明 参数说明inputNumShow下拉框的数量&#xff0c;最多4个defaultAddress默认显示省市区 例&#xff1a;[‘安徽’, …
阅读更多...
按形如 a*sqrt(b) 的格式输出一个非负整数的平方根

按形如 a*sqrt(b) 的格式输出一个非负整数的平方根

【题目描述】 输入一个非负整数 x&#xff0c;若能完全开平方根&#xff0c;则输出其对应的整数平方根值。 否则&#xff0c;按形如 a*sqrt(b) 的格式输出其平方根值&#xff08;a 与 b 均为整数&#xff0c;且 a≠1&#xff0c;b≠1&#xff09;。【输入输出】 典型的输入输出…
阅读更多...
【C++初阶】--类和对象（下）

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

目录 一.const成员 1.权限放大问题 2.权限的缩小 二.再谈构造函数 1.构造函数体赋值 2.初始化列表 (1)概念 (2)使用 ①在对象实例化过程中&#xff0c;成员变量先依次进行初始化 ②再进行函数体内二次赋值 3.explicit关键字 (1)C为什么要存在自动隐式类型转换…
阅读更多...
Linux内核中自旋锁驱动代码举例二

Linux内核中自旋锁驱动代码举例二

一. 简介 前面学习了不考虑中断的自旋锁的代码举例，文章地址： Linux内核自旋锁驱动代码举例一-CSDN博客 但是在 Linux系统中，中断时存在的。所以，这里学习使用带保存中断状态的自旋锁API函数，实现对Led设备的互斥访问。 二. 带保存中断状态的自旋锁函数使用 1. 准备…
阅读更多...
Linux第64步_编译移植好的虚拟机文件

Linux第64步_编译移植好的虚拟机文件

最好还是认真了解linux系统移植的整个过程&#xff0c;否则&#xff0c;可能会让你误入歧途。 1、编译移植好的tf-a 1)、编译生成“tf-a-stm32mp157d-atk-trusted.stm32” 输入“cd /home/zgq/linux/atk-mp1/tfa/my-tfa/tf-a-stm32mp-2.2.r1/回车”&#xff0c;切换到“/hom…
阅读更多...
算法打卡day1|数组篇|Leetcode 704.二分查找、27.移除元素

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

数组理论基础 数组是存放在连续内存空间上的相同类型数据的集合&#xff0c;可以方便的通过下标索引的方式获取到下标下对应的数据。 1.数组下标都是从0开始的。 2.数组内存空间的地址是连续的。 正是因为数组的在内存空间的地址是连续的&#xff0c;所以我们在删除或者增添…
阅读更多...
最新文章

Copyright @ 2022~2023