手眼标定学习总结：camera和vicon标定实践

最新推荐文章于 2024-06-21 19:00:43 发布

larry_dongy

最新推荐文章于 2024-06-21 19:00:43 发布

阅读量2.4k

点赞数 1

分类专栏：数学基础文章标签： matlab 矩阵

本文链接：https://blog.csdn.net/tfb760/article/details/122309232

版权

数学基础专栏收录该内容

14 篇文章 0 订阅

订阅专栏

本文档仅用于记录自己在科研过程中研究手眼标定的过程。

关于原理的总结，见上一篇博客。手眼标定学习总结：原理、Tsai方法和Matlab代码

又经过了元旦3天的调试，今天终于搞定了标定过程。特此记录！

1. camera to target

首先标定相机在target（棋盘格）的坐标。采用核心matlab代码如下：

% calcCameraPose
function [R, t, imagePoints] = calcCameraPose(I, markerSize)
    cameraParams= load('cameraParams.mat');
    [imagePoints,boardSize] = detectCheckerboardPoints(I);
    worldPoints =  generateCheckerboardPoints(boardSize, markerSize);
    worldPoints = [worldPoints, zeros(length(worldPoints), 1)];
    [R,t] = estimateWorldCameraPose(imagePoints,worldPoints,cameraParams.cameraParams);
    t = t';
return 

% 计算camera2target的R和t
[orientation, location, imagePoints] = calcCameraPose(I, 20);
t_t_c = location;
R_t_c = orientation';

需要注意的是：

R_t_c/t_t_c表示将一个点在camera系下的坐标，变换到target系下的坐标
所调用的函数estimateWorldCameraPose ，得到的是camera在world下的“朝向”和“位置”。所谓的“朝向”是相机按照当前相机坐标系，旋转后抵达world系。所以“将一个点从camera移动到world”对应的旋转矩阵应该是“朝向”的逆变换。（坐标系->坐标系一点）

2. gipper to base

采用VICON系统提供从gripper到base的坐标变换。

首先在VICON中创建刚体，#1和#2两个点连线确定的是z轴的负方向，#1和#3与z-方向的分量为y+方向，剩下确定的是x方向。
在这里插入图片描述
其次，VICON提供的旋转角度和平移，有多种表示方式，例如XYZ/ZYX以及 Helical Axis。前面的都是欧拉角，最后一个是轴角表示。最终导出的数据也是轴角表示。

注意轴角的旋转部分，导出单位是度°，在计算时需要变成弧度，且模长表示旋转角度。

这里的“旋转”和“平移”表示，base系经过此变换后，抵达gripper系（询问了杜博，一般人体是以胯关节为起点，计算出base到胯的变换后，后续其他关节全部相对于胯进行计算）。所以将一个点从gripper系变换到base系恰好是这个旋转，不需要求逆。

将VICON数据转成标准的gripper2base的轴角，核心代码如下：

filename = "vicon_data/"+int2str(idx)+".csv";
fid = fopen(filename, "r");
for j = 1:20             % skip the "titles" of csv file.
    tmp = fgetl(fid);
end
d = textscan(fid, '%d,%d,%f,%f,%f,%f,%f,%f', 1);
rvec(i, :) = deg2rad([d{3}, d{4}, d{5}]);
tvec(i, :) = [d{6}, d{7}, d{8}];

3. 手眼标定

直接进入手眼标定部分，载入两个csv文件，之后进行（核心部分代码）

%% calculate relative transformation
N = 18;
T_b_g_list = zeros(4, 4, N);
T_t_c_list = zeros(4, 4, N);
for i = 1:N
    r_t_c = rvec_t_c(i, :)';
    t_t_c = tvec_t_c(i, :)';
    R_t_c = Rodrigues(r_t_c);
    T_t_c = [R_t_c, t_t_c; 0,0,0,  1];
    
    r_b_g = rvec_b_g(i, :)';
    t_b_g = tvec_b_g(i, :)';
    R_b_g = Rodrigues(r_b_g);
    T_b_g = [R_b_g, t_b_g; 0,0,0,  1];
    T_t_c_list(:,:,i) = T_t_c;
    T_b_g_list(:,:,i) = T_b_g;
end

%% calculate Gij and Cij
Cij_list = [];
Gij_list = [];
for k = 1:N-1
    i = k;
    j = k+1;
    Cij = inv(T_t_c_list(:,:,i)) * T_t_c_list(:,:,j);
    Gij = inv(T_b_g_list(:,:,i)) * T_b_g_list(:,:,j);
    Cij_list = [Cij_list, Cij];
    Gij_list = [Gij_list, Gij];
end

% X is T_g_c when given GX = XC
T_g_c = tsai(Gij_list, Cij_list)

4. 注意事项

手眼标定的精度受实验实采数据的影响极大！
参考文章：手眼标定，我的结果显示手和眼相距上千米！手眼标定结果准确率如何提高？

一开始没有注意摆放的位姿，比较随意，甚至用代码随机生成了一系列的位姿，进行手眼标定。虽然仿真时精确计算的结果是准确的，但只要增加些小的扰动，会产生极大的误差。一开始以为是算法的数值稳定性问题，后来发现是数据给的不合适。所以一定要用实测数据进行验证！

简单来说，总结如下：

由于camera和gripper之间距离较近，所以尽可能让pose和pose之间的相对距离变化尽可能的小，如果太远，标定的距离远小于移动距离，会造成计算不精确。同时，我注意到，如果单位用m来表示，误差会较大。如果改成cm或mm，则相对误差较小，可能是Tsai算法或代码实现中数值计算中的一些问题。
计算一组相对位姿时，尽可能让相机有不同的旋转，以体现出camera到gripper的旋转。

其实这些在Tsai的论文中做了数值上的推导，只是我一开始没有当回事而已。截图一些结论：
在这里插入图片描述