描述
我有一个点云相机，仅仅能输出点云数据，它要被装到一个机械臂上
现在我需要进行手眼标定，如何完成呢？
本篇求出的手眼矩阵，是相机坐标系在手（机械臂末端或者工具末端）坐标系下的位姿。
原理
简单来说，2D相机通过张正友标定法，是提取黑白棋盘格的角点，来得到相机位姿变换的。
点云相机呢，就需要利用它的点云数据了。
原理用几句话说，就是： 在机械臂上安装好相机（需绝对固定），使用代码或者示教器变换多个位姿，让相机对着一个物体进行拍照。我们通过这几帧点云之间的变换关系，及采集这几帧点云时的机械臂位姿，就能得到手眼矩阵了
原理详见我的另外一篇博客
点云相机手眼标定原理（AX=XB）
公式
-
根据原理公式:
A X = X B AX=XB AX=XB
其中
A = T p o s e 1 − 1 ⋅ T p o s e 2 A\; = \; T_{pose1}^{-1}\cdot T_{pose2} A=Tpose1−1⋅Tpose2
B = T o b j e c t 1 ⋅ T o b j e c t 2 − 1 B\; =\; T_{object1}\cdot T_{object2}^{-1} B=Tobject1⋅Tobject2−1 -
A矩阵的计算
假设我们采集15帧点云，那么可以令第一帧为标准点云。则采集第一帧标准点云时的机械臂位姿设为 T p o s e 1 T_{pose1} Tpose1，剩下的14帧可以依次成为 T p o s e 2 T_{pose2} Tpose2，按照上面的公式就可以得到14个A矩阵了
-
B矩阵的计算
T o b j e c t T_{object} Tobject的意思是目标在相机中的位姿，那么点云数据中每个点的xyz就是 T o b j e c t T_{object} Tobject。ICP的操作是将source点云的全部数据点，通过矩阵变换T，变换到target点云下。
所以ICP的操作可以用如下公式：
T o b j e c t 1 = T i c p ⋅ T o b j e c t 2 T_{object1}=T_{icp}\cdot T_{object2} Tobject1=Ticp⋅Tobject2
ICP返回的结果就是B矩阵，因此14次ICP的输出，就是14个B矩阵
解决方案
上面的原理已经提到了，大概分为以下几个步骤
- 机械臂安装相机，搞定相机接口及机械臂接口
- 移动机械臂到多个位姿下，记录当时的机械臂六维位姿，并采集当时的相机点云数据
- 利用ICP算法得到点云之间的变换矩阵
- 根据记录的机械臂位姿，得到位姿之间的变换矩阵
- 3、4步骤得到的矩阵一一对应，利用代码计算，得到最终的手眼标定矩阵
下面我将对3、4、5三个关键步骤进行具体解释，并贴出代码
3. 利用ICP算法得到点云之间的变换矩阵
这一步的保证就是，你已经有了采集到的n帧点云。
具体步骤：
- 假设我有0，1，…，n-1一共n帧点云，将0帧点云作为标准点云，并命名为standard.ply
- 从剩下的n-1帧中挑出1帧点云，命名为source.ply，运行下面的程序，得到1帧点云与0帧点云之间的变换矩阵
- 重复2步骤，一共可以得到n-1个变换矩阵
- 完成此步骤
C++代码有三个
- main.cpp
#include <librealsense2/rs.hpp>
#include <cstdio>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <dirent.h>
#include <unistd.h>
#define SUCCESS 0
#include "ICP.h"
int main(int argc, char** argv)
{
ICP_progress icp_handler;
std::string format = "ply";
// 读取点云文件，作为目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr draw_camera_standard;
std::string path_standard = "../standard.ply";
int isRead_target = icp_handler.readPointCloud_target(format, path_standard);
draw_camera_standard = icp_handler.returnPointCloud("target");
// 读取点云数据，作为源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr draw_camera_source;
std::string path_source = "../source.ply";
int isRead_source = icp_handler.readPointCloud_source(format, path_source);
draw_camera_source = icp_handler.returnPointCloud("source");
// 对目标点云和源点云，都进行区域生长分割
// 可以选择性使用，不是必须的
draw_camera_standard = icp_handler.cut(draw_camera_standard);
draw_camera_source = icp_handler.cut(draw_camera_source);
// 设置目标点云和源点云
icp_handler.setInput_target(draw_camera_standard);
icp_handler.setInput_source(draw_camera_source);
// ICP算法得到转换矩阵
Eigen::Matrix4d ICPtrans;
ICPtrans = icp_handler.getT_Source_to_Target();
icp_handler.printMatrix(ICPtrans);
// 画出ICP的结果
pcl::PointCloud<pcl::PointXYZ>::Ptr draw_target;
pcl::PointCloud<pcl::PointXYZ>::Ptr draw_source;
draw_target = icp_handler.returnPointCloud("target");
draw_source = icp_handler.returnPointCloud("source");
icp_handler.drawPointCloud(draw_target, draw_source, "1");
return 1;
}
- ICP.h
#ifndef __ICP_H__
#define __ICP_H__
//点云需要的头文件
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/console/time.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
//矩阵变换需要的头文件
#include <math.h>
#include <Eigen/Dense>
#include <librealsense2/rs.hpp>
#define PI 3.14159265
// isinf()
#include <stdlib.h>
// segmentation needs
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/io/io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/common/impl/io.hpp>
#include <pcl/point_cloud.h>
#include <pcl/conversions.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/console/parse.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <vtkAutoInit.h>
class ICP_progress
{
public:
ICP_progress();
~ICP_progress();
int init();
public:
int readPointCloud(std::string format, std::string path);
int readPointCloud_target(std::string format, std::string path);
int readPointCloud_source(std::string format, std::string path);
pcl::PointCloud<pcl::PointXYZ>::Ptr returnPointCloud(std::string type);
void setInput_target(pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_tmp);
void setInput_source(pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_tmp);
Eigen::Matrix4d getT_Source_to_Target();
Eigen::MatrixXd cal_T(double x, double y, double z, double rx, double ry, double rz);
std::vector<double> cal_pose(Eigen::MatrixXd T);
pcl::PointCloud<pcl::PointXYZ>::Ptr downSampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float size);
void savePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string outpath);
void drawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string titleName);
void drawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2, std::string titleName);
void drawTransResult();
void printMatrix(const Eigen::Matrix4d & matrix);
pcl::PointCloud<pcl::PointXYZ>::Ptr cut (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
void quickSort(int left, int right, std::vector<double>& arr, std::vector<int>& arr_son);
public:
pcl::PointCloud<pcl::PointXYZ>::Ptr m_pointCloud_target;
pcl::PointCloud<pcl::PointXYZ>::Ptr m_pointCloud_source;
pcl::PointCloud<pcl::PointXYZ>::Ptr m_pointCloud_trans;
Eigen::Matrix4d m_transMatrix;
};
#endif
- ICP.cpp
#include "ICP.h"
ICP_progress::ICP_progress()
{
}
ICP_progress::~ICP_progress()
{
}
int ICP_progress::init()
{
return 1;
}
// 读取点云
int ICP_progress::readPointCloud(std::string format, std::string path)
{
if (format == "ply")
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile<pcl::PointXYZ>(path, *cloud) == -1)
{
PCL_ERROR("Couldnot read file.\n");
return 0;
}
}
return 1;
}
// 读取点云，并将此点云作为目标点云
int ICP_progress::readPointCloud_target(std::string format, std::string path)
{
if (format == "ply")
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile<pcl::PointXYZ>(path, *cloud) == -1)
{
PCL_ERROR("Couldnot read file.\n");
return 0;
}
m_pointCloud_target = cloud;
}
std::cout<<"target pointcloud size: "<<m_pointCloud_target->width<<" * "<<m_pointCloud_target->height << std::endl;
return 1;
}
// 读取点云，并将此点云作为源点云
int ICP_progress::readPointCloud_source(std::string format, std::string path)
{
if (format == "ply")
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile<pcl::PointXYZ>(path, *cloud) == -1)
{
PCL_ERROR("Couldnot read file.\n");
return 0;
}
m_pointCloud_source = cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile<pcl::PointXYZ>(path, *cloud) == -1)
{
PCL_ERROR("Couldnot read file.\n");
return 0;
}
m_pointCloud_trans = cloud1;
}
std::cout<<"source pointcloud size: "<<m_pointCloud_source->width<<" * "<<m_pointCloud_source->height << std::endl;
return 1;
}
// 返回想要的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr ICP_progress::returnPointCloud(std::string type)
{
if (type == "target")
{
return m_pointCloud_target;
}
if (type == "source")
{
return m_pointCloud_source;
}
if (type == "trans")
{
return m_pointCloud_trans;
}
}
// 设置输入点云为目标点云
void ICP_progress::setInput_target(pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_tmp)
{
m_pointCloud_target = pointCloud_tmp;
}
// 设置输入点云为源点云
void ICP_progress::setInput_source(pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_tmp)
{
m_pointCloud_source = pointCloud_tmp;
}
// ICP配准算法
Eigen::Matrix4d ICP_progress::getT_Source_to_Target()
{
int iterations = 800; // 设置迭代次数
pcl::console::TicToc time;
std::cerr << "Target PointCloud has : " << m_pointCloud_target->width * m_pointCloud_target->height << " data points before DownSampling." << std::endl;
std::cerr << "source PointCloud has : " << m_pointCloud_source->width * m_pointCloud_source->height << " data points before DownSampling." << std::endl;
m_pointCloud_target = downSampling(m_pointCloud_target, 0.2f);
m_pointCloud_source = downSampling(m_pointCloud_source, 0.2f);
std::cerr << "Target PointCloud has : " << m_pointCloud_target->width * m_pointCloud_target->height << " data points after DownSampling." << std::endl;
std::cerr << "Source PointCloud has : " << m_pointCloud_source->width * m_pointCloud_source->height << " data points after DownSampling." << std::endl;
time.tic ();
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setMaximumIterations (iterations);
icp.setInputSource (m_pointCloud_source);
icp.setInputTarget (m_pointCloud_target);
icp.align (*m_pointCloud_source); // 这句话会使用ICP匹配结果将源点云进行变换，变换后的点云重新再赋给源点云
icp.setMaximumIterations (1);
std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
if (icp.hasConverged ())
{
std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
transformation_matrix = icp.getFinalTransformation ().cast<double>(); // ICP匹配的结果
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
std::cerr << "FFFFFFFFFFFFFFFuck!!!!!!!!!!" << std::endl;
}
m_transMatrix = transformation_matrix;
return transformation_matrix;
}
// 降采样
pcl::PointCloud<pcl::PointXYZ>::Ptr ICP_progress::downSampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float size)
{
pcl::PCLPointCloud2::Ptr cloud_in (new pcl::PCLPointCloud2), cloud_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
pcl::toPCLPointCloud2(*cloud, *cloud_in);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloud_in);
sor.setLeafSize(size, size, size);
sor.filter(*cloud_blob);
pcl::fromPCLPointCloud2(*cloud_blob, *cloud_out);
return cloud_out;
}
// 保存点云
void ICP_progress::savePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string outpath)
{
std::cerr << "save path is :"<< outpath<< endl;
//将string保存路径转为char*
char *path = new char[outpath.size() +1];
strcpy(path , outpath.c_str());
std::cerr << "Path is : " << path << " ." << std::endl;
//写出点云图
pcl::PLYWriter writer;
writer.write(path, *cloud, true);
std::cerr << "PointCloud has : " << cloud->width * cloud->height << " data points." << std::endl;
}
// 画出点云
void ICP_progress::drawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string titleName)
{
pcl::visualization::PCLVisualizer viewer (titleName);
int v (0);
viewer.createViewPort (0.0, 0.0, 1.0, 1.0, v);
viewer.addCoordinateSystem(0.5);
float bckgr_gray_level = 0.0; // Black
float txt_gray_lvl = 1.0 - bckgr_gray_level;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h (cloud, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl);
viewer.addPointCloud (cloud, cloud_in_color_h, "cloud_in_v1", v);
viewer.addText (titleName, 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v);
viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v);
viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
viewer.setSize (1280, 1024);
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
}
// 两片点云画在一起
void ICP_progress::drawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2, std::string titleName)
{
pcl::visualization::PCLVisualizer viewer (titleName);
int v (0);
viewer.createViewPort (0.0, 0.0, 1.0, 1.0, v);
viewer.addCoordinateSystem(0.5);
float bckgr_gray_level = 0.0; // Black
float txt_gray_lvl= 1.0 - bckgr_gray_level;
// 第一个点云画成红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h1 (cloud1, (int) 255 * 1, (int) 255 * 0, (int) 255 * 0);
viewer.addPointCloud (cloud1, cloud_in_color_h1, "cloud_in_v1", v); // 字符串"cloud_in_v1"代表
// 第一个点云画成蓝色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h2 (cloud2, (int) 255 * 0, (int) 255 * 0, (int) 255 * 1);
viewer.addPointCloud (cloud2, cloud_in_color_h2, "cloud_in_v2", v);
// 增加文本
viewer.addText (titleName, 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v);
// 设置背景颜色为黑
viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v);
viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
viewer.setSize (1280, 1024);
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
}
// 六维pose转变换矩阵
Eigen::MatrixXd ICP_progress::cal_T(double x, double y, double z, double rx, double ry, double rz)
{
Eigen::MatrixXd Rx(3, 3);
Eigen::MatrixXd Ry(3, 3);
Eigen::MatrixXd Rz(3, 3);
Rx << 1, 0, 0, 0, cos(rx), -sin(rx), 0, sin(rx), cos(rx);
Ry << cos(ry), 0, sin(ry), 0, 1, 0, -sin(ry), 0, cos(ry);
Rz << cos(rz), - sin(rz), 0, sin(rz), cos(rz), 0, 0, 0, 1;
Eigen::MatrixXd R(3, 3);
R = Rz * Ry * Rx;
Eigen::MatrixXd P(3, 1);
P << x, y, z;
Eigen::MatrixXd T(4,4);
T << R, P, Eigen::MatrixXd::Zero(1, 3), Eigen::MatrixXd::Identity(1,1);
return T;
}
// 变换矩阵转六维pose
std::vector<double> ICP_progress::cal_pose(Eigen::MatrixXd T)
{
double x = T(0, 3);
double y = T(1, 3);
double z = T(2, 3);
double rx = atan2(T(2, 1), T(2, 2));
double ry = asin(-T(2, 0));
double rz = atan2(T(1, 0), T(0, 0));
std::vector<double> pose;
pose.push_back(x);
pose.push_back(y);
pose.push_back(z);
pose.push_back(rx);
pose.push_back(ry);
pose.push_back(rz);
return pose;
}
// 矩阵打印
void ICP_progress::printMatrix (const Eigen::Matrix4d & matrix)
{
printf ("Rotation matrix :\n");
printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
printf ("Translation vector :\n");
printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
printf ("Transform matrix :\n");
printf (" | %6.3f %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2), matrix (0, 3));
printf ("T = | %6.3f %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2), matrix (1, 3));
printf (" | %6.3f %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2), matrix (2, 3));
printf (" | %6.3f %6.3f %6.3f %6.3f | \n", matrix (3, 0), matrix (3, 1), matrix (3, 2), matrix (3, 3));
}
// 快排算法
// quickSort(数组id起始位置(0), 数组id最后位置(size-1), 想要排序的父数组，想根据父数组排序同时排序的子数组)
void ICP_progress::quickSort(int left, int right, std::vector<double>& arr, std::vector<int>& arr_son)
{
if (left >= right)
{
return;
}
int i, j, base, temp;
i = left;
j = right;
base = arr[left];
int base_son;
base_son = arr_son[left];
int temp_son;
while( i < j)
{
while (arr[j] >= base && i < j)
{
j--;
}
while (arr[i] <= base && i < j)
{
i++;
}
if (i < j)
{
temp = arr[i];
arr[i] = arr[j];
arr[j] = temp;
temp_son = arr_son[i];
arr_son[i] = arr_son[j];
arr_son[j] = temp_son;
}
}
arr[left] = arr[i];
arr[i] = base;
arr_son[left] = arr_son[i];
arr_son[i] = base_son;
quickSort(left, i-1, arr, arr_son);
quickSort(i+1, right, arr, arr_son);
}
// 用区域生长的办法，分割出满足需要的点云簇
pcl::PointCloud<pcl::PointXYZ>::Ptr ICP_progress::cut (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
std::cout<<"segmentation input pointcloud size: "<<cloud->points.size()<<std::endl;
// 1. 进行滤波
pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
voxelSampler.setInputCloud(cloud);
voxelSampler.setLeafSize(0.1f, 0.1f, 0.1f);
voxelSampler.filter(*cloud);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
statFilter.setInputCloud(cloud);
statFilter.setMeanK(10);
statFilter.setStddevMulThresh(0.2);
statFilter.filter(*cloud);
std::cout<<"[segmentation] voxel filter finished!"<<std::endl;
// 2. 计算点云的法线
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
// 设置搜索方式
// ne.setRadiusSearch(10); // 设置欧式距离搜索
ne.setKSearch(10); // 设置为K近邻搜索，较好用
ne.compute(*normals);
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
// 设置滤波轴，我设置为z轴，也就是相机的景深方向
pass.setFilterFieldName("z");
// 设置滤波范围，我只取相机z轴的-1米到1米范围内的数据点(我的相机比较特殊，有z值小于0的情况)
pass.setFilterLimits(-1000, 1000);
std::cout<<"[segmentation] compute normals finished!"<<std::endl;
// 3. 进行区域生长
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize(500);
reg.setMaxClusterSize(10000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(500);
reg.setInputCloud(cloud);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(50.0/ 180.0 * M_PI);
reg.setCurvatureThreshold(1.0);
std::vector<pcl::PointIndices> clusters;
reg.extract(clusters);
std::cout<<"[segmentation] grow finished!"<<std::endl;
// 输出区域生长后，分割出的点云簇数
std::cout<<"Number of cluster is equal to " << clusters.size()<<std::endl;
// id为0的点云簇中，点云的个数
std::cout<<"First cluster has "<<clusters[0].indices.size()<<" points."<<std::endl;
// 由于点云簇的id没有规则，我要按照每簇点云数据在相机z轴上的平均值，进行排序
std::vector<double> z_dist;
std::vector<int> z_dist_id;
for (int i = 0 ; i < clusters.size(); i++)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_part(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, clusters[i].indices, *cloud_part);
double z_sum = 0;
for (int j = 0 ; j < cloud_part->points.size(); j++)
{
z_sum = z_sum + cloud_part->points[i].z;
}
double z_mean = z_sum/ cloud_part->points.size();
z_dist.push_back(z_mean);
z_dist_id.push_back(i);
}
// std::cout<<"before sort:"<<std::endl;
for (int i = 0 ; i < clusters.size(); i++)
{
// std::cout<<"segmentation cluster z value:"<<z_dist[i]<<" its id: "<<z_dist_id[i]<<std::endl;
}
quickSort(0, clusters.size(), z_dist, z_dist_id);
// std::cout<<"after sort:"<<std::endl;
for (int i = 0 ; i < clusters.size(); i++)
{
// std::cout<<"segmentation cluster z value:"<<z_dist[i]<<" its id: "<<z_dist_id[i]<<std::endl;
}
// 以自己的标准，来筛选满足要求的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_objectonly(new pcl::PointCloud<pcl::PointXYZ>);
int x = 0;
while (true)
{
if (z_dist_id[clusters.size()-1-x] >=0 && z_dist_id[clusters.size()-1-x] < 10
&& z_dist[clusters.size()-1-x] < 100 && z_dist[clusters.size()-1-x] > -100 )
{
pcl::copyPointCloud(*cloud, clusters[z_dist_id[clusters.size()-1-x]].indices, *cloud_objectonly);
break;
}
x++;
}
int cluster_need_num = 2; // 在满足要求的点云中，最后保留的簇数
int num = 0;
for (int i = x+1 ; i < clusters.size(); i++)
{
if (z_dist_id[clusters.size()-1-i] >=0 && z_dist_id[clusters.size()-1-i] < 100
&& z_dist[clusters.size()-1-i] < 0 && z_dist[clusters.size()-1-i] > -100 )
{
int id = z_dist_id[clusters.size()-1-i];
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, clusters[id].indices, *cloud_tmp);
std::cout<<"segmentation keep id: "<<id<<" cluster pointcloud"<<std::endl;
*cloud_objectonly = (*cloud_objectonly) + (*cloud_tmp);
num ++;
}
if (num > cluster_need_num)
{
break;
}
}
std::cout<<"After segmentation pointcloud of object size: "<<cloud_objectonly->points.size()<<std::endl;
return cloud_objectonly;
}
- 附赠一个CMakeLists.txt
cmake_minimum_required(VERSION 2.8.7)
project(test)
find_package(PCL 1.5 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fexceptions -frtti -pthread -O3 -march=core2")
find_package(OpenCV 3 REQUIRED)
set(ROOT "${CMAKE_CURRENT_SOURCE_DIR}/")
include_directories(
${OpenCV_INCLUDE_DIRS}
${ROOT}
${ROOT}/include
)
file(GLOB SOURCES
"*.cpp"
)
link_directories(
${ROOT}/lib
)
set( PROJECT_LINK_LIBS
${OpenCV_LIBRARIES}
)
add_executable(test ${SOURCES})
target_link_libraries(test ${PROJECT_LINK_LIBS})
target_link_libraries(test ${PCL_LIBRARIES})
上述代码最终会打印出变换矩阵T，长成这样
Transform matrix :
| 1.000 -0.002 -0.006 1.241 |
T = | 0.002 0.999 0.054 -42.445 |
| 0.006 -0.054 0.999 -12.251 |
| 0.000 0.000 0.000 1.000 |
这就是两帧点云之间的变换矩阵T，请你记录这个T。
你有1个目标点云和n-1个源点云，因此一共会记录n-1个这样的矩阵。
4. 得到机械臂位姿之间的变换矩阵
我把这一步骤，写在了第5步骤的C++代码中。
所以在这一步骤，你可以将n次机械臂的位姿进行记录，然后填写第5步骤的C++代码中。
5. 计算出手眼标定矩阵
- 大体步骤：
- 根据步骤3和步骤4，你已经得到了n-1个点云变换矩阵，和n-1个机械臂位姿变换矩阵，请你把它输入到C++代码中，C++代码会输出n-1个A矩阵和n-1个B矩阵
- 将这n-1个A矩阵和n-1个B矩阵输入到matlab代码中，得到最终的手眼变换矩阵
-
具体步骤：
先执行cpp代码，代码中有以下两行，这是我们需要的n-1组矩阵A和矩阵Bstd::cout<<"A: "<<m<<" "<<A_1<<std::endl; std::cout<<"B: "<<m<<" "<<B_1<<std::endl;再将cpp代码运行出来的A和B，原封不动的拷贝到matlab代码中，matlab代码会运行出来最终需要的手眼矩阵
-
C++代码注释
C++代码里面都是我当时采集的数据，我采集了15组数据，因此你会看到有1组数据作为标准，还有14组数据作为资源。T_standard // 采集标准点云时的机械臂位姿 T_pose_X // 采集14组源点云时的机械臂位姿 T_icp_X // 每个源点云与标准点云进行ICP得到的矩阵T，一共14个 // 一般ICP变换需要一个初始值才能得到好的结果， // 11、12、14次ICP前我进行了一个初始位姿变换， // 初始变换矩阵也在代码中 // 剩下的ICP匹配没有进行初始变换，因此不需要对ICP输出的T进行补偿 -
第一步，C++代码
#include <string>
#include <chrono>
#include <iostream>
#include <stdlib.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
cv::Mat skew( cv::Mat res )
{
cv::Mat result = (cv::Mat_<double>(3, 3) << 0, -res.at<double>(2), res.at<double>(1),
res.at<double>(2), 0, -res.at<double>(0),
-res.at<double>(1), res.at<double>(0), 0);
return result;
}
void Tsai_HandEye(cv::Mat Hcg, std::vector<cv::Mat> Hgij, std::vector<cv::Mat> Hcij)
{
CV_Assert(Hgij.size() == Hcij.size());
int nStatus = Hgij.size();
cv::Mat Rgij(3, 3, CV_64FC1);
cv::Mat Rcij(3, 3, CV_64FC1);
cv::Mat rgij(3, 1, CV_64FC1);
cv::Mat rcij(3, 1, CV_64FC1);
double theta_gij;
double theta_cij;
cv::Mat rngij(3, 1, CV_64FC1);
cv::Mat rncij(3, 1, CV_64FC1);
cv::Mat Pgij(3, 1, CV_64FC1);
cv::Mat Pcij(3, 1, CV_64FC1);
cv::Mat tempA(3, 3, CV_64FC1);
cv::Mat tempb(3, 1, CV_64FC1);
cv::Mat A;
cv::Mat b;
cv::Mat pinA;
cv::Mat Pcg_prime(3, 1, CV_64FC1);
cv::Mat Pcg(3, 1, CV_64FC1);
cv::Mat PcgTrs(1, 3, CV_64FC1);
cv::Mat Rcg(3, 3, CV_64FC1);
cv::Mat eyeM = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat Tgij(3, 1, CV_64FC1);
cv::Mat Tcij(3, 1, CV_64FC1);
cv::Mat tempAA(3, 3, CV_64FC1);
cv::Mat tempbb(3, 1, CV_64FC1);
cv::Mat AA;
cv::Mat bb;
cv::Mat pinAA;
cv::Mat Tcg(3, 1, CV_64FC1);
for (int i = 0; i < nStatus; i++)
{
Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij);
Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij);
Rodrigues(Rgij, rgij);
Rodrigues(Rcij, rcij);
theta_gij = norm(rgij);
theta_cij = norm(rcij);
rngij = rgij / theta_gij;
rncij = rcij / theta_cij;
Pgij = 2 * sin(theta_gij / 2) * rngij;
Pcij = 2 * sin(theta_cij / 2) * rncij;
tempA = skew(Pgij + Pcij);
tempb = Pcij - Pgij;
A.push_back(tempA);
b.push_back(tempb);
}
//Compute rotation
invert(A, pinA, cv::DECOMP_SVD);
Pcg_prime = pinA * b;
Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
PcgTrs = Pcg.t();
Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));
//Computer Translation
for (int i = 0; i < nStatus; i++)
{
Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij);
Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij);
Hgij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tgij);
Hcij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tcij);
tempAA = Rgij - eyeM;
tempbb = Rcg * Tcij - Tgij;
AA.push_back(tempAA);
bb.push_back(tempbb);
}
invert(AA, pinAA, cv::DECOMP_SVD);
Tcg = pinAA * bb;
std::cout<< Rcg << std::endl;
// std::cout<<pinAA<<std::endl;
Rcg.copyTo(Hcg(cv::Rect(0, 0, 3, 3)));
Tcg.copyTo(Hcg(cv::Rect(3, 0, 1, 3)));
Hcg.at<double>(3, 0) = 0.0;
Hcg.at<double>(3, 1) = 0.0;
Hcg.at<double>(3, 2) = 0.0;
Hcg.at<double>(3, 3) = 1.0;
}
cv::Mat turnMat(double* t)
{
cv::Mat T = cv::Mat::eye(4, 4, CV_64FC1);
for (int i = 0 ; i < 4 ; i ++)
{
for (int j = 0 ; j < 4 ; j++)
{
T.at<double>(i, j) = t[i*4+j];
}
}
return T;
}
int main(int argc, char** argv)
{
double T_standard[16] = {-0.25524255, 0.12031712, -0.95936178, -0.6760785,
-0.09978927, 0.9836505, 0.14991265, -0.06037279,
0.96171375, 0.1339981, -0.2390631, 0.50568673,
0, 0, 0, 1 };
double T_pose[14][16] =
{
{-0.26383388, 0.18034671, -0.94755831, -0.67968672,
-0.08162853, 0.97466671, 0.20823448, -0.0178505,
0.96110794, 0.1322871, -0.24242867, 0.50596305,
0, 0, 0, 1 },
{-0.1963116, 0.21239575, -0.95726162, -0.68870174,
-0.11550519, 0.96445191, 0.23767849, -0.04151841,
0.9737147, 0.15722772, -0.16480029, 0.51909631,
0, 0, 0, 1 },
{-0.169679, 0.23561441, -0.95691948, -0.7170147,
-0.10342051, 0.96138082, 0.2550512, -0.02941691,
0.98005777, 0.14224194, -0.13875878, 0.52580124,
0, 0, 0, 1 },
{-0.26520668, 0.10583734, -0.95836521, -0.6773803,
-0.15608149, 0.97613524, 0.15099193, -0.05860629,
0.95147464, 0.18962714, -0.24235833, 0.50599987,
0, 0, 0, 1},
{-0.24069052, 0.15361661, -0.95836841, -0.67738464,
0.0294564, 0.98809724, 0.15098397, -0.05861155,
0.97015483, 0.00811033, -0.24235063, 0.50599069,
0, 0, 0, 1 },
{-0.3615591, 0.19425222, -0.91188875, -0.64662315,
-0.67335138, 0.62209175, 0.39949941, 0.11971361,
0.64488211, 0.75846419, -0.09412294, 0.59935488,
0, 0, 0, 1 },
{-0.19507234, 0.07267798, -0.97809237, -0.68566007,
-0.51631044, 0.84027536, 0.16541117, 0.05632625,
0.83388867, 0.53726644, -0.12639009, 0.56325553,
0, 0, 0, 1 },
{-0.23237602, 0.15030715, -0.9609418, -0.66938051,
-0.09921504, 0.97917007, 0.17715065, -0.08792771,
0.96755246, 0.13650544, -0.21262292, 0.49808188,
0, 0, 0, 1 },
{-0.08356536, 0.24551896, -0.96578324, -0.72048522,
0.1361197, 0.9629011, 0.23300838, -0.0834392 ,
0.98716172, -0.1119907, -0.11388515, 0.53090047,
0, 0, 0, 1 },
{-0.04729372, 0.14458483, -0.98836154, -0.70446721,
0.47438652, 0.87401335, 0.10515744, -0.1747646,
0.87904535, -0.4638921, -0.10992444, 0.53345066,
0, 0, 0, 1 },
{-0.17503298, -0.27461233, -0.9454901, -0.69823802,
-0.66734834, -0.67296667, 0.31900166, 0.10523705,
-0.72388511, 0.68680706, -0.06547072, 0.88044311,
0, 0, 0, 1 },
{-0.06531844, -0.26620852, -0.96169981, -0.69689061,
-0.44634325, -0.85417599, 0.26676034, 0.07357126,
-0.89247477, 0.44667259, -0.06302693, 0.92860157,
0, 0, 0, 1 },
{-0.02199697, 0.27766358, -0.9604265, -0.71570685,
0.22554917, 0.9372706, 0.26580328, -0.09693875,
0.97398342, -0.21077654, -0.0832439, 0.5338395,
0, 0, 0, 1 },
{-0.1473856, -0.17992332, -0.97257652, -0.68905526,
-0.80031314, -0.55610438, 0.22415797, 0.13696474,
-0.58118531, 0.81140343, -0.06203322, 0.83710394,
0, 0, 0, 1 }
};
double T_icp[14][16] =
{
{1.000, -0.011, -0.007, 1.496,
0.011, 0.998, 0.055, -42.174,
0.006, -0.055, 0.998, -12.222,
0.000, 0.000, 0.000, 1.000 },
{0.998, 0.004, 0.069, -12.265,
-0.010, 0.996, 0.090, -11.046,
-0.068, -0.091, 0.994, 3.755,
0.000, 0.000, 0.000, 1.000},
{ 0.996, -0.015, 0.093, -22.542,
0.005, 0.994, 0.108, -20.474,
-0.094, -0.107, 0.990, -18.740,
0.000, 0.000, 0.000, 1.000},
{0.998, 0.064, -0.001, -0.909,
-0.064, 0.998, 0.001, 11.387,
0.001, -0.001, 1.000, -1.757,
0.000, 0.000, 0.000, 1.000},
{0.992, -0.128, 0.001, 0.013,
0.128, 0.992, 0.001, -28.970,
-0.001, -0.001, 1.000, -1.337,
0.000, 0.000, 0.000, 1.000},
{0.775, 0.623, 0.106, -17.595,
-0.625, 0.733, 0.270, -48.337,
0.091, -0.275, 0.957, 0.140,
0.000, 0.000, 0.000, 1.000},
{0.989, -0.127, 0.069, -12.234,
0.122, 0.989, 0.080, -24.133,
-0.078, -0.070, 0.994, -0.615,
0.000, 0.000, 0.000, 1.000},
{1.000, -0.005, 0.030, 5.644,
0.005, 1.000, 0.028, 27.656,
-0.030, -0.028, 0.999, 16.522,
0.000, 0.000, 0.000, 1.000},
{0.956, -0.263, 0.128, -24.772,
0.253, 0.963, 0.087, -20.774,
-0.146, -0.051, 0.988, -0.945,
0.000, 0.000, 0.000, 1.000},
{0.808, -0.569, 0.149, 1.221,
0.580, 0.813, -0.040, -8.701,
-0.098, 0.119, 0.988, 19.394,
0.000, 0.000, 0.000, 1.000},
// T_icp的第11、12、14 进行过初始变换
{0.723, -0.680, 0.125, -8.953,
0.651, 0.730, 0.207, -36.900,
-0.232, -0.068, 0.970, -20.702,
0.000, 0.000, 0.000, 1.000},
{0.870, -0.476, 0.125, -11.664,
0.455, 0.875, 0.168, -56.815,
-0.189, -0.089, 0.978, 4.210,
0.000, 0.000, 0.000, 1.000},
{0.920, -0.364, 0.143, -14.118,
0.351, 0.930, 0.113, -22.443,
-0.174, -0.054, 0.983, 11.453,
0.000, 0.000, 0.000, 1.000},
{0.584, -0.802, 0.126, 9.211,
0.798, 0.596, 0.089, -39.305,
-0.146, 0.049, 0.988, -0.394,
0.000, 0.000, 0.000, 1.000}
};
// 初始变换的矩阵
double T_icp_111214[16] = {-0.98768834, 0.15643447, 0.0, 0.0,
-0.15643447, -0.98768834, 0.0, 0.0,
0, 0, 1, 0,
0.000, 0.000, 0.000, 1.000};
// ICP的结果，x,y,z为毫米单位，所以需要转化
for (int i = 0; i < 14; i++)
{
T_icp[i][3] = T_icp[i][3] * 0.001;
T_icp[i][7] = T_icp[i][7] * 0.001;
T_icp[i][11] = T_icp[i][11] * 0.001;
}
// ICP结果由数组变为矩阵
std::vector<cv::Mat> T_icp_mat;
for (int i = 0; i < 14; i++)
{
cv::Mat X = turnMat(T_icp[i]);
T_icp_mat.push_back(X);
}
// 对第11、12、14次ICP结果进行补偿
cv::Mat T_icp_initial = turnMat(T_icp_111214);
cv::Mat T_icp_11_afterinitial = turnMat(T_icp[10]);
cv::Mat T_icp_12_afterinitial = turnMat(T_icp[11]);
cv::Mat T_icp_14_afterinitial = turnMat(T_icp[13]);
T_icp_mat[10] = T_icp_11_afterinitial * T_icp_initial;
T_icp_mat[11] = T_icp_12_afterinitial * T_icp_initial;
T_icp_mat[13] = T_icp_14_afterinitial * T_icp_initial;
std::vector<cv::Mat> A;
std::vector<cv::Mat> B;
cv::Mat A_ = cv::Mat::eye(4, 4, CV_64FC1);
cv::Mat B_ = cv::Mat::eye(4, 4, CV_64FC1);
cv::Mat T_0 = turnMat(T_standard);
for (int i = 0 ; i < 14; i++)
{
cv::Mat T = turnMat(T_pose[i]);
cv::Mat K = T_0.inv() * T;
for (int m = 0 ; m < 4 ; m ++)
{
for (int n = 0 ; n < 4 ; n++)
{
A_.at<double>(m, n) = K.at<double>(m, n);
B_.at<double>(m, n) = T_icp_mat[i].at<double>(m, n);
}
}
A.push_back(A_);
B.push_back(B_);
std::cout<<"A: "<<i<<" "<<A_<<std::endl;
std::cout<<"B: "<<i<<" "<<B_<<std::endl;
}
cv::Mat eyeInHand = cv::Mat::eye(4, 4, CV_64FC1);
std::cout<<"calib result"<<std::endl;
Tsai_HandEye(eyeInHand, A, B);
std::cout<<eyeInHand<<std::endl;
return 1;
}
- 第二步，matlab代码
写在了我另外一篇博客中Matlab求解AX=XB（手眼标定用）
总结
写了这么多，点个赞，加个关注鼓励一下吧
2904
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
