如何运行和集成 Zivid 手眼标定
目标 |
推荐工具 |
|---|---|
引导式、无代码工作流程 |
|
最小集成示例 |
|
已有数据集 |
|
任何机器人 |
|
UR 机器人 |
|
使用 HALCON |
|
使用ROS |
手眼图形用户界面（推荐）
小技巧
如果您想要一个引导式、无需代码的工作流程，这是最佳选择。
对于希望进行手眼标定但又不想自行开发解决方案的用户，Zivid 推荐：
此图形化工具包含在我们的 Python sample library 中。它涵盖了所有基本功能，例如预热、现场标定、手眼标定和验证测试。虽然该工具支持与 RoboDK 集成，但这仅是可选项；该 GUI 可独立用于 Zivid 相机的功能。
如果您更喜欢不同的工作流程或与系统更紧密的集成，我们提供多种替代方案。
RoboDK + Python（与机器人无关）
如果您更喜欢使用 RoboDK 和 Python 编写脚本来执行此过程，而不是使用 GUI，请参阅：
特征：
适用于任何 RoboDK-supported robots
捕获位姿是在 .rdk 文件中手动定义的
全自动机器人控制
Universal Robots UR5 + Python
对于希望直接使用 UR5 驱动程序而不是 RoboDK 或 GUI 的用户，请参阅：
特征：
专为UR机器人设计
全自动机器人控制
手眼标定 CLI 工具
对于偏好不基于 Python 或依赖项最少的工作流程的用户，请参阅：
这款实验性的命令行工具可根据提供的数据集计算手眼变换，并将变换矩阵和残差保存到用户指定的文件中。如果您有以下需求，请使用此工具：
已有数据集（机器人位姿+点云）
想要一个命令行、批处理式的工作流程
安装配置如下：
适用于 Windows 的 Zivid 安装程序
适用于Ubuntu的
toolsdeb 软件包
程序化手眼标定
如果您希望进行更定制化的集成，例如将手眼标定 API 直接嵌入到您自己的解决方案中，请按照以下步骤操作。如果您符合以下条件，请使用此方法：
想要最简单的集成示例
正在构建自己的标定流程
工作流程：
用户以 4x4 变换矩阵的形式输入机器人位姿（手动输入）
相机捕获标定对象
用户将机器人移动到新的捕获位姿，并输入命令以添加新位姿
前三个步骤重复进行（通常为 10-20 对位姿）
用户输入标定命令后，应用程序返回一个手眼变换矩阵
要了解如何将手眼标定集成到您的解决方案中，请参阅下面的交互式代码示例。
代码示例
/*
Perform Hand-Eye calibration.
For more information on Hand-Eye calibration, check out this tutorial:
https://support.zivid.com/en/latest/camera/academy/applications/hand-eye.html
*/
#include <Zivid/Application.h>
#include <Zivid/Calibration/Detector.h>
#include <Zivid/Calibration/HandEye.h>
#include <Zivid/Calibration/Pose.h>
#include <Zivid/Exception.h>
#include <Zivid/Zivid.h>
#include <clipp.h>
#include <iostream>
#include <stdexcept>
namespace
{
std::string presetPath(const Zivid::Camera &camera)
{
const std::string presetsPath = std::string(ZIVID_SAMPLE_DATA_DIR) + "/Settings";
switch(camera.info().model().value())
{
case Zivid::CameraInfo::Model::ValueType::zividTwo:
{
return presetsPath + "/Zivid_Two_M70_ManufacturingSpecular.yml";
}
case Zivid::CameraInfo::Model::ValueType::zividTwoL100:
{
return presetsPath + "/Zivid_Two_L100_ManufacturingSpecular.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid2PlusM130:
{
return presetsPath + "/Zivid_Two_Plus_M130_ConsumerGoodsQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid2PlusM60:
{
return presetsPath + "/Zivid_Two_Plus_M60_ConsumerGoodsQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid2PlusL110:
{
return presetsPath + "/Zivid_Two_Plus_L110_ConsumerGoodsQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid2PlusMR130:
{
return presetsPath + "/Zivid_Two_Plus_MR130_ConsumerGoodsQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid2PlusMR60:
{
return presetsPath + "/Zivid_Two_Plus_MR60_ConsumerGoodsQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid2PlusLR110:
{
return presetsPath + "/Zivid_Two_Plus_LR110_ConsumerGoodsQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zivid3XL250:
{
return presetsPath + "/Zivid_Three_XL250_DepalletizationQuality.yml";
}
case Zivid::CameraInfo::Model::ValueType::zividOnePlusSmall:
case Zivid::CameraInfo::Model::ValueType::zividOnePlusMedium:
case Zivid::CameraInfo::Model::ValueType::zividOnePlusLarge: break;
default: throw std::runtime_error("Unhandled enum value '" + camera.info().model().toString() + "'");
}
throw std::invalid_argument("Invalid camera model");
}
enum class CommandType
{
AddPose,
Calibrate,
Unknown
};
std::string getInput()
{
std::string command;
std::getline(std::cin, command);
return command;
}
CommandType enterCommand()
{
std::cout << "Enter command, p (to add robot pose) or c (to perform calibration): ";
const auto command = getInput();
if(command == "P" || command == "p")
{
return CommandType::AddPose;
}
if(command == "C" || command == "c")
{
return CommandType::Calibrate;
}
return CommandType::Unknown;
}
Zivid::Calibration::Pose enterRobotPose(size_t index)
{
std::cout << "Enter pose with id (a line with 16 space separated values describing 4x4 row-major matrix) : "
<< index << std::endl;
std::stringstream input(getInput());
float element{ 0 };
std::vector<float> transformElements;
for(size_t i = 0; i < 16 && input >> element; ++i)
{
transformElements.emplace_back(element);
}
const auto robotPose{ Zivid::Matrix4x4{ transformElements.cbegin(), transformElements.cend() } };
std::cout << "The following pose was entered: \n" << robotPose << std::endl;
return robotPose;
}
std::string markersToString(const std::vector<int> &markerIds)
{
std::ostringstream oss;
for(const auto &id : markerIds)
{
oss << id << " ";
}
return oss.str();
}
void handleAddPose(
size_t ¤tPoseId,
std::vector<Zivid::Calibration::HandEyeInput> &handEyeInput,
Zivid::Camera &camera,
const std::string &calibrationObject,
const Zivid::Settings &settings)
{
const auto robotPose = enterRobotPose(currentPoseId);
std::cout << "Detecting calibration object in point cloud" << std::endl;
if(calibrationObject == "c")
{
const auto frame = camera.capture2D3D(settings);
const auto detectionResult = Zivid::Calibration::detectCalibrationBoard(frame);
if(detectionResult.valid())
{
std::cout << "Calibration board detected " << std::endl;
handEyeInput.emplace_back(robotPose, detectionResult);
currentPoseId++;
}
else
{
std::cout << "Failed to detect calibration board. " << detectionResult.statusDescription() << std::endl;
}
}
else if(calibrationObject == "m")
{
const auto frame = camera.capture2D3D(settings);
auto markerDictionary = Zivid::Calibration::MarkerDictionary::aruco4x4_50;
std::vector<int> markerIds = { 1, 2, 3 };
std::cout << "Detecting arUco marker IDs " << markersToString(markerIds) << "from the dictionary "
<< markerDictionary << std::endl;
auto detectionResult = Zivid::Calibration::detectMarkers(frame, markerIds, markerDictionary);
if(detectionResult.valid())
{
std::cout << "ArUco marker(s) detected: " << detectionResult.detectedMarkers().size() << std::endl;
handEyeInput.emplace_back(robotPose, detectionResult);
currentPoseId++;
}
else
{
std::cout
<< "Failed to detect any ArUco markers, ensure that at least one ArUco marker is in the view of the camera"
<< std::endl;
}
}
}
std::vector<Zivid::Calibration::HandEyeInput> readHandEyeInputs(
Zivid::Camera &camera,
const Zivid::Settings &settings)
{
size_t currentPoseId{ 0 };
bool calibrate{ false };
std::string calibrationObject;
while(true)
{
std::cout
<< "Enter calibration object you are using, m (for ArUco marker(s)) or c (for Zivid checkerboard): "
<< std::endl;
calibrationObject = getInput();
if(calibrationObject == "m" || calibrationObject == "c")
{
break;
}
}
std::cout << "Zivid primarily operates with a (4x4) transformation matrix. To convert" << std::endl;
std::cout << "from axis-angle, rotation vector, roll-pitch-yaw, or quaternion, check out" << std::endl;
std::cout << "our PoseConversions sample." << std::endl;
std::vector<Zivid::Calibration::HandEyeInput> handEyeInput;
do
{
switch(enterCommand())
{
case CommandType::AddPose:
{
try
{
handleAddPose(currentPoseId, handEyeInput, camera, calibrationObject, settings);
}
catch(const std::exception &e)
{
std::cout << "Error: " << Zivid::toString(e) << std::endl;
continue;
}
break;
}
case CommandType::Calibrate:
{
calibrate = true;
break;
}
case CommandType::Unknown:
{
std::cout << "Error: Unknown command" << std::endl;
break;
}
default: throw std::runtime_error{ "Unhandled command type" };
}
} while(!calibrate);
return handEyeInput;
}
Zivid::Calibration::HandEyeOutput performCalibration(
const std::vector<Zivid::Calibration::HandEyeInput> &handEyeInput)
{
while(true)
{
std::cout << "Enter type of calibration, eth (for eye-to-hand) or eih (for eye-in-hand): ";
const auto calibrationType = getInput();
if(calibrationType == "eth" || calibrationType == "ETH")
{
std::cout << "Performing eye-to-hand calibration with " << handEyeInput.size() << " dataset pairs"
<< std::endl;
std::cout << "The resulting transform is the camera pose in robot base frame" << std::endl;
return Zivid::Calibration::calibrateEyeToHand(handEyeInput);
}
if(calibrationType == "eih" || calibrationType == "EIH")
{
std::cout << "Performing eye-in-hand calibration with " << handEyeInput.size() << " dataset pairs"
<< std::endl;
std::cout << "The resulting transform is the camera pose in flange (end-effector) frame" << std::endl;
return Zivid::Calibration::calibrateEyeInHand(handEyeInput);
}
std::cout << "Entered uknown method" << std::endl;
}
}
} // namespace
int main(int argc, char *argv[])
{
try
{
std::string settingsPath;
bool showHelp = false;
auto cli =
(clipp::option("-h", "--help").set(showHelp) % "Show help message",
clipp::option("--settings-path")
& clipp::value("path", settingsPath) % "Path to the camera settings YML file");
if(!clipp::parse(argc, argv, cli) || showHelp)
{
auto fmt = clipp::doc_formatting{}.alternatives_min_split_size(1).surround_labels("\"", "\"");
std::cout << "USAGE:" << std::endl;
std::cout << clipp::usage_lines(cli, argv[0], fmt) << std::endl;
std::cout << "OPTIONS:" << std::endl;
std::cout << clipp::documentation(cli) << std::endl;
return showHelp ? EXIT_SUCCESS : EXIT_FAILURE;
}
Zivid::Application zivid;
std::cout << "Connecting to camera" << std::endl;
auto camera{ zivid.connectCamera() };
if(settingsPath.empty())
{
settingsPath = presetPath(camera);
}
const auto settings = Zivid::Settings(settingsPath);
const auto handEyeInput{ readHandEyeInputs(camera, settings) };
const auto calibrationResult{ performCalibration(handEyeInput) };
std::cout << "Zivid primarily operates with a (4x4) transformation matrix. To convert" << std::endl;
std::cout << "to axis-angle, rotation vector, roll-pitch-yaw, or quaternion, check out" << std::endl;
std::cout << "our PoseConversions sample." << std::endl;
if(calibrationResult.valid())
{
std::cout << "Hand-Eye calibration OK\n"
<< "Result:\n"
<< calibrationResult << std::endl;
}
else
{
std::cout << "Hand-Eye calibration FAILED" << std::endl;
return EXIT_FAILURE;
}
}
catch(const std::exception &e)
{
std::cerr << "\nError: " << Zivid::toString(e) << std::endl;
std::cout << "Press enter to exit." << std::endl;
std::cin.get();
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
/*
Perform Hand-Eye calibration.
For more information on Hand-Eye calibration, check out this tutorial:
https://support.zivid.com/en/latest/camera/academy/applications/hand-eye.html
*/
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using Zivid.NET.Calibration;
using Duration = Zivid.NET.Duration;
class Program
{
static int Main(string[] args)
{
try
{
var userOptions = ParseOptions(args);
if (userOptions.ShowHelp)
{
ShowHelp();
return 0;
}
var zivid = new Zivid.NET.Application();
Console.WriteLine("Connecting to camera");
var camera = zivid.ConnectCamera();
var settingsPath = userOptions.SettingsPath;
if (string.IsNullOrEmpty(settingsPath))
{
settingsPath = PresetPath(camera);
}
var settings = new Zivid.NET.Settings(settingsPath);
var handEyeInput = ReadHandEyeInputs(camera, settings);
var calibrationResult = PerformCalibration(handEyeInput);
Console.WriteLine("Zivid primarily operates with a (4x4) transformation matrix. To convert");
Console.WriteLine("to axis-angle, rotation vector, roll-pitch-yaw, or quaternion, check out");
Console.WriteLine("our PoseConversions sample.");
if (calibrationResult.Valid())
{
Console.WriteLine("{0}\n{1}\n{2}", "Hand-Eye calibration OK", "Result: ", calibrationResult);
}
else
{
Console.WriteLine("Hand-Eye calibration FAILED");
return 1;
}
}
catch (Exception ex)
{
Console.WriteLine("Error: {0}", ex.ToString());
return 1;
}
return 0;
}
static (string SettingsPath, bool ShowHelp) ParseOptions(string[] args)
{
string settingsPath = "";
bool showHelp = false;
foreach (var arg in args)
{
if (arg.StartsWith("--settings-path="))
{
settingsPath = arg.Substring("--settings-path=".Length);
}
else if (arg.StartsWith("-h") || arg.StartsWith("--help"))
{
showHelp = true;
}
}
return (settingsPath, showHelp);
}
static List<HandEyeInput> ReadHandEyeInputs(Zivid.NET.Camera camera, Zivid.NET.Settings settings)
{
var handEyeInput = new List<HandEyeInput>();
var currentPoseId = 0U;
var beingInput = true;
var calibrationObject = "";
while (true)
{
Console.WriteLine("Enter calibration object you are using, m (for ArUco marker(s)) or c (for Zivid checkerboard): ");
calibrationObject = Console.ReadLine();
if (calibrationObject.Equals("m", StringComparison.CurrentCultureIgnoreCase) ||
calibrationObject.Equals("c", StringComparison.CurrentCultureIgnoreCase))
{
break;
}
}
Interaction.ExtendInputBuffer(2048);
Console.WriteLine("Zivid primarily operates with a (4x4) transformation matrix. To convert");
Console.WriteLine("from axis-angle, rotation vector, roll-pitch-yaw, or quaternion, check out");
Console.WriteLine("our PoseConversions sample.");
do
{
switch (Interaction.EnterCommand())
{
case CommandType.AddPose:
try
{
HandleAddPose(ref currentPoseId, ref handEyeInput, camera, calibrationObject, settings);
}
catch (Exception ex)
{
Console.WriteLine("Error: {0}", ex.ToString());
continue;
}
break;
case CommandType.Calibrate: beingInput = false; break;
case CommandType.Unknown: Console.WriteLine("Error: Unknown command"); break;
}
} while (beingInput);
return handEyeInput;
}
public static void HandleAddPose(ref uint currentPoseId, ref List<HandEyeInput> handEyeInput, Zivid.NET.Camera camera, string calibrationObject, Zivid.NET.Settings settings)
{
var robotPose = Interaction.EnterRobotPose(currentPoseId);
Console.Write("Detecting calibration object in point cloud");
if (calibrationObject.Equals("c", StringComparison.CurrentCultureIgnoreCase))
{
using (var frame = camera.Capture2D3D(settings))
{
var detectionResult = Detector.DetectCalibrationBoard(frame);
if (detectionResult.Valid())
{
Console.WriteLine("Calibration board detected");
handEyeInput.Add(new HandEyeInput(robotPose, detectionResult));
++currentPoseId;
}
else
{
Console.WriteLine("Failed to detect calibration board, ensure that the entire board is in the view of the camera");
}
}
}
else if (calibrationObject.Equals("m", StringComparison.CurrentCultureIgnoreCase))
{
using (var frame = camera.Capture2D3D(settings))
{
var markerDictionary = Zivid.NET.MarkerDictionary.Aruco4x4_50;
var markerIds = new List<int> { 1, 2, 3 };
Console.WriteLine("Detecting arUco marker IDs " + string.Join(", ", markerIds));
var detectionResult = Detector.DetectMarkers(frame, markerIds, markerDictionary);
if (detectionResult.Valid())
{
Console.WriteLine("ArUco marker(s) detected: " + detectionResult.DetectedMarkers().Length);
handEyeInput.Add(new HandEyeInput(robotPose, detectionResult));
++currentPoseId;
}
else
{
Console.WriteLine("Failed to detect any ArUco markers, ensure that at least one ArUco marker is in the view of the camera");
}
}
}
}
static string PresetPath(Zivid.NET.Camera camera)
{
var presetsPath = Environment.GetFolderPath(Environment.SpecialFolder.CommonApplicationData)
+ "/Zivid/Settings/";
switch (camera.Info.Model)
{
case Zivid.NET.CameraInfo.ModelOption.ZividTwo:
{
return presetsPath + "Zivid_Two_M70_ManufacturingSpecular.yml";
}
case Zivid.NET.CameraInfo.ModelOption.ZividTwoL100:
{
return presetsPath + "Zivid_Two_L100_ManufacturingSpecular.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid2PlusM130:
{
return presetsPath + "Zivid_Two_Plus_M130_ConsumerGoodsQuality.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid2PlusM60:
{
return presetsPath + "Zivid_Two_Plus_M60_ConsumerGoodsQuality.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid2PlusL110:
{
return presetsPath + "Zivid_Two_Plus_L110_ConsumerGoodsQuality.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid2PlusMR130:
{
return presetsPath + "Zivid_Two_Plus_MR130_ConsumerGoodsQuality.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid2PlusMR60:
{
return presetsPath + "Zivid_Two_Plus_MR60_ConsumerGoodsQuality.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid2PlusLR110:
{
return presetsPath + "Zivid_Two_Plus_LR110_ConsumerGoodsQuality.yml";
}
case Zivid.NET.CameraInfo.ModelOption.Zivid3XL250:
{
return presetsPath + "Zivid_Three_XL250_DepalletizationQuality.yml";
}
default: throw new System.InvalidOperationException("Unhandled camera model: " + camera.Info.Model.ToString());
}
throw new System.InvalidOperationException("Invalid camera model");
}
static Zivid.NET.Calibration.HandEyeOutput PerformCalibration(List<HandEyeInput> handEyeInput)
{
while (true)
{
Console.WriteLine("Enter type of calibration, eth (for eye-to-hand) or eih (for eye-in-hand): ");
var calibrationType = Console.ReadLine();
if (calibrationType.Equals("eth", StringComparison.CurrentCultureIgnoreCase))
{
Console.WriteLine("Performing eye-to-hand calibration with " + handEyeInput.Count + " dataset pairs");
Console.WriteLine("The resulting transform is the camera pose in robot base frame");
return Calibrator.CalibrateEyeToHand(handEyeInput);
}
if (calibrationType.Equals("eih", StringComparison.CurrentCultureIgnoreCase))
{
Console.WriteLine("Performing eye-in-hand calibration with " + handEyeInput.Count + " dataset pairs");
Console.WriteLine("The resulting transform is the camera pose in flange (end-effector) frame");
return Calibrator.CalibrateEyeInHand(handEyeInput);
}
Console.WriteLine("Entered unknown method");
}
}
static void ShowHelp()
{
Console.WriteLine("Usage: HandEyeCalibration.exe [options]");
Console.WriteLine("Options:");
Console.WriteLine(" --settings-path=<path> Path to the camera settings YML file (default: based on camera model)");
Console.WriteLine(" -h, --help Show this help message");
}
}
enum CommandType
{
AddPose,
Calibrate,
Unknown
}
class Interaction
{
// Console.ReadLine only supports reading 256 characters, by default. This limit is modified
// by calling ExtendInputBuffer with the maximum length of characters to be read.
public static void ExtendInputBuffer(int size)
{
Console.SetIn(new StreamReader(Console.OpenStandardInput(), Console.InputEncoding, false, size));
}
public static CommandType EnterCommand()
{
Console.Write("Enter command, p (to add robot pose) or c (to perform calibration): ");
var command = Console.ReadLine().ToLower();
switch (command)
{
case "p": return CommandType.AddPose;
case "c": return CommandType.Calibrate;
default: return CommandType.Unknown;
}
}
public static Pose EnterRobotPose(ulong index)
{
var elementCount = 16;
Console.WriteLine(
"Enter pose with id (a line with {0} space separated values describing 4x4 row-major matrix) : {1}",
elementCount,
index);
var input = Console.ReadLine();
var elements = input.Split().Where(x => !string.IsNullOrEmpty(x.Trim())).Select(x => float.Parse(x)).ToArray();
var robotPose = new Pose(elements); Console.WriteLine("The following pose was entered: \n{0}", robotPose);
return robotPose;
}
}
"""
Perform Hand-Eye calibration.
For more information on Hand-Eye calibration, check out this tutorial:
https://support.zivid.com/en/latest/camera/academy/applications/hand-eye.html
"""
import argparse
from pathlib import Path
from typing import List, Tuple
import numpy as np
import zivid
from zividsamples.paths import get_sample_data_path
from zividsamples.save_load_matrix import assert_affine_matrix_and_save
def _options() -> argparse.Namespace:
"""Function to read user arguments.
Returns:
Arguments from user
"""
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--settings-path",
required=False,
type=Path,
help="Path to the camera settings YML file",
)
return parser.parse_args()
def _preset_path(camera: zivid.Camera) -> Path:
"""Get path to preset settings YML file, depending on camera model.
Args:
camera: Zivid camera
Raises:
ValueError: If unsupported camera model for this code sample
Returns:
Path: Zivid 2D and 3D settings YML path
"""
presets_path = get_sample_data_path() / "Settings"
if camera.info.model == zivid.CameraInfo.Model.zivid3XL250:
return presets_path / "Zivid_Three_XL250_DepalletizationQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zivid2PlusMR60:
return presets_path / "Zivid_Two_Plus_MR60_ConsumerGoodsQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zivid2PlusMR130:
return presets_path / "Zivid_Two_Plus_MR130_ConsumerGoodsQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zivid2PlusLR110:
return presets_path / "Zivid_Two_Plus_LR110_ConsumerGoodsQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zivid2PlusM60:
return presets_path / "Zivid_Two_Plus_M60_ConsumerGoodsQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zivid2PlusM130:
return presets_path / "Zivid_Two_Plus_M130_ConsumerGoodsQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zivid2PlusL110:
return presets_path / "Zivid_Two_Plus_L110_ConsumerGoodsQuality.yml"
if camera.info.model == zivid.CameraInfo.Model.zividTwo:
return presets_path / "Zivid_Two_M70_ManufacturingSpecular.yml"
if camera.info.model == zivid.CameraInfo.Model.zividTwoL100:
return presets_path / "Zivid_Two_L100_ManufacturingSpecular.yml"
raise ValueError("Invalid camera model")
def _enter_robot_pose(index: int) -> zivid.calibration.Pose:
"""Robot pose user input.
Args:
index: Robot pose ID
Returns:
robot_pose: Robot pose
"""
inputted = input(
f"Enter pose with id={index} (a line with 16 space separated values describing 4x4 row-major matrix): "
)
elements = inputted.split(maxsplit=15)
data = np.array(elements, dtype=np.float64).reshape((4, 4))
robot_pose = zivid.calibration.Pose(data)
print(f"The following pose was entered:\n{robot_pose}")
return robot_pose
def _perform_calibration(hand_eye_input: List[zivid.calibration.HandEyeInput]) -> zivid.calibration.HandEyeOutput:
"""Hand-Eye calibration type user input.
Args:
hand_eye_input: Hand-Eye calibration input
Returns:
hand_eye_output: Hand-Eye calibration result
"""
while True:
calibration_type = input("Enter type of calibration, eth (for eye-to-hand) or eih (for eye-in-hand): ").strip()
if calibration_type.lower() == "eth":
print(f"Performing eye-to-hand calibration with {len(hand_eye_input)} dataset pairs")
print("The resulting transform is the camera pose in robot base frame")
hand_eye_output = zivid.calibration.calibrate_eye_to_hand(hand_eye_input)
return hand_eye_output
if calibration_type.lower() == "eih":
print(f"Performing eye-in-hand calibration with {len(hand_eye_input)} dataset pairs")
print("The resulting transform is the camera pose in flange (end-effector) frame")
hand_eye_output = zivid.calibration.calibrate_eye_in_hand(hand_eye_input)
return hand_eye_output
print(f"Unknown calibration type: '{calibration_type}'")
def _handle_add_pose(
current_pose_id: int,
hand_eye_input: List,
camera: zivid.Camera,
calibration_object: str,
settings: zivid.Settings,
) -> Tuple[int, List]:
"""Acquire frame and keeps track of the robot's pose id.
Args:
current_pose_id: Counter of the current pose in the hand-eye calibration dataset
hand_eye_input: List of hand-eye calibration dataset pairs (poses and point clouds)
camera: Zivid camera
calibration_object: m (for ArUco marker(s)) or c (for Zivid checkerboard)
settings: Zivid camera settings
Returns:
Tuple[int, List]: Updated current_pose_id and hand_eye_input
"""
robot_pose = _enter_robot_pose(current_pose_id)
print("Detecting calibration object in point cloud")
if calibration_object == "c":
frame = zivid.calibration.capture_calibration_board(camera)
detection_result = zivid.calibration.detect_calibration_board(frame)
if detection_result.valid():
print("Calibration board detected")
hand_eye_input.append(zivid.calibration.HandEyeInput(robot_pose, detection_result))
current_pose_id += 1
else:
print(f"Failed to detect calibration board. {detection_result.status_description()}")
elif calibration_object == "m":
frame = camera.capture_2d_3d(settings)
marker_dictionary = zivid.calibration.MarkerDictionary.aruco4x4_50
marker_ids = [1, 2, 3]
print(f"Detecting arUco marker IDs {marker_ids} from the dictionary {marker_dictionary}")
detection_result = zivid.calibration.detect_markers(frame, marker_ids, marker_dictionary)
if detection_result.valid():
print(f"ArUco marker(s) detected: {len(detection_result.detected_markers())}")
hand_eye_input.append(zivid.calibration.HandEyeInput(robot_pose, detection_result))
current_pose_id += 1
else:
print(
"Failed to detect any ArUco markers, ensure that at least one ArUco marker is in the view of the camera"
)
return current_pose_id, hand_eye_input
def _main() -> None:
user_options = _options()
app = zivid.Application()
print("Connecting to camera")
camera = app.connect_camera()
if user_options.settings_path is None:
user_options.settings_path = _preset_path(camera)
settings = zivid.Settings.load(user_options.settings_path)
current_pose_id = 0
hand_eye_input = []
calibrate = False
while True:
calibration_object = input(
"Enter calibration object you are using, m (for ArUco marker(s)) or c (for Zivid checkerboard): "
).strip()
if calibration_object.lower() == "m" or calibration_object.lower() == "c":
break
print(
"Zivid primarily operates with a (4x4) transformation matrix. To convert\n"
"from axis-angle, rotation vector, roll-pitch-yaw, or quaternion, check out\n"
"our pose_conversions sample."
)
while not calibrate:
command = input("Enter command, p (to add robot pose) or c (to perform calibration): ").strip()
if command == "p":
try:
current_pose_id, hand_eye_input = _handle_add_pose(
current_pose_id, hand_eye_input, camera, calibration_object, settings
)
except ValueError as ex:
print(ex)
elif command == "c":
calibrate = True
else:
print(f"Unknown command '{command}'")
calibration_result = _perform_calibration(hand_eye_input)
transform = calibration_result.transform()
transform_file_path = Path(Path(__file__).parent / "transform.yaml")
assert_affine_matrix_and_save(transform, transform_file_path)
print(
"Zivid primarily operates with a (4x4) transformation matrix. To convert\n"
"to axis-angle, rotation vector, roll-pitch-yaw, or quaternion, check out\n"
"our pose_conversions sample."
)
if calibration_result.valid():
print("Hand-Eye calibration OK")
print(f"Result:\n{calibration_result}")
else:
print("Hand-Eye calibration FAILED")
if __name__ == "__main__":
_main()
备注
Zivid 点云数据的单位为毫米（mm）；因此，请确保手眼标定中使用的机器人输入位姿在平移部分也采用毫米（mm）作为单位。
以下章节对于处理不同机器人控制器所使用的各种位姿表示方法（例如欧拉角、四元数或轴角）特别有用。
辅助工具：位姿转换
Zivid 主要使用 (4x4) 变换矩阵（旋转矩阵 + 平移向量）。机器人控制器和软件通常将位姿表示为位置向量和方向向量的组合，并采用多种格式之一。位姿转换工具可帮助实现以下格式之间的相互转换：
轴-角（Axis-Angle）
旋转矢量（Rotation Vector）
横滚-俯仰-偏航（Roll-Pitch-Yaw）
欧拉角（Euler Angles）
四元数（Quaternion）
我们提供几种用于机器人位姿转换的工具：
Mathematical conversions (including robot-specific pose conventions for several robot brands)
代码示例（C++、C#、Python）
代码示例
/*
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()
使用 HALCON 进行手眼标定
也可以使用 HALCON 进行手眼标定。这种方法对于已经在工作流程中使用 HALCON 并希望使用 HALCON 提供的标定工具的用户尤其有用。基本原理类似，因此大多数建议仍然适用。主要区别在于使用了不同的标定对象（3D 标定对象或 HALCON 标定板）和 HDevelop 脚本语言。更多信息，请参阅教程：
以及我们 HALCON sample library 中的以下代码示例：
示例数据集
如果想在没有自己的数据集的情况下测试手眼标定，可以使用我们的示例数据中的数据集：
Zivid 手眼标定： Zivid Sample Dataset
Halcon 手眼标定： Halcon Sample Dataset
Sample Data（示例数据） 解释了从哪里提取数据集以供代码示例使用。
使用 ROS 进行手眼标定
对于需要在 ROS2 应用中集成手眼标定的用户，请参阅 zivid-ros hand-eye sample 。zivid-ros 仓库中的示例演示了基于服务的完整工作流程：
启动手眼标定会话并选择标定对象类型
通过向捕获服务提供机器人位姿来捕获多组数据
运行标定程序以计算最终的手眼变换矩阵
可选的工作目录可用于保存采集的 frame 和机器人位姿，以便后续复用。
备注
Zivid 点云数据的单位为毫米（mm）；因此，请确保手眼标定中使用的机器人输入位姿在平移部分也采用毫米（mm）作为单位。
现在您已确认了将要使用的手眼标定方法，接下来请查看 如何获取用于手眼标定的优质数据集 。