点云处理PCL常用函数与工具
文章目录
前言
本文主要记录pcl点云处理常用工具，在此将每种方法写为一个函数，便于使用的时候直接调用。
一、点云读取与保存
数据读取
数据读取的类型包括.pcd和.ply文件
void loadFile(std::string path, pcl::PointCloud<POINTTYPE>::Ptr cloud) {
std::cout << path << endl;
std::string fileType = path.substr(path.find_last_of('.') + 1);//获取文件后缀
if (fileType == "ply") {
if (pcl::io::loadPLYFile <POINTTYPE>(path, *cloud) == -1)
{
std::cout << "Cloud reading .ply failed." << std::endl;
}
}
else if (fileType == "pcd") {
if (pcl::io::loadPCDFile <POINTTYPE>(path, *cloud) == -1)
{
std::cout << "Cloud reading .pcd failed." << std::endl;
}
}
}
数据保存
数据保存的格式将根据自定义的文件名后缀确定，包括ply和pcd两种类型
int saveFile(std::string path, pcl::PointCloud<POINTTYPE>::Ptr cloud) {
std::string fileType = path.substr(path.find_last_of('.') + 1);//获取文件后缀
if (fileType == "ply") {
if (pcl::io::savePLYFile(path, *cloud) == -1)
{
std::cout << "Cloud saving .ply failed." << std::endl;
return 0;
}
}
else if (fileType == "pcd") {
if (pcl::io::savePCDFile(path, *cloud) == -1)
{
std::cout << "Cloud saving .pcd failed." << std::endl;
return 0;
}
}
return 1;
}
以上两种点云保存的数据格式包括：
pcl::PointXYZI
pcl::PointXYZRGB
pcl::PointXYZ
自定义的点云保存格式
那么除了以上几种点云数据格式，如果需要保存自定义的数据格式，例如在包含坐标、法线、颜色、强度信息外、额外的保存一些其信息（标签、id）等信息那么就需要对保存的点云格式进行自定义，具体方法如下：
//首先定义新增数据格式的结构体
#define PCL_ADD_LABEL \
struct \
{
\
int p_label;\
int p_id;\
}; \
// 定义点云数据类型的结构体
struct PointXYZRGBINormalLI
{
PCL_ADD_POINT4D;
PCL_ADD_RGB;
PCL_ADD_INTENSITY;
PCL_ADD_NORMAL4D;
PCL_ADD_LABEL;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZRGBINormalLI,
(float, x, x)
(float, y, y)
(float, z, z)
(uint8_t, b, b)
(uint8_t, g, g)
(uint8_t, r, r)
(float, rgb, rgb)
(float, intensity, intensity)
(float, normal_x, normal_x)
(float, normal_y, normal_y)
(float, normal_z, normal_z)
(int, p_label, p_label)
(int, p_id, p_id)
)
以上数据结构体几乎包含了点特征的基本信息，如果有其他信息需要保存可自行修改。
随后保存的时候将 POINTTYPE的类型改为PointXYZRGBINormalLI即可
pcl::PointCloud<PointXYZRGBINormalSI>::Ptr point_cloud(new pcl::PointCloud<PointXYZRGBINormalSI>);
pcl::io::loadPCDFile(pcd_path, *point_cloud);// 点云读取
saveFile(pcd_path, point_cloud);//点云保存
二、点云显示
点云显示-根据颜色
void ShowPCLPointsXYZRGB(pcl::PointCloud<pcl::PointXYZRGB>::Ptr color_cloud)
{
cout << "point size:" << color_cloud->size() << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer1->addPointCloud<pcl::PointXYZRGB>(color_cloud);
viewer1->setBackgroundColor(0, 0, 0); // 设置点云大小
viewer1->addCoordinateSystem(0);
while (!viewer1->wasStopped())
{
viewer1->spinOnce(100);
}
}
根据点云指定轴数值进行颜色显示
点云显示-根据指定轴数值
void ShowPCLPointsXYZRGB(pcl::PointCloud<pcl::PointXYZ>::Ptr color_cloud)
{
cout << "point size:" << color_cloud->size() << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer1->addPointCloud<pcl::PointXYZ>(color_cloud);
viewer1->setBackgroundColor(0, 0, 0); // 设置点云大小
viewer1->addCoordinateSystem(0);
while (!viewer1->wasStopped())
{
viewer1->spinOnce(100);
}
}
点云显示-根据指定信息显示
void ShowPCLPointsInput(pcl::PointCloud<pcl::PointXYZI>::Ptr color_cloud,std::string str)
{
// str: x,y,z intensity
std::cout << "point size:" << color_cloud->size() << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::visualization::PointCloudColorHandlerGenericField<PointT> fildColor(Input_cloud, str); // 按照x字段进行渲染
viewer1->addPointCloud<PointT>(color_cloud, fildColor, "Input_RGB"); // 显示点云，其中fildColor为颜色显示
viewer1->setBackgroundColor(0, 0, 0); // 设置点云大小
viewer1->addCoordinateSystem(0);
while (!viewer1->wasStopped())
{
viewer1->spinOnce(100);
}
}
多组点云显示
给定一个vector容器，里面用于存储多组不同的点云,然后为每组点云随机赋予颜色
#define Random(x) (rand() % x)
void ShowClusterPointsXYZRGB(std::vector<pcl::PointCloud<pcl::PointXYZ>>& cloud_clusters) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointXYZRBG(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB point;
cout << "End show particle size:" << cloud_clusters.size() << endl;
for (int i = 0; i < cloud_clusters.size(); i++) {
int color_B = Random(255);
int color_G = Random(255);
int color_R = Random(255);
for (int j = 0; j < cloud_clusters[i].size(); j++) {
point.x = cloud_clusters[i].points[j].x;
point.y = cloud_clusters[i].points[j].y;
point.z = cloud_clusters[i].points[j].z;
point.r = color_R;
point.g = color_G;
point.b = color_B;
pointXYZRBG->push_back(point);
}
}
ShowPCLPointsXYZRGB(pointXYZRBG);
}
三、点云滤波
直通滤波
// 点云直通滤波
void parrProcessZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<float>& start_end, pcl::PointCloud<pcl::PointXYZ>::Ptr pass_cloud) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PassThrough<pcl::PointXYZ>pass;
//沿z轴过滤
pass.setInputCloud(cloud); //输入点云
pass.setFilterFieldName("z"); // 沿Z轴滤波，也可沿 X Y
pass.setFilterLimits(start_end[0], start_end[1]); //选取0-1之间
//pass.setFilterLimitsNegative(true); //可选择0-1之间数据保留还是舍弃
pass.filter(*pass_cloud); //过滤
}
统计滤波
void StatisticalFilterCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr inputCloud, int k, int std, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &outCloud)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filterCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sta;
sta.setInputCloud(inputCloud);
sta.setMeanK(k);
sta.setStddevMulThresh(std);
sta.filter(*filterCloud);
outCloud = filterCloud;
}
均匀下采样滤波
void UniformSamplingFilterCloud(pcl::PointCloud<PointT>::Ptr inputCloud, pcl::PointCloud<PointT>::Ptr &outCloud)
{
pcl::UniformSampling<PointT> us; //创建滤波器对象
us.setInputCloud(inputCloud); //设置待滤波点云
us.setRadiusSearch(0.005f); //设置滤波球半径
us.filter(*outCloud); //执行滤波
}
VoxelGrid 点云体素下采样滤波
void VoxelGridFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{
pcl::VoxelGrid<pcl::PointXYZ> vg; //创建滤波器对象
vg.setInputCloud(inputCloud); //设置待滤波点云
vg.setLeafSize(0.05f, 0.05f, 0.05f); //设置体素大小
vg.filter(*outCloud); //执行滤波
}
RadiusOutlierRemoval 半径滤波
void RadiusOutlierFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{
pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror; //创建滤波器对象
ror.setInputCloud(inputCloud); //设置待滤波点云
ror.setRadiusSearch(0.02); //设置查询点的半径范围
ror.setMinNeighborsInRadius(5); //设置判断是否为离群点的阈值，即半径内至少包括的点数
//ror.setNegative(true); //默认false，保存内点；true，保存滤掉的外点
ror.filter(*outCloud); //执行滤波
}
ConditionRemoval 条件滤波器
#include <pcl/filters/conditional_removal.h>
void ConditionalFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{
/*创建条件限定下的滤波器*/
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());//创建条件定义对象range_cond
//为条件定义对象添加比较算子
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, -0.1)));//添加在x字段上大于 -0.1 的比较算子
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 1.0)));//添加在x字段上小于 1.0 的比较算子
pcl::ConditionalRemoval<pcl::PointXYZ> cr; //创建滤波器对象
cr.setCondition(range_cond); //用条件定义对象初始化
cr.setInputCloud(inputCloud); //设置待滤波点云
//cr.setKeepOrganized(true); //设置保持点云的结构
//cr.setUserFilterValue(5); //将过滤掉的点用（5，5，5）代替
cr.filter(*outCloud); //执行滤波，保存滤波结果于cloud_filtered
平面投影滤波器
void SACMODEL_PLANEFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{
//平面投影
//创建 x+y+z=0 平面
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
coefficients->values.resize(4); //设置模型系数的大小
coefficients->values[0] = 1.0; //x系数
coefficients->values[1] = 1.0; //y系数
coefficients->values[2] = 1.0; //z系数
coefficients->values[3] = 0.0; //常数项
//投影滤波
pcl::ProjectInliers<pcl::PointXYZ> proj;//创建投影滤波器对象
proj.setModelType(pcl::SACMODEL_PLANE); //设置对象对应的投影模型
proj.setInputCloud(inputCloud); //设置输入点云
proj.setModelCoefficients(coefficients);//设置模型对应的系数
proj.filter(*outCloud); //执行投影滤波
//圆柱投影
}
球面投影滤波器
void SACMODEL_SPHEREFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{
//--------- 创建球面模型 (x - x0)^2 + (y - y0)^2 + (z - z0)^2 = r^2 -------------
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
coefficients->values.resize(4);
coefficients->values[0] = 0;
coefficients->values[1] = 0;
coefficients->values[2] = -1.0;
coefficients->values[3] = 2.0;
//========= 创建球面模型 (x - x0)^2 + (y - y0)^2 + (z - z0)^2 = r^2 ============
//-------------------------------- 执行投影滤波 --------------------------------
PointCloudT::Ptr cloud_projected(new PointCloudT);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_SPHERE); //球面模型
proj.setInputCloud(inputCloud);
proj.setModelCoefficients(coefficients);
proj.filter(*outCloud);
}
四、点云轮廓提取
点云轮廓点提取
void GetEageXYZ(pcl::PointCloud<pcl::PointXYZ>cloud_clusters, pcl::PointCloud<pcl::PointXYZ>& boundPointsXYZ) {
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est;
pcl::search::Search<pcl::PointXYZ>::Ptr GetEageTree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;
pcl::PointCloud<pcl::Normal>::Ptr beGetEageNormals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Boundary> boundaries;
pcl::PointCloud<pcl::PointXYZ>::Ptr beGetEageClusters(new pcl::PointCloud<pcl::PointXYZ>);
beGetEageClusters = cloud_clusters.makeShared(); //use .makeShared() -> pcl::PointCloud<pcl::PointXYZ> -> pcl::PointCloud<pcl::PointXYZ>::Ptr
normEst.setInputCloud(beGetEageClusters);
normEst.setSearchMethod(GetEageTree);
normEst.setKSearch(9);
normEst.compute(*beGetEageNormals);
est.setInputCloud(beGetEageClusters);
est.setInputNormals(beGetEageNormals);/*M_PI_2 */
est.setAngleThreshold(M_PI_2); ///在这里 由于构造函数已经对其进行了初始化 为Π/2 ，必须这样 使用 M_PI/2 M_PI_2
est.setSearchMethod(GetEageTree);
est.setKSearch(30); //一般这里的数值越高，最终边界识别的精度越好 20
// est.setRadiusSearch(everagedistance); //搜索半径
est.compute(boundaries);
pcl::PointCloud<pcl::PointXYZ>::Ptr boundPoints(new pcl::PointCloud<pcl::PointXYZ>);
int countBoundaries = 0;
for (int j = 0; j < beGetEageClusters->size(); j++) {
uint8_t x = (boundaries.points[j].boundary_point);
int a = static_cast<int>(x); //该函数的功能是强制类型转换
if (a == 1)
{
boundPoints->push_back(beGetEageClusters->points[j]);
}
}
boundPointsXYZ = *boundPoints;
}
提取凸包轮廓
/** 提取轮廓凸包*/
void getPointCloudConvexBoundary(pcl::PointCloud<pcl::PointXYZ> inputCluster, pcl::PointCloud<pcl::PointXYZ>& outputTbBoundary) {
// 采用Graham凸包检测算法获取点云的2D凸包轮廓
pcl::ConvexHull<pcl::PointXYZ> convexhull;
convexhull.setInputCloud(inputCluster.makeShared());//载入点云
convexhull.setDimension(2);//设置凸包维度为2维
std::vector<pcl::Vertices> polygons;//保存凸包的容器
convexhull.reconstruct(outputTbBoundary, polygons);//计算凸包结果
}
提取凹包轮廓
/** 提取轮廓凹包*/
void getPointCloudConcaveBoundary(pcl::PointCloud<pcl::PointXYZ> inputCluster, pcl::PointCloud<pcl::PointXYZ>& outputAbBoundary) {
//
pcl::ConcaveHull<pcl::PointXYZ> concavehull;
concavehull.setInputCloud(inputCluster.makeShared());//载入点云
concavehull.setDimension(2);//设置凹包维度为2维
concavehull.setAlpha(2);
std::vector<pcl::Vertices> polygons;//保存凹包的容器
concavehull.reconstruct(outputAbBoundary, polygons);//计算凹包结果
}
常用处理工具
点云沿指定轴旋转
//点云旋转
void PointCloudRoate(pcl::PointCloud<pcl::PointXYZ>::Ptr inputcloud,int type, pcl::PointCloud<pcl::PointXYZ>::Ptr outcloud) {
double angle = M_PI;
Eigen::Matrix4f transform_z = Eigen::Matrix4f::Identity();
//沿X旋转
if (type == 1) {
transform_z(1, 1) = cos(angle);
transform_z(1, 2) = -sin(angle);
transform_z(2, 1) = sin(angle);
transform_z(2, 2) = cos(angle);
pcl::transformPointCloud(*inputcloud, *outcloud, transform_z);
}
//沿X旋转
if (type == 2) {
transform_z(0, 0) = cos(angle);
transform_z(0, 2) = sin(angle);
transform_z(2, 0) = -sin(angle);
transform_z(2, 2) = cos(angle);
pcl::transformPointCloud(*inputcloud, *outcloud, transform_z);
}
//沿X旋转
if (type == 3) {
transform_z(0, 0) = cos(angle);
transform_z(0, 1) = -sin(angle);
transform_z(1, 0) = sin(angle);
transform_z(1, 1) = cos(angle);
pcl::transformPointCloud(*inputcloud, *outcloud, transform_z);
}
}
点云沿向量旋转
void RoateByVector(pcl::PointCloud<PointT>::Ptr input, pcl::PointCloud<PointT>::Ptr& out，double v_y,double v_z，double v_z){
//向量旋转到(0,0,1)
Eigen::Vector3f vecbefore;
vecbefore << v_x, v_y, v_z;
Eigen::Vector3f vecafter;
vecafter << 0, 0, 1;
double tem = vecbefore.dot(vecafter);//分子
double tep = sqrt(vecbefore.dot(vecbefore) * vecafter.dot(vecafter));//分母
double angle = acos(tem / tep);
if (isnan(angle))//acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00
{
angle = acos(tep / tem);
}
Eigen::Vector3f axis2 = vecbefore.cross(vecafter);
Eigen::Affine3f RoatTransform = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
RoatTransform.translation() << 0, 0, 0;
// The same rotation matrix as before; theta radians arround Z axis
RoatTransform.rotate(Eigen::AngleAxisf(angle, axis2.normalized()));
pcl::transformPointCloud(*input, *out, RoatTransform);
贪婪三角剖分
void Greedy_projection_triangulation(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_Cloud) {
// 法线估计
pcl::search::Search<pcl::PointXYZ>::Ptr InputTree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr Inputnormals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(InputTree);
normal_estimator.setInputCloud(inputCloud);
normal_estimator.setKSearch(20);
normal_estimator.compute(*Inputnormals);
// 将点云位姿、颜色、法线信息连接到一起
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*inputCloud, *Inputnormals, *cloud_with_normals);
pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal>);
tree->setInputCloud(cloud_with_normals);
//三角化
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
gp3.setSearchRadius(1);
gp3.setMu(5); //设置被样本点搜索其邻近点的最远距离为2.5，为了适应点云密度的变化
gp3.setMaximumNearestNeighbors(100); //设置样本点可搜索的邻域个数为100
gp3.setMinimumAngle(M_PI / 18); // 设置三角化后得到的三角形内角的最小的角度为10°
gp3.setMaximumAngle(2 * M_PI / 3); // 设置三角化后得到的三角形内角的最大角度为120°
gp3.setMaximumSurfaceAngle(M_PI / 4); // 设置某点法线方向偏离样本点法线的最大角度45°，如果超过，连接时不考虑该点
gp3.setNormalConsistency(false); //设置该参数为true保证法线朝向一致，设置为false的话不会进行法线一致性检查
gp3.setInputCloud(cloud_with_normals); //设置输入点云为有向点云
gp3.setSearchMethod(tree); //设置搜索方式
gp3.reconstruct(triangles); //重建提取三角化
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0); //
viewer->addPolygonMesh(triangles, "wangge"); // 基于三角剖分的曲面重建
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
}
//std::vector<int> parts = gp3.getPartIDs();//获得重建后每点的 ID, Parts 从 0 开始编号， a-1 表示未连接的点。
/*
获得重建后每点的状态，取值为 FREE 、 FRINGE 、 BOUNDARY 、 COMPLETED 、 NONE 常量，
其中 NONE 表示未定义，
FREE 表示该点没有在 三 角化后的拓扑内，为自由点，
COMPLETED 表示该点在三角化后的拓扑内，并且邻域都有拓扑点，
BOUNDARY 表示该点在三角化后的拓扑边缘，
FRINGE 表示该点在 三 角化后的拓扑内，其连接会产生重叠边。
*/
//std::vector<int> states = gp3.getPointStates();
// First get the xyz information
pcl::PointCloud<pcl::PointXYZ> xyz_cloud;
pcl::fromPCLPointCloud2(triangles.cloud, xyz_cloud);
out_Cloud = xyz_cloud.makeShared();
}
计算坐标点向对应的两组点云之间的旋转矩阵
void pose_estimation_3d3d(
const std::vector<cv::Point3f>& pts1,
const std::vector<cv::Point3f>& pts2,
Eigen::Matrix4f& TransMatrix
)
{
cv::Point3f p1, p2; // center of mass
int N = pts1.size();
for (int i = 0; i < N; i++)
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 = cv::Point3f(cv::Vec3f(p1) / N);
p2 = cv::Point3f(cv::Vec3f(p2) / N);
std::vector<cv::Point3f> q1(N), q2(N); // remove the center
for (int i = 0; i < N; i++)
{
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < N; i++)
{
W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
}
//cout << "W=" << W << endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
//cout << "U=" << U << endl;
//cout << "V=" << V << endl;
Eigen::Matrix3d R_ = U * (V.transpose());
Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
// convert to cv::Mat
/*R = (Mat_<double>(3, 3) <<
R_(0, 0), R_(0, 1), R_(0, 2),
R_(1, 0), R_(1, 1), R_(1, 2),
R_(2, 0), R_(2, 1), R_(2, 2)
);
t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));*/
TransMatrix << R_(0, 0), R_(0, 1), R_(0, 2), t_(0, 0),
R_(1, 0), R_(1, 1), R_(1, 2), t_(1, 0),
R_(2, 0), R_(2, 1), R_(2, 2), t_(2, 0),
0, 0, 0, 1;
}