ABB_Robot/PCL/PCL_C++/ICP_H.h
2024-05-07 11:50:04 +08:00

77 lines
1.6 KiB
C++

#ifndef _ICP_H
#define _ICP_H
#include <vector>
#include <string>
#include <pcl\point_cloud.h>
#include <pcl\point_types.h>
#include <pcl\visualization\pcl_visualizer.h>
struct Vertex
{
double coord[3];
};
struct Point
{
float x;
float y;
float z;
};
class ICP
{
public:
ICP();
ICP(int controlnum = 1000, double thre = 0.01, int iter = 100);
virtual ~ICP();
void readfile(std::string firstname, std::string secondname);
void run();
void writefile(std::string name);
void showcloud(std::string firstname, std::string secondname, std::string thirdname);
private:
void initransmat();//初始化旋转矩阵和平移矩阵
void sample();//采样控制点
double closest();//找出最近点并返回误差
void getcenter();//获取两个控制点的中心
void rmcontcenter();//移动两个控制点的中心
void transform();//将四元数转换成矩阵并更新整个旋转矩阵
void uprotate();// 更新变换矩阵
void uptranslate();
void updata();//更新控制点坐标
void applyall();
private:
double distance(Vertex a, Vertex b);
void printTT();
void printTR();
private:
int conNum;//控制点数目
int iterate;//迭代次数
double threshold;//阈值
std::vector<Vertex> VarrP;//起始点
std::vector<Vertex> VarrQ;
Vertex meanP;//控制点中心
Vertex meanQ;
Vertex* contP;//P控制点
Vertex* contQ;
Vertex* rmcoP;//移动后的控制点
Vertex* rmcoQ;
int* index; //在采样控制点和寻找相应的点索引时使用
double TT[3];//平移向量
double TR[3][3];//旋转矩阵
double Rw[3][3];//旋转的步距
double Tw[3];//平移的步距
double quad[4];//四元数
};
#endif /* ICP_H */