터치 테스트는 핸드-아이 캘리브레이션불량 (잘못된 회전 규칙)으로 인해 수행하기에 안전하지 않습니다.
문제
핸드-아이 캘리브레이션 결과가 좋지 않아 터치 테스트를 진행하기에 안전하지 않습니다.
핸드-아이 캘리브레이션 변환 |
핸드-아이 캘리브레이션 잔차 |
프로젝션 검증 |
터치 테스트 |
|---|---|---|---|
나쁜 |
나쁜 |
나쁜 |
위험한 |
잘못된 핸드-아이 캘리브레이션 결과의 예
핸드-아이 캘리브레이션 행렬은 여러 값이 물리적 측정값과 지나치게 큰 차이를 보이므로 정확하지 않은 것으로 보입니다. 이러한 차이는 해당 설정에서 합리적이지 않습니다.
-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 값은 대략 130mm이고, -440.256 값은 대략 90mm입니다. 이 단계에서는 부호(+/-)는 좌표계 정의에 따라 달라지므로 무시하고, 거리 자체에만 집중합니다.
보정 잔차 또한 예상보다 상당히 크며, 두 자릿수 정도 더 높습니다.
N (포즈) |
회전 평균 (°) |
최대 회전각(°) |
이동 평균(mm) |
최대 이동 거리(mm) |
|---|---|---|---|---|
14 |
10.764 |
20.647 |
99.307 |
176.708 |
소형 로봇의 경우 잔차가 1mm 미만 범위에 있을 것으로 예상되며, 대형 로봇의 경우 잔차가 1mm 범위에 있을 수 있습니다. 잔차의 회전 성분은 1도 미만 범위에 있을 것으로 예상됩니다.
전체 잔차
포즈 |
회전 (°) |
이동 (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) 변환 행렬(회전 행렬 + 이동 벡터)을 사용합니다. 로봇 컨트롤러와 소프트웨어는 일반적으로 다양한 형식 중 하나로 위치 벡터와 방향 표현을 조합하여 포즈를 나타냅니다. 포즈 변환 도구를 사용하면 다음과 같은 형식으로 변환할 수 있습니다.
축-각도
회전 벡터
롤-피치-요
오일러 각
쿼터니언
저희는 로봇 자세 변환을 위한 몇 가지 도구를 제공합니다.
코드 샘플
/*
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 |
핸드-아이 캘리브레이션은 터치 테스트를 통해 검증되었습니다.
터치 테스트가 성공적으로 완료되어 핸드-아이 캘리브레이션이 정확하고 정밀하게 조정되었음을 확인했습니다.
잘못된 데이터셋 (잘못된 회전 규칙)
잘못된 회전 규칙을 사용하는 오류를 시뮬레이션하기 위해, 정상적인 데이터 세트의 각 로봇 포즈에 대해 다음과 같은 작업을 수행했습니다.
외부 고정 ZYX를 가정하여 변환 행렬을 롤-피치-요 각도로 변환했습니다.
결과적으로 얻어진 롤-피치-요 각도(동일한 수치 각도)는 변환 행렬 형태로 다시 변환되었지만, 잘못된 규칙(내재적 zyx)을 사용했습니다.
이동 벡터는 변경되지 않았습니다.
보정 결과는 다음과 같습니다.
-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 |