Zivid 핸드-아이 캘리브레이션 실행 및 통합 방법
목표 |
추천 도구 |
|---|---|
가이드 방식의 코드 없는 워크플로우 |
|
최소제곱법 예시 |
|
기존 데이터 세트 |
|
어떤 로봇 |
|
UR 로봇 |
|
HALCON 사용 |
|
ROS 사용 |
핸드-아이 GUI (권장)
팁
가이드가 제공되는 코딩 필요 없는 워크플로우를 원한다면 최고의 선택입니다.
자체 솔루션을 개발하지 않고 핸드-아이 캘리브레이션을 수행하려는 사용자를 위해 Zivid는 다음을 권장합니다.
이 그래픽 도구는 Python sample library 의 일부로 제공됩니다. 여기에는 워밍업, 현장 보정, 캘리브레이션 및 검증 테스트와 같은 모든 필수 기능이 포함되어 있습니다. 이 도구는 RoboDK와의 통합을 지원하지만, 사용은 선택 사항입니다. GUI는 Zivid 카메라 관련 기능을 위해 독립적으로 사용할 수 있습니다.
다른 워크플로우를 선호하거나 시스템과의 더욱 긴밀한 통합을 원하시는 경우, 여러 대안이 있습니다.
RoboDK + Python (로봇 종류에 관계 없음)
GUI를 사용하는 대신 RoboDK와 Python을 사용하여 스크립트를 작성하는 것을 선호하는 사용자는 다음을 참조하십시오.
특징:
RoboDK-supported robots 과 호환됩니다.
촬영 자세는 .rdk 파일에서 수동으로 정의됩니다.
완전 자동 로봇 제어
유니버설 로봇 UR5 + Python
RoboDK나 GUI 대신 UR5 드라이버를 직접 사용하려는 사용자는 다음을 참조하십시오.
특징:
UR 로봇 전용으로 설계되었습니다.
완전 자동 로봇 제어
핸드-아이 캘리브레이션 CLI 도구
Python 기반이 아니거나 최소한의 의존성만 사용하는 워크플로를 선호하는 사용자는 다음을 참조하세요.
이 실험적인 CLI 도구는 제공된 데이터 세트에서 핸드-아이 캘리브레이션을 계산하고 변환 행렬과 잔차를 사용자가 지정한 파일에 저장합니다. 다음과 같은 경우에 사용하십시오.
이미 데이터셋(로봇 포즈 + 포인트 클라우드)을 보유하고 있습니다.
명령줄 방식의 배치 처리 워크플로우를 원하십니까?
설치 방법:
Windows Zivid 설치 프로그램
toolsUbuntu 에서의 deb 패키지
프로그램 방식 핸드-아이 캘리브레이션
보다 맞춤화된 통합, 예를 들어 핸드-아이 캘리브레이션 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 단위를 사용해야 합니다.
For the corresponding API, see Hand-Eye Calibration.
다음 섹션은 오일러 각도, 쿼터니언 또는 축 각도와 같이 다양한 로봇 컨트롤러에서 사용되는 여러 가지 방향 표현을 처리하는 데 특히 유용합니다.
도움 도구: 포즈 변환
Zivid는 주로 (4x4) 변환 행렬(회전 행렬 + 이동 벡터)을 사용합니다. 로봇 컨트롤러와 소프트웨어는 일반적으로 다양한 형식 중 하나로 위치 벡터와 방향 표현을 조합하여 포즈를 나타냅니다. 포즈 변환 도구를 사용하면 다음과 같은 형식으로 변환할 수 있습니다.
축-각도
회전 벡터
롤-피치-요
오일러 각
쿼터니언
저희는 로봇 자세 변환을 위한 몇 가지 도구를 제공합니다.
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 Hand-Eye 캘리브레이션: Halcon Sample Dataset
Sample Data 코드 샘플에서 사용할 데이터 세트를 추출하는 위치를 설명합니다.
ROS를 이용한 핸드-아이 캘리브레이션
ROS2 애플리케이션에 핸드-아이 캘리브레이션 기능을 통합하려는 사용자는 zivid-ros hand-eye sample 참조하십시오. zivid-ros 저장소의 샘플은 전체 서비스 기반 워크플로를 보여줍니다.
핸드-아이 캘리브레이션 훈련을 시작하고 캘리브레이션 대상 유형을 선택하세요.
로봇의 자세 정보를 캡처 서비스에 제공하여 여러 장의 이미지를 캡처하세요.
최종 핸드-아이 캘리브레이션 행렬을 계산하기 위해 보정 작업을 실행합니다.
캡처된 프레임과 로봇 자세를 저장하여 나중에 재사용할 수 있도록 선택적으로 작업 디렉토리를 제공할 수 있습니다.
참고
Zivid 포인트 클라우드는 밀리미터(mm) 단위로 제공되므로, 핸드-아이 캘리브레이션을 위한 로봇 입력 포즈 또한 이동에 mm 단위를 사용해야 합니다.
핸드-아이 캘리브레이션 방식을 선택했다면, 핸드-아이 캘리브레이션을 위한 좋은 데이터셋을 얻는 방법 을 확인해보세요.