由于手眼标定不佳（旋转约定错误），触碰测试不安全。
问题
手眼标定结果很差，进行触碰测试是不安全的。
手眼变换 |
手眼残差 |
投影验证 |
触碰测试 |
|---|---|---|---|
差 |
差 |
差 |
不安全 |
手眼标定结果不佳的示例
手眼变换矩阵看起来不正确，因为它的几个值与物理测量值相差极大，对于这种设置来说不合理。
-0.402 |
0.908 |
-0.117 |
134.552 |
-0.910 |
-0.411 |
-0.060 |
-440.257 |
-0.102 |
0.082 |
0.991 |
-706.041 |
0 |
0 |
0 |
1 |
数值 -706.040 应约为 130 mm，而 -440.256 则大致为 90 mm。此阶段我们忽略正负号（+/-），因为它取决于坐标系的定义；我们仅关注距离大小。
标定残差也明显大于预期，高出几个数量级。
N（位姿） |
平均旋转角度（°） |
最大旋转角度（°） |
平均平移残差（mm） |
最大平移残差（mm） |
|---|---|---|---|---|
14 |
10.764 |
20.647 |
99.307 |
176.708 |
对于小型机器人，我们预计残差在亚毫米范围内；而对于大型机器人，残差则在毫米范围内。至于残差的旋转分量，我们预计其在亚度范围内。
全部的残差数据
位姿 |
旋转角度（°） |
平移量（mm） |
|---|---|---|
1 |
1.800 |
46.595 |
2 |
15.119 |
64.707 |
3 |
14.726 |
143.476 |
4 |
7.726 |
176.708 |
5 |
12.562 |
99.719 |
6 |
5.175 |
94.542 |
7 |
20.020 |
109.891 |
8 |
14.028 |
92.717 |
9 |
20.647 |
88.241 |
10 |
10.870 |
142.423 |
11 |
1.818 |
52.156 |
12 |
7.910 |
108.207 |
13 |
11.969 |
65.376 |
14 |
6.329 |
105.534 |
通过投影验证失败，这表明手眼标定不正确，触碰测试将会严重失败。
潜在原因
在将位姿从机器人控制器传输到 PC 时，使用了错误的旋转约定进行转换。
如果使用错误的旋转约定转换机器人位姿，则方向信息会出错。由于手眼标定高度依赖于精确的旋转数据，即使是微小的约定偏差也会导致较大的方向误差，进而影响平移结果。这会导致残差极大、投影验证失败，以及手眼变换矩阵完全无效，从而使触碰测试变得不安全。
辅助工具：位姿转换
Zivid 主要使用 (4x4) 变换矩阵（旋转矩阵 + 平移向量）。机器人控制器和软件通常将位姿表示为位置向量和方向向量的组合，并采用多种格式之一。位姿转换工具可帮助实现以下格式之间的相互转换：
轴角（Axis-Angle）
旋转矢量（Rotation Vector）
横滚-俯仰-偏航（Roll-Pitch-Yaw）
欧拉角（Euler Angles）
四元数（Quaternion）
我们提供几种用于机器人位姿转换的工具：
代码示例
/*
Convert to/from Transformation Matrix (Rotation Matrix + Translation Vector)
Zivid primarily operate with a (4x4) transformation matrix. This example shows how to use Eigen to
convert to and from: AxisAngle, Rotation Vector, Roll-Pitch-Yaw, Quaternion
Note: the translation part of the transformation matrix is in millimeters (mm), since Zivid point clouds are in
millimeters. If your robot reports translation in meters, multiply it by 1000 before saving the pose.
The convenience functions from this example can be reused in applicable applications. The YAML files for this sample can
be found under the main instructions for Zivid samples.
For more information on pose conversions, check out this article:
https://support.zivid.com/en/latest/camera/reference-articles/pose-conversions.html
*/
#include <Zivid/Zivid.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <stdexcept>
namespace
{
enum class RotationConvention
{
zyxIntrinsic,
xyzExtrinsic,
xyzIntrinsic,
zyxExtrinsic,
nofROT
};
constexpr size_t nofRotationConventions = static_cast<size_t>(RotationConvention::nofROT);
struct RollPitchYaw
{
RotationConvention convention;
Eigen::Array3f rollPitchYaw;
};
std::string toString(RotationConvention convention)
{
switch(convention)
{
case RotationConvention::xyzIntrinsic: return "xyzIntrinsic";
case RotationConvention::xyzExtrinsic: return "xyzExtrinsic";
case RotationConvention::zyxIntrinsic: return "zyxIntrinsic";
case RotationConvention::zyxExtrinsic: return "zyxExtrinsic";
case RotationConvention::nofROT: break;
default: throw std::runtime_error{ "Unhandled rotation convention " };
}
throw std::invalid_argument("Invalid RotationConvention");
}
// The following function converts Roll-Pitch-Yaw angles in radians to Rotation Matrix.
// This function takes an array of angles, and a rotation convention, as input parameters.
// Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
// The order of elements in the output array follow the convention, i.e., for ZYX and zyx, the first
// element of the array is rotation around z-axis, for XYZ and xyz, the first element is rotation
// around x-axis.
Eigen::Matrix3f rollPitchYawToRotationMatrix(const Eigen::Array3f &rollPitchYaw, const RotationConvention &rotation)
{
switch(rotation)
{
case RotationConvention::xyzIntrinsic:
return (Eigen::AngleAxisf(rollPitchYaw[0], Eigen::Vector3f::UnitX())
* Eigen::AngleAxisf(rollPitchYaw[1], Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(rollPitchYaw[2], Eigen::Vector3f::UnitZ()))
.matrix();
case RotationConvention::zyxExtrinsic:
return (Eigen::AngleAxisf(rollPitchYaw[2], Eigen::Vector3f::UnitX())
* Eigen::AngleAxisf(rollPitchYaw[1], Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(rollPitchYaw[0], Eigen::Vector3f::UnitZ()))
.matrix();
case RotationConvention::zyxIntrinsic:
return (Eigen::AngleAxisf(rollPitchYaw[0], Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(rollPitchYaw[1], Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(rollPitchYaw[2], Eigen::Vector3f::UnitX()))
.matrix();
case RotationConvention::xyzExtrinsic:
return (Eigen::AngleAxisf(rollPitchYaw[2], Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(rollPitchYaw[1], Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(rollPitchYaw[0], Eigen::Vector3f::UnitX()))
.matrix();
case RotationConvention::nofROT: break;
default: throw std::runtime_error{ "Unhandled rotation convention" };
}
throw std::invalid_argument("Invalid orientation");
}
void rollPitchYawListToRotationMatrix(const std::vector<RollPitchYaw> &rpyList)
{
Eigen::IOFormat matrixFmt(4, 0, ", ", "\n", "[", "]", "[", "]");
for(const auto &rotation : rpyList)
{
std::cout << "Rotation Matrix from Roll-Pitch-Yaw angles (" << toString(rotation.convention)
<< "):" << std::endl;
const auto rotationMatrixFromRollPitchYaw =
rollPitchYawToRotationMatrix(rotation.rollPitchYaw, rotation.convention);
std::cout << rotationMatrixFromRollPitchYaw.format(matrixFmt) << std::endl;
}
}
// The following function converts Rotation Matrix to Roll-Pitch-Yaw angles in radians.
// Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
// The order of elements in the output array follow the convention, i.e., for ZYX and zyx, the first
// element of the array is rotation around z-axis, for XYZ and xyz, the first element is rotation
// around x-axis.
// Eigen only supports intrinsic Euler systems for simplicity. If you want to use extrinsic Euler systems,
// Eigen recoments just to use the equal intrinsic opposite order for axes and angles, i.e., axes (A,B,C)
// becomes (C,B,A), and angles (a,b,c) becomes (c,b,a).
Eigen::Array3f rotationMatrixToRollPitchYaw(
const Eigen::Matrix3f &rotationMatrix,
const RotationConvention &convention)
{
switch(convention)
{
case RotationConvention::zyxExtrinsic: return rotationMatrix.eulerAngles(0, 1, 2).reverse();
case RotationConvention::xyzIntrinsic: return rotationMatrix.eulerAngles(0, 1, 2);
case RotationConvention::xyzExtrinsic: return rotationMatrix.eulerAngles(2, 1, 0).reverse();
case RotationConvention::zyxIntrinsic: return rotationMatrix.eulerAngles(2, 1, 0);
case RotationConvention::nofROT: break;
default: throw std::runtime_error{ "Unhandled rotation convention" };
}
throw std::invalid_argument("Invalid rotation");
}
std::vector<RollPitchYaw> rotationMatrixToRollPitchYawList(const Eigen::Matrix3f &rotationMatrix)
{
const Eigen::IOFormat vectorFmt(4, 0, ", ", "", "", "", "[", "]");
std::vector<RollPitchYaw> rpyList;
for(size_t i = 0; i < nofRotationConventions; i++)
{
RotationConvention convention{ static_cast<RotationConvention>(i) };
std::cout << "Roll-Pitch-Yaw angles (" << toString(convention) << "):" << std::endl;
rpyList.push_back({ convention, rotationMatrixToRollPitchYaw(rotationMatrix, convention) });
std::cout << rpyList[i].rollPitchYaw.format(vectorFmt) << std::endl;
}
return rpyList;
}
Eigen::Vector3f rotationMatrixToRotationVector(const Eigen::Matrix3f &rotationMatrix)
{
const Eigen::AngleAxisf axisAngle(rotationMatrix);
return axisAngle.angle() * axisAngle.axis();
}
Eigen::Matrix3f rotationVectorToRotationMatrix(const Eigen::Vector3f &rotationVector)
{
Eigen::AngleAxisf axisAngle(rotationVector.norm(), rotationVector.normalized());
return axisAngle.toRotationMatrix();
}
void printHeader(const std::string &txt)
{
const std::string asterixLine = "****************************************************************";
std::cout << asterixLine << "\n* " << txt << std::endl << asterixLine << std::endl;
}
Eigen::Affine3f zividToEigen(const Zivid::Matrix4x4 &zividMatrix)
{
Eigen::Matrix4f eigenMatrix;
for(std::size_t row = 0; row < Zivid::Matrix4x4::rows; row++)
{
for(std::size_t column = 0; column < Zivid::Matrix4x4::cols; column++)
{
eigenMatrix(row, column) = zividMatrix(row, column);
}
}
Eigen::Affine3f eigenTransform{ eigenMatrix };
return eigenTransform;
}
Zivid::Matrix4x4 eigenToZivid(const Eigen::Affine3f &eigenTransform)
{
Eigen::Matrix4f eigenMatrix = eigenTransform.matrix();
Zivid::Matrix4x4 zividMatrix;
for(Eigen::Index row = 0; row < eigenMatrix.rows(); row++)
{
for(Eigen::Index column = 0; column < eigenMatrix.cols(); column++)
{
zividMatrix(row, column) = eigenMatrix(row, column);
}
}
return zividMatrix;
}
} // namespace
int main()
{
try
{
Zivid::Application zivid;
std::cout << std::setprecision(4);
Eigen::IOFormat matrixFormatRules(4, 0, ", ", "\n", "[", "]", "[", "]");
Eigen::IOFormat vectorFormatRules(4, 0, ", ", "", "", "", "[", "]");
printHeader("This example shows conversions to/from Transformation Matrix");
Zivid::Matrix4x4 transformationMatrixZivid(std::string(ZIVID_SAMPLE_DATA_DIR) + "/RobotTransform.yaml");
const Eigen::Affine3f transformationMatrix = zividToEigen(transformationMatrixZivid);
std::cout << transformationMatrix.matrix().format(matrixFormatRules) << std::endl;
// Extract Rotation Matrix and Translation Vector from Transformation Matrix
const Eigen::Matrix3f rotationMatrix = transformationMatrix.linear();
const Eigen::Vector3f translationVector = transformationMatrix.translation();
std::cout << "RotationMatrix:\n" << rotationMatrix.format(matrixFormatRules) << std::endl;
std::cout << "TranslationVector:\n" << translationVector.format(vectorFormatRules) << std::endl;
/*
* Convert from Rotation Matrix (Zivid) to other representations of orientation (Robot)
*/
printHeader("Convert from Zivid (Rotation Matrix) to Robot");
const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
<< axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;
const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;
const Eigen::Quaternionf quaternion(rotationMatrix);
std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;
const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);
/*
* Convert to Rotation Matrix (Zivid) from other representations of orientation (Robot)
*/
printHeader("Convert from Robot to Zivid (Rotation Matrix)");
const Eigen::Matrix3f rotationMatrixFromAxisAngle = axisAngle.toRotationMatrix();
std::cout << "Rotation Matrix from Axis Angle:\n"
<< rotationMatrixFromAxisAngle.format(matrixFormatRules) << std::endl;
const Eigen::Matrix3f rotationMatrixFromRotationVector = rotationVectorToRotationMatrix(rotationVector);
std::cout << "Rotation Matrix from Rotation Vector:\n"
<< rotationMatrixFromRotationVector.format(matrixFormatRules) << std::endl;
const Eigen::Matrix3f rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
std::cout << "Rotation Matrix from Quaternion:\n"
<< rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;
rollPitchYawListToRotationMatrix(rpyList);
// Combine Rotation Matrix with Translation Vector to form Transformation Matrix
Eigen::Affine3f transformationMatrixFromQuaternion(rotationMatrixFromQuaternion);
transformationMatrixFromQuaternion.translation() = translationVector;
Zivid::Matrix4x4 transformationMatrixFromQuaternionZivid = eigenToZivid(transformationMatrixFromQuaternion);
transformationMatrixFromQuaternionZivid.save("RobotTransformOut.yaml");
}
catch(const std::exception &e)
{
std::cerr << "Error: " << e.what() << std::endl;
std::cout << "Press enter to exit." << std::endl;
std::cin.get();
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
/*
Convert to/from Transformation Matrix (Rotation Matrix + Translation Vector)
Zivid primarily operate with a (4x4) transformation matrix. This example implements functions to convert to and from:
AxisAngle, Rotation Vector, Roll-Pitch-Yaw, Quaternion.
Note: the translation part of the transformation matrix is in millimeters (mm), since Zivid point clouds are in
millimeters. If your robot reports translation in meters, multiply it by 1000 before saving the pose.
The convenience functions from this example can be reused in applicable applications. The YAML files for this sample
can be found under the main instructions for Zivid samples.
For more information on pose conversions, check out this article:
https://support.zivid.com/en/latest/camera/reference-articles/pose-conversions.html
*/
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
class Program
{
static void Main()
{
try
{
var zivid = new Zivid.NET.Application();
PrintHeader("This example shows conversions to/from Transformation Matrix");
var transformationMatrixZivid = new Zivid.NET.Matrix4x4(Environment.GetFolderPath(Environment.SpecialFolder.CommonApplicationData) + "/Zivid/RobotTransform.yaml");
var transformationMatrix = ZividToMathDotNet(transformationMatrixZivid);
Console.WriteLine(MatrixToString(transformationMatrix));
// Extract Rotation Matrix and Translation Vector from Transformation Matrix
var rotationMatrix = transformationMatrix.SubMatrix(0, 3, 0, 3);
var translationVector = transformationMatrix.SubMatrix(0, 3, 3, 1);
Console.WriteLine("RotationMatrix:\n" + MatrixToString(rotationMatrix));
Console.WriteLine("TranslationVector:\n" + MatrixToString(translationVector.Transpose()));
/*
* Convert from Rotation Matrix (Zivid) to other representations of orientation (Robot)
*/
PrintHeader("Convert from Zivid (Rotation Matrix) to Robot");
var axisAngle = RotationMatrixToAxisAngle(rotationMatrix);
Console.WriteLine("AxisAngle:\n" + MatrixToString(axisAngle.Axis.Transpose()) + ", " + String.Format(" {0:G4} ", axisAngle.Angle));
var rotationVector = axisAngle.Axis * axisAngle.Angle;
Console.WriteLine("Rotation Vector:\n" + MatrixToString(rotationVector.Transpose()));
var quaternion = RotationMatrixToQuaternion(rotationMatrix);
Console.WriteLine("Quaternion:\n" + MatrixToString(quaternion.Transpose()));
var rpyList = RotationMatrixToRollPitchYawList(rotationMatrix);
/*
* Convert to Rotation Matrix (Zivid) from other representations of orientation (Robot)
*/
PrintHeader("Convert from Robot to Zivid (Rotation matrix)");
var rotationMatrixFromAxisAngle = AxisAngleToRotationMatrix(axisAngle);
Console.WriteLine("Rotation Matrix from Axis Angle:\n" + MatrixToString(rotationMatrixFromAxisAngle));
var rotationMatrixFromRotationVector = RotationVectorToRotationMatrix(rotationVector);
Console.WriteLine("Rotation Matrix from Rotation Vector:\n" + MatrixToString(rotationMatrixFromRotationVector));
var rotationMatrixFromQuaternion = QuaternionToRotationMatrix(quaternion);
Console.WriteLine("Rotation Matrix from Quaternion:\n" + MatrixToString(rotationMatrixFromQuaternion));
RollPitchYawListToRotationMatrix(rpyList);
// Combine Rotation Matrix with Translation Vector to form Transformation Matrix
var transformationMatrixFromQuaternion = Matrix<double>.Build.Dense(4, 4);
transformationMatrixFromQuaternion.SetSubMatrix(0, 3, 0, 3, rotationMatrixFromQuaternion);
transformationMatrixFromQuaternion.SetSubMatrix(0, 3, 3, 1, translationVector);
var transformationMatrixFromQuaternionZivid = MathDotNetToZivid(transformationMatrixFromQuaternion);
transformationMatrixFromQuaternionZivid.Save("RobotTransformOut.yaml");
}
catch (Exception ex)
{
Console.WriteLine("Error: {0}", ex.ToString());
Environment.ExitCode = 1;
}
}
enum RotationConvention
{
zyxIntrinsic,
xyzExtrinsic,
xyzIntrinsic,
zyxExtrinsic
};
public class AxisAngle
{
public AxisAngle(Matrix<double> axis, double angle)
{
this.Axis = axis;
this.Angle = angle;
}
public Matrix<double> Axis { get; set; }
public double Angle { get; set; }
}
static Matrix<double> Skew(Matrix<double> vector)
// Assumes vector to be [3x1]
{
return CreateMatrix.DenseOfArray<double>(new double[,]
{
{ 0, -vector[2,0], vector[1,0] },
{ vector[2,0], 0, -vector[0,0] },
{ -vector[1,0], vector[0,0], 0 }
});
}
static AxisAngle RotationMatrixToAxisAngle(Matrix<double> rotationMatrix)
{
// See Rodrigues' formula or Skew-symmetric method
var skewSymmetricMatrix = (rotationMatrix - rotationMatrix.Transpose()) / 2;
Matrix<double> skewElements = CreateMatrix.DenseOfArray<double>(new double[,] { { skewSymmetricMatrix[2, 1] }, { skewSymmetricMatrix[0, 2] }, { skewSymmetricMatrix[1, 0] } });
var skewNorm = skewElements.Column(0, 0, 3).L2Norm();
Matrix<double> u = skewElements / skewNorm;
var theta = Math.Atan2(skewNorm, (rotationMatrix.Trace() - 1) / 2);
return new AxisAngle(u, theta);
}
static Matrix<double> AxisAngleToRotationMatrix(AxisAngle axisAngle)
{
//See Rodrigues' formula or Skew-symmetric method
var u = axisAngle.Axis;
var firstTerm = CreateMatrix.DenseIdentity<double>(3) * Math.Cos(axisAngle.Angle);
var secondTerm = u.Multiply(u.Transpose()) * (1 - Math.Cos(axisAngle.Angle));
var thirdTerm = Skew(u) * Math.Sin(axisAngle.Angle);
return firstTerm + secondTerm + thirdTerm;
}
static Matrix<double> RotationVectorToRotationMatrix(Matrix<double> rotationVector)
{
double theta = rotationVector.L2Norm();
return AxisAngleToRotationMatrix(new AxisAngle(rotationVector / theta, theta));
}
static Matrix<double> RotationMatrixToQuaternion(Matrix<double> rotationMatrix)
{
var qw = (Math.Sqrt(1 + rotationMatrix[0, 0] + rotationMatrix[1, 1] + rotationMatrix[2, 2])) / 2;
Matrix<double> quaternion = CreateMatrix.DenseOfArray<double>(new double[,]
{
{ (rotationMatrix[2,1] - rotationMatrix[1,2])/(4*qw) },
{ (rotationMatrix[0,2] - rotationMatrix[2,0])/(4*qw) },
{ (rotationMatrix[1,0] - rotationMatrix[0,1])/(4*qw) },
{ qw } // REAL PART
});
return quaternion;
}
static Matrix<double> QuaternionToRotationMatrix(Matrix<double> quaternion)
{
// Normalize quaternion
var nQ = quaternion / quaternion.L2Norm();
var firstTerm = CreateMatrix.DenseIdentity<double>(3);
var secondTerm = 2 * Skew(nQ.SubMatrix(0, 3, 0, 1)) * Skew(nQ.SubMatrix(0, 3, 0, 1));
var thirdTerm = 2 * nQ[3, 0] * Skew(nQ.SubMatrix(0, 3, 0, 1));
return firstTerm + secondTerm + thirdTerm;
}
static Matrix CreateRotationMatrix(string axis, double angle)
{
var matrix = DenseMatrix.CreateIdentity(3);
var cosAngle = Math.Cos(angle);
var sinAngle = Math.Sin(angle);
switch (axis)
{
case ("x"):
matrix[1, 1] = cosAngle;
matrix[2, 2] = cosAngle;
matrix[1, 2] = -sinAngle;
matrix[2, 1] = sinAngle;
break;
case ("y"):
matrix[0, 0] = cosAngle;
matrix[2, 2] = cosAngle;
matrix[0, 2] = sinAngle;
matrix[2, 0] = -sinAngle;
break;
case ("z"):
matrix[0, 0] = cosAngle;
matrix[1, 1] = cosAngle;
matrix[0, 1] = -sinAngle;
matrix[1, 0] = sinAngle;
break;
default: throw new Exception("Wrong axis; options are x, y, z.");
}
return matrix;
}
static Matrix CreateXRotation(double angle)
{
return CreateRotationMatrix("x", angle);
}
static Matrix CreateYRotation(double angle)
{
return CreateRotationMatrix("y", angle);
}
static Matrix CreateZRotation(double angle)
{
return CreateRotationMatrix("z", angle);
}
static Vector<double> RotationMatrixToRollPitchYaw(Matrix<double> rotationMatrix, RotationConvention convention)
{
double roll = -1;
double pitch = -1;
double yaw = -1;
switch (convention)
{
case RotationConvention.zyxExtrinsic:
case RotationConvention.xyzIntrinsic:
if (rotationMatrix[0, 2] < 1)
{
if (rotationMatrix[0, 2] > -1)
{
roll = Math.Atan2(-rotationMatrix[1, 2], rotationMatrix[2, 2]);
pitch = Math.Asin(rotationMatrix[0, 2]);
yaw = Math.Atan2(-rotationMatrix[0, 1], rotationMatrix[0, 0]);
}
else // R02 = −1
{
// Not a unique solution: yaw − roll = atan2(R01, R11)
roll = -Math.Atan2(rotationMatrix[1, 0], rotationMatrix[1, 1]);
pitch = -Math.PI / 2;
yaw = 0;
}
}
else // R02 = +1
{
// Not a unique solution: yaw + roll = atan2(R10, R11)
roll = Math.Atan2(rotationMatrix[1, 0], rotationMatrix[1, 1]);
pitch = Math.PI / 2;
yaw = 0;
}
break;
case RotationConvention.xyzExtrinsic:
case RotationConvention.zyxIntrinsic:
if (rotationMatrix[2, 0] < 1)
{
if (rotationMatrix[2, 0] > -1)
{
roll = Math.Atan2(rotationMatrix[1, 0], rotationMatrix[0, 0]);
pitch = Math.Asin(-rotationMatrix[2, 0]);
yaw = Math.Atan2(rotationMatrix[2, 1], rotationMatrix[2, 2]);
}
else // R20 = −1
{
// Not a unique solution: yaw − roll = atan2(−R12, R11)
roll = -Math.Atan2(rotationMatrix[1, 2], rotationMatrix[1, 1]);
pitch = Math.PI / 2;
yaw = 0;
}
}
else // R20 = +1
{
// Not a unique solution: yaw + roll = atan2(−R12, R11)
roll = Math.Atan2(rotationMatrix[1, 2], rotationMatrix[1, 1]);
pitch = -Math.PI / 2;
yaw = 0;
}
break;
}
switch (convention)
{
case RotationConvention.zyxExtrinsic:
case RotationConvention.xyzExtrinsic:
return CreateVector.DenseOfArray<double>(new double[] { yaw, pitch, roll });
case RotationConvention.xyzIntrinsic:
case RotationConvention.zyxIntrinsic:
return CreateVector.DenseOfArray<double>(new double[] { roll, pitch, yaw });
}
throw new ArgumentException("Invalid RotationConvention");
}
static Vector<double>[] RotationMatrixToRollPitchYawList(Matrix<double> rotationMatrix)
{
Vector<double>[] rpyList = new Vector<double>[Enum.GetValues(typeof(RotationConvention)).Length];
foreach (int i in Enum.GetValues(typeof(RotationConvention)))
{
RotationConvention convention = (RotationConvention)i;
Console.WriteLine("Roll-Pitch-Yaw angles (" + convention + "):");
rpyList[i] = RotationMatrixToRollPitchYaw(rotationMatrix, convention);
Console.WriteLine(VectorToString(rpyList[i]));
}
return rpyList;
}
static Matrix<double> RollPitchYawToRotationMatrix(Vector<double> rollPitchYaw, RotationConvention convention)
{
switch (convention)
{
case RotationConvention.xyzIntrinsic:
return CreateXRotation(rollPitchYaw[0]) * CreateYRotation(rollPitchYaw[1]) * CreateZRotation(rollPitchYaw[2]);
case RotationConvention.zyxIntrinsic:
return CreateZRotation(rollPitchYaw[0]) * CreateYRotation(rollPitchYaw[1]) * CreateXRotation(rollPitchYaw[2]);
case RotationConvention.zyxExtrinsic:
return CreateXRotation(rollPitchYaw[2]) * CreateYRotation(rollPitchYaw[1]) * CreateZRotation(rollPitchYaw[0]);
case RotationConvention.xyzExtrinsic:
return CreateZRotation(rollPitchYaw[2]) * CreateYRotation(rollPitchYaw[1]) * CreateXRotation(rollPitchYaw[0]);
default: throw new ArgumentException("Invalid RotationConvention");
}
}
static void RollPitchYawListToRotationMatrix(Vector<double>[] rpyList)
{
foreach (int i in Enum.GetValues(typeof(RotationConvention)))
{
RotationConvention convention = (RotationConvention)i;
Console.WriteLine("Rotation Matrix from Roll-Pitch-Yaw angles (" + convention + "):");
Console.WriteLine(MatrixToString(RollPitchYawToRotationMatrix(rpyList[i], convention)));
}
}
static Matrix<double> ZividToMathDotNet(Zivid.NET.Matrix4x4 zividMatrix)
{
return CreateMatrix.DenseOfArray(zividMatrix.ToArray()).ToDouble();
}
static Zivid.NET.Matrix4x4 MathDotNetToZivid(Matrix<double> mathNetMatrix)
{
return new Zivid.NET.Matrix4x4(mathNetMatrix.ToSingle().ToArray());
}
static void PrintHeader(string text)
{
string asterixLine = "****************************************************************";
Console.WriteLine(asterixLine + "\n* " + text + "\n" + asterixLine);
}
static string MatrixToString(Matrix<double> matrix)
{
string matrixString = "";
if (matrix.RowCount != 1)
{
matrixString = "[";
}
for (var i = 0; i < matrix.RowCount; i++)
{
matrixString += "[";
for (var j = 0; j < matrix.ColumnCount; j++)
{
matrixString += String.Format(" {0,9:G4} ", matrix[i, j]);
}
matrixString += "]\n ";
}
matrixString = matrixString.TrimEnd(' ');
matrixString = matrixString.TrimEnd('\n');
if (matrix.RowCount != 1)
{
matrixString += "]";
}
return matrixString;
}
static string VectorToString(Vector<double> vector)
{
string vectorString = "[";
for (var i = 0; i < vector.Count; i++)
{
vectorString += String.Format("{0,9:G4}", vector[i]);
}
vectorString += "]";
return vectorString;
}
}
"""
Convert to/from Transformation Matrix (Rotation Matrix + Translation Vector).
Zivid primarily operate with a (4x4) transformation matrix. This example shows how to use Eigen to convert to and from:
AxisAngle, Rotation Vector, Roll-Pitch-Yaw, Quaternion.
Note: the translation part of the transformation matrix is in millimeters (mm), since Zivid point clouds are in
millimeters. If your robot reports translation in meters, multiply it by 1000 before saving the pose.
The convenience functions from this example can be reused in applicable applications. The YAML files for this sample
can be found under the main instructions for Zivid samples.
For more information on pose conversions, check out this article:
https://support.zivid.com/en/latest/camera/reference-articles/pose-conversions.html
"""
import enum
from dataclasses import dataclass, field
from pathlib import Path
from typing import Dict, List, Optional
import numpy as np
import zivid
from scipy.spatial.transform import Rotation as R
from zividsamples.paths import get_sample_data_path
from zividsamples.save_load_matrix import assert_affine_matrix_and_save, load_and_assert_affine_matrix
class RotationConvention(enum.Enum):
"""Convenience enum class to list rotation conventions for Roll Pitch Yaw."""
ZYX_INTRINSIC = "ZYX"
XYZ_EXTRINSIC = "xyz"
XYZ_INTRINSIC = "XYZ"
ZYX_EXTRINSIC = "zyx"
class AxisAngle:
"""Convenience class to access rotation axis and angle."""
axis: np.ndarray
angle: np.floating
def __init__(self, axis: np.ndarray = np.array([0, 0, 1]), angle: Optional[float] = None):
"""Initialize class and its variables.
Can be initialized with a unit vector and an angle, or only a rotation vector.
Args:
axis: Rotation axis
angle: Rotation angle
Raises:
ValueError: If angle vector is provided, but vector is not a unit vector
"""
self.axis = axis
if angle is None:
self.angle = np.linalg.norm(axis)
self.axis = axis / self.angle
elif np.linalg.norm(axis) != 0:
raise ValueError("Angle provided, but vector is not unit vector")
else:
self.angle: np.floating = np.floating(angle)
def as_rotvec(self) -> np.ndarray:
"""Return rotation vector from axis angle.
Returns:
Rotation vector
"""
return self.axis * self.angle
def as_quaternion(self) -> np.ndarray:
"""Return quaternion from axis angle.
Returns:
Quaternion
"""
return R.from_rotvec(self.as_rotvec()).as_quat()
@dataclass
class Representations:
"""Class to hold various transformation representations."""
axis_angle: AxisAngle = AxisAngle()
rotation_vector: np.ndarray = field(default_factory=lambda: np.zeros(3))
quaternion: np.ndarray = field(default_factory=lambda: np.zeros(4))
rotations: list = field(default_factory=list)
def rotation_matrix_to_axis_angle(rotation_matrix: np.ndarray) -> AxisAngle:
"""Convert from Rotation Matrix --> Axis Angle.
Args:
rotation_matrix: A numpy array (3x3)
Returns:
AxisAngle
"""
rotation = R.from_matrix(rotation_matrix)
return AxisAngle(rotation.as_rotvec())
def rotation_matrix_to_rotation_vector(rotation_matrix: np.ndarray) -> np.ndarray:
"""Convert from Rotation Matrix --> Rotation Vector.
Args:
rotation_matrix: A numpy array (3x3)
Returns:
Rotation Vector
"""
rotation = R.from_matrix(rotation_matrix)
return rotation.as_rotvec()
def rotation_matrix_to_quaternion(rotation_matrix: np.ndarray) -> np.ndarray:
"""Convert from Rotation Matrix --> Quaternion.
Args:
rotation_matrix: A numpy array (3x3)
Returns:
Quaternion
"""
rotation = R.from_matrix(rotation_matrix)
return rotation.as_quat()
def rotation_matrix_to_roll_pitch_yaw(rotation_matrix: np.ndarray) -> List[Dict]:
"""Convert from Rotation Matrix --> Roll Pitch Yaw.
Args:
rotation_matrix: A numpy array (3x3)
Returns:
rpy_list: List of Roll Pitch Yaw angles in radians
"""
rpy_list = []
rotation = R.from_matrix(rotation_matrix)
for convention in RotationConvention:
roll_pitch_yaw = rotation.as_euler(convention.value)
print(f"Roll-Pitch-Yaw angles ({convention.name}):")
print(f"{roll_pitch_yaw}")
rpy_list.append({"convention": convention, "roll_pitch_yaw": roll_pitch_yaw})
return rpy_list
def axis_angle_to_rotation_matrix(axis_angle: AxisAngle) -> np.ndarray:
"""Convert from AxisAngle --> Rotation Matrix.
Args:
axis_angle: An AxisAngle object with axis and angle
Returns:
Rotation Matrix (3x3 numpy array)
"""
return R.from_quat(axis_angle.as_quaternion()).as_matrix()
def rotation_vector_to_rotation_matrix(rotvec: np.ndarray) -> np.ndarray:
"""Convert from Rotation Vector --> Rotation Matrix.
Args:
rotvec: A 3x1 numpy array
Returns:
Rotation Matrix (3x3 numpy array)
"""
return R.from_rotvec(rotvec).as_matrix()
def quaternion_to_rotation_matrix(quaternion: np.ndarray) -> np.ndarray:
"""Convert from Quaternion --> Rotation Matrix.
Args:
quaternion: A 4x1 numpy array
Returns:
Rotation Matrix (3x3 numpy array)
"""
return R.from_quat(quaternion).as_matrix()
def roll_pitch_yaw_to_rotation_matrix(rpy_list: List[Dict]) -> None:
"""Convert from Roll Pitch Yaw --> Rotation Matrix.
Args:
rpy_list: List of Roll Pitch Yaw angles in radians
"""
for rotation in rpy_list:
rotation_matrix = R.from_euler(rotation["convention"].value, rotation["roll_pitch_yaw"]).as_matrix()
print(f"Rotation Matrix from Roll-Pitch-Yaw angles ({rotation['convention'].name}):")
print(f"{rotation_matrix}")
def print_header(txt: str) -> None:
"""Print decorated header.
Args:
txt: Text to be printed in header
"""
terminal_width = 70
print()
print(f"{'*' * terminal_width}")
print(f"* {txt} {' ' * (terminal_width - len(txt) - 4)}*")
print(f"{'*' * terminal_width}")
def _main() -> None:
# Application class must be initialized before using other Zivid classes.
app = zivid.Application() # noqa: F841 # pylint: disable=unused-variable
np.set_printoptions(precision=4, suppress=True)
print_header("This example shows conversions to/from Transformation Matrix")
transformation_matrix = load_and_assert_affine_matrix(get_sample_data_path() / "RobotTransform.yaml")
print(f"Transformation Matrix:\n{transformation_matrix}")
# Extract Rotation Matrix and Translation Vector from Transformation Matrix
rotation_matrix = transformation_matrix[:3, :3]
translation_vector = transformation_matrix[:-1, -1]
print(f"Rotation Matrix:\n{rotation_matrix}")
print(f"Translation Vector:\n{translation_vector}")
###
# Convert from Zivid to Robot (Transformation Matrix --> any format)
###
print_header("Convert from Zivid (Rotation Matrix) to Robot")
axis_angle = rotation_matrix_to_axis_angle(rotation_matrix)
print(f"AxisAngle:\n{axis_angle.axis}, {axis_angle.angle:.4f}")
rotation_vector = rotation_matrix_to_rotation_vector(rotation_matrix)
print(f"Rotation Vector:\n{rotation_vector}")
quaternion = rotation_matrix_to_quaternion(rotation_matrix)
print(f"Quaternion:\n{quaternion}")
rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)
###
# Convert from Robot to Zivid (any format --> Rotation Matrix (part of Transformation Matrix))
###
print_header("Convert from Robot to Zivid (Rotation Matrix)")
rotation_matrix_from_axis_angle = axis_angle_to_rotation_matrix(axis_angle)
print(f"Rotation Matrix from Axis Angle:\n{rotation_matrix_from_axis_angle}")
rotation_matrix_from_rotation_vector = rotation_vector_to_rotation_matrix(rotation_vector)
print(f"Rotation Matrix from Rotation Vector:\n{rotation_matrix_from_rotation_vector}")
rotation_matrix_from_quaternion = quaternion_to_rotation_matrix(quaternion)
print(f"Rotation Matrix from Quaternion:\n{rotation_matrix_from_quaternion}")
roll_pitch_yaw_to_rotation_matrix(rpy_list)
# Replace rotation matrix in transformation matrix
transformation_matrix_from_quaternion = np.eye(4)
transformation_matrix_from_quaternion[:3, :3] = rotation_matrix_from_quaternion
transformation_matrix_from_quaternion[:-1, -1] = translation_vector
# Save transformation matrix which has passed through quaternion representation
assert_affine_matrix_and_save(
transformation_matrix_from_quaternion, Path(__file__).parent / "RobotTransformOut.yaml"
)
if __name__ == "__main__":
_main()
潜在解决方案
将机器人控制器输出的机器人位姿转换为 Zivid 手眼标定所需的变换矩阵时，确保使用正确的旋转约定。
重新运行手眼标定（使用正确的数据集）。
验证手眼标定结果（变换矩阵、残差、投影）。
运行触碰测试（如果手眼标定结果良好）。
重现问题（旋转约定错误）
良好的数据集
使用下图所示的数据集成功进行了手眼标定。
标定结果如下：
0.9991385 |
-0.00262377 |
0.04141687 |
-55.73573 |
0.001416378 |
0.9995739 |
0.02915465 |
-91.3017 |
-0.04147572 |
-0.02907087 |
0.9987165 |
129.1951 |
0 |
0 |
0 |
1 |
N（位姿） |
平均旋转角度（°） |
最大旋转角度（°） |
平均平移残差（mm） |
最大平移残差（mm） |
|---|---|---|---|---|
14 |
0.028 |
0.068 |
0.133 |
0.278 |
全部的残差数据
位姿 |
旋转角度（°） |
平移量（mm） |
|---|---|---|
1 |
0.030 |
0.149 |
2 |
0.039 |
0.278 |
3 |
0.021 |
0.065 |
4 |
0.037 |
0.138 |
5 |
0.013 |
0.272 |
6 |
0.014 |
0.079 |
7 |
0.018 |
0.133 |
8 |
0.013 |
0.075 |
9 |
0.038 |
0.088 |
10 |
0.015 |
0.061 |
11 |
0.026 |
0.179 |
12 |
0.036 |
0.065 |
13 |
0.068 |
0.209 |
14 |
0.023 |
0.066 |
通过触碰测试验证了手眼标定。
触碰测试成功，证实了手眼标定的正确性和准确性。
错误数据集（旋转约定错误）
为了模拟使用错误旋转约定所导致的误差，我们对来自正确数据集的每个机器人位姿执行以下操作：
该变换矩阵已转换为 Roll-Pitch-Yaw 角度（假设采用 extrinsic fixed Z-Y-X）：
得到的横滚-俯仰-偏航角（数值角度相同）被转换回旋转矩阵形式，但使用了错误的约定（intrinsic z-y-x）：
平移向量保持不变。
标定结果如下：
-0.402 |
0.908 |
-0.117 |
134.552 |
-0.910 |
-0.411 |
-0.060 |
-440.257 |
-0.102 |
0.082 |
0.991 |
-706.041 |
0 |
0 |
0 |
1 |
N（位姿） |
平均旋转角度（°） |
最大旋转角度（°） |
平均平移残差（mm） |
最大平移残差（mm） |
|---|---|---|---|---|
14 |
10.764 |
20.647 |
99.307 |
176.708 |
全部的残差数据
位姿 |
旋转角度（°） |
平移量（mm） |
|---|---|---|
1 |
1.800 |
46.595 |
2 |
15.119 |
64.707 |
3 |
14.726 |
143.476 |
4 |
7.726 |
176.708 |
5 |
12.562 |
99.719 |
6 |
5.175 |
94.542 |
7 |
20.020 |
109.891 |
8 |
14.028 |
92.717 |
9 |
20.647 |
88.241 |
10 |
10.870 |
142.423 |
11 |
1.818 |
52.156 |
12 |
7.910 |
108.207 |
13 |
11.969 |
65.376 |
14 |
6.329 |
105.534 |
投影验证失败，这表明手眼标定不正确，触碰测试将会严重失败。
比较
下表比较了手眼标定矩阵（平移值）和手眼标定残差的结果。
数据集 |
X (mm) |
Y (mm) |
Z (mm) |
|---|---|---|---|
好 |
-55.73573 |
-91.3017 |
129.1951 |
差（错误旋转约定） |
134.552 |
-440.257 |
-706.041 |
Δ（好 - 差） |
-190.288 |
348.955 |
835.236 |
数据集 |
N |
平均旋转角度（°） |
最大旋转角度（°） |
平均平移残差（mm） |
最大平移残差（mm） |
|---|---|---|---|---|---|
好 |
14 |
0.028 |
0.068 |
0.133 |
0.278 |
差（错误旋转约定） |
14 |
10.764 |
20.647 |
99.307 |
176.708 |