Ensenso手眼标定cpp实现

2024-01-27 13:48
文章标签 实现 标定 cpp 手眼 ensenso

本文主要是介绍Ensenso手眼标定cpp实现,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

原理Ensenso SDK有介绍,这里是代码实现的简易版本。

需要修改serial number ,我的是194224。

使用方法:

将halcon标定板固定在机械臂上,运行代码,移动机械臂,输入机械臂上标定板当前的姿态(重复次数大于5即可)

成功的话,相机参数的link就已经被修改了,此时的坐标系统一为机械臂基座标系。

目前机械臂的移动是我手动控制的,也可以固定某些点输入,自动运行。

有问题的话,可以加我QQ:1441405602

#include <stdio.h>#include "nxLib.h"
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // Needed for conversion from geometry_msgs to tf2::Transform
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <string>
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Transform.h>#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>#include <iostream>
using namespace ros;
using namespace std;
bool poseIsValid(const tf::Pose& pose)
{auto origin = pose.getOrigin();if (std::isnan(origin.x()) || std::isnan(origin.y()) || std::isnan(origin.z())){return false;}auto rotation = pose.getRotation();if (std::isnan(rotation.getAngle())){return false;}auto rotationAxis = rotation.getAxis();if (std::isnan(rotationAxis.x()) || std::isnan(rotationAxis.y()) || std::isnan(rotationAxis.z())){return false;}return true;
}void writePoseToNxLib(tf::Pose const& pose, NxLibItem const& node)
{// Initialize the node to be empty. This is necessary, because there is a bug in some versions of the NxLib that// overwrites the whole transformation node with an identity transformation as soon as a new node in /Links gets// created.node.setNull();if (poseIsValid(pose)){auto origin = pose.getOrigin();node[itmTranslation][0] = origin.x() * 1000;  // ROS transformation is in// meters, NxLib expects it to// be in millimeters.node[itmTranslation][1] = origin.y() * 1000;node[itmTranslation][2] = origin.z() * 1000;auto rotation = pose.getRotation();node[itmRotation][itmAngle] = rotation.getAngle();auto rotationAxis = rotation.getAxis();node[itmRotation][itmAxis][0] = rotationAxis.x();node[itmRotation][itmAxis][1] = rotationAxis.y();node[itmRotation][itmAxis][2] = rotationAxis.z();}else{// Use an identity transformation as a reasonable default value.node[itmTranslation][0] = 0;node[itmTranslation][1] = 0;node[itmTranslation][2] = 0;node[itmRotation][itmAngle] = 0;node[itmRotation][itmAxis][0] = 1;node[itmRotation][itmAxis][1] = 0;node[itmRotation][itmAxis][2] = 0;}
}int main(void)
{try {// Initialize NxLib and enumerate camerasnxLibInitialize(true);// Reference to the first camera in the node BySerialNoNxLibItem root;NxLibItem camera = root[itmCameras][itmBySerialNo][0];// Open the EnsensoNxLibCommand open(cmdOpen);open.parameters()[itmCameras] = camera[itmSerialNumber].asString();open.execute();// We assume that the camera with the serial "1234" is already open. See here for information on how this// can be done.// Move your robot into a suitable starting position here. Make sure that the pattern can be seen from// the selected position.//tf::StampedTransform robotPose;// std::vector<tf::Pose> handEyeCalibrationRobotPoses;vector<tf::Transform> robotPoses;geometry_msgs::Pose robot_pose;// Set the pattern's grid spacing so that we don't need to decode the data from the pattern later. You// will need to adapt this line to the size of the calibration pattern that you are using.NxLibItem()[itmParameters][itmPattern][itmGridSpacing] = 20;// Discard any pattern observations that might already be in the pattern buffer.NxLibCommand(cmdDiscardPatterns).execute();// Turn off the camera's projector so that we can observe the calibration pattern.NxLibItem()[itmCameras]["194224"][itmParameters][itmCapture][itmProjector] = false;NxLibItem()[itmCameras]["194224"][itmParameters][itmCapture][itmFrontLight] = true;// We will observe the pattern 20 times. You can adapt this number depending on how accurate you need the// calibration to be.for (int i = 0; i < 10; i++) {// Move your robot to a new position from which the pattern can be seen. It might be a good idea to// choose these positions randomly.cout<<"Please enter x:";cin>>robot_pose.position.x;cout<<"Please enter y:";cin>>robot_pose.position.y;cout<<"Please enter z:";cin>>robot_pose.position.z;cout<<"Please enter rw:";cin>>robot_pose.orientation.w;cout<<"Please enter rx:";cin>>robot_pose.orientation.x;cout<<"Please enter ry:";cin>>robot_pose.orientation.y;cout<<"Please enter rz:";cin>>robot_pose.orientation.z;tf::Pose tfPose;tf::poseMsgToTF(robot_pose, tfPose);robotPoses.push_back(tfPose);// Make sure that the robot is not moving anymore. You might want to wait for a few seconds to avoid// any oscillations.sleep(2);// Observe the calibration pattern and store the observation in the pattern buffer.NxLibCommand capture(cmdCapture);capture.parameters()[itmCameras] = "194224";capture.execute();bool foundPattern = false;try {NxLibCommand collectPattern(cmdCollectPattern);collectPattern.parameters()[itmCameras] = "194224";collectPattern.execute();foundPattern = true;} catch (NxLibException& e) {printf("An NxLib API error with code %d (%s) occurred while accessing item %s.\n", e.getErrorCode(),e.getErrorText().c_str(), e.getItemPath().c_str());}if (foundPattern) {// We actually found a pattern. Get the current pose of your robot (from which the pattern was// observed) and store it somewhere.cout<< i <<"success"<<endl;} else {// The calibration pattern could not be found in the camera image. When your robot poses are// selected randomly, you might want to choose a different one.}}// You can insert a recalibration here, as you already captured stereo patterns anyway. See here for a// code snippet that does a recalibration.// We collected enough patterns and can start the calibration.NxLibCommand calibrateHandEye(cmdCalibrateHandEye);calibrateHandEye.parameters()[itmSetup] = valFixed; // Or "valMoving" when your have a moving setup.// At this point, you need to put your stored robot poses into the command's Transformations parameter.//calibrateHandEye.parameters()[itmTransformations] = robotPoses;for (size_t i = 0; i < robotPoses.size(); i++){writePoseToNxLib(robotPoses[i], calibrateHandEye.parameters()[itmTransformations][i]);}// Start the calibration. Note that this might take a few minutes if you did a lot of pattern observations.calibrateHandEye.execute();// Store the new calibration to the camera's EEPROM.NxLibCommand storeCalibration(cmdStoreCalibration);storeCalibration.parameters()[itmCameras] = "194224";storeCalibration.parameters()[itmLink] = true;storeCalibration.execute();// Close & finalizeNxLibCommand close(cmdClose);close.execute();} catch (NxLibException& e) { // Display NxLib API exceptions, if anyprintf("An NxLib API error with code %d (%s) occurred while accessing item %s.\n", e.getErrorCode(),e.getErrorText().c_str(), e.getItemPath().c_str());if (e.getErrorCode() == NxLibExecutionFailed)printf("/Execute:\n%s\n", NxLibItem(itmExecute).asJson(true).c_str());} nxLibFinalize();return 0;
}

 

 

这篇关于Ensenso手眼标定cpp实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/650423

相关文章

pandas中位数填充空值的实现示例

《pandas中位数填充空值的实现示例》中位数填充是一种简单而有效的方法,用于填充数据集中缺失的值,本文就来介绍一下pandas中位数填充空值的实现,具有一定的参考价值,感兴趣的可以了解一下... 目录什么是中位数填充?为什么选择中位数填充?示例数据结果分析完整代码总结在数据分析和机器学习过程中,处理缺失数

Golang HashMap实现原理解析

《GolangHashMap实现原理解析》HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持高效的插入、查找和删除操作,:本文主要介绍GolangH... 目录HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持

Pandas使用AdaBoost进行分类的实现

《Pandas使用AdaBoost进行分类的实现》Pandas和AdaBoost分类算法,可以高效地进行数据预处理和分类任务,本文主要介绍了Pandas使用AdaBoost进行分类的实现,具有一定的参... 目录什么是 AdaBoost?使用 AdaBoost 的步骤安装必要的库步骤一:数据准备步骤二:模型

使用Pandas进行均值填充的实现

《使用Pandas进行均值填充的实现》缺失数据(NaN值)是一个常见的问题,我们可以通过多种方法来处理缺失数据,其中一种常用的方法是均值填充,本文主要介绍了使用Pandas进行均值填充的实现,感兴趣的... 目录什么是均值填充?为什么选择均值填充?均值填充的步骤实际代码示例总结在数据分析和处理过程中,缺失数

Java对象转换的实现方式汇总

《Java对象转换的实现方式汇总》:本文主要介绍Java对象转换的多种实现方式,本文通过实例代码给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录Java对象转换的多种实现方式1. 手动映射(Manual Mapping)2. Builder模式3. 工具类辅助映

Go语言开发实现查询IP信息的MCP服务器

《Go语言开发实现查询IP信息的MCP服务器》随着MCP的快速普及和广泛应用,MCP服务器也层出不穷,本文将详细介绍如何在Go语言中使用go-mcp库来开发一个查询IP信息的MCP... 目录前言mcp-ip-geo 服务器目录结构说明查询 IP 信息功能实现工具实现工具管理查询单个 IP 信息工具的实现服

SpringBoot基于配置实现短信服务策略的动态切换

《SpringBoot基于配置实现短信服务策略的动态切换》这篇文章主要为大家详细介绍了SpringBoot在接入多个短信服务商(如阿里云、腾讯云、华为云)后,如何根据配置或环境切换使用不同的服务商,需... 目录目标功能示例配置(application.yml)配置类绑定短信发送策略接口示例:阿里云 & 腾

python实现svg图片转换为png和gif

《python实现svg图片转换为png和gif》这篇文章主要为大家详细介绍了python如何实现将svg图片格式转换为png和gif,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录python实现svg图片转换为png和gifpython实现图片格式之间的相互转换延展:基于Py

Python利用ElementTree实现快速解析XML文件

《Python利用ElementTree实现快速解析XML文件》ElementTree是Python标准库的一部分,而且是Python标准库中用于解析和操作XML数据的模块,下面小编就来和大家详细讲讲... 目录一、XML文件解析到底有多重要二、ElementTree快速入门1. 加载XML的两种方式2.

Java的栈与队列实现代码解析

《Java的栈与队列实现代码解析》栈是常见的线性数据结构,栈的特点是以先进后出的形式,后进先出,先进后出,分为栈底和栈顶,栈应用于内存的分配,表达式求值,存储临时的数据和方法的调用等,本文给大家介绍J... 目录栈的概念(Stack)栈的实现代码队列(Queue)模拟实现队列(双链表实现)循环队列(循环数组