realsense d435和KUKA lbr 7轴机械臂 手眼标定 opencv

手眼标定 opencv"/>

realsense d435和KUKA lbr 7轴机械臂 手眼标定 opencv

科研项目用到realsense d435和kuka机器人进行手眼标定
主要用到realsense sdk，opencv，opencv-contrib
将一个ArUco maker放在机器人末端，然后可以使用相机读取ArUco中心在相机坐标系下的坐标，同时可以通过机器人那边得到ArUco在机器人基座标系下的坐标，移动多个位置，记录多组（我用了8个点）对应的点，求个伪逆，就可以得到转换矩阵
some points:
1、在pc端记下一个点时，机器人端也要同时记下点
2、kuka机器人端用到了阻抗模式，可以拖动机器人末端，没有力作用时，机器人会静止，有力作用时，开启阻抗模式。在机器人记录完8个点之后，还需要拖动一下机器人末端，跳出循环。之后等待PC发送“point”命令。建立tcp连接，发送点的数据。
3、机器人端程序只适用于kuka lbr机器人，ArUco中心点需要在sunrise里面建一个Tool和Frame，然后用4点法标定这个Frame，然后再程序里面才能用这个frame
4、opencv里面是没有ArUco库的，需要下载opencv-contrib,然后重新编译。opencv的和opencv-contirb库的安装很麻烦。。。csdn应该有很多帖子讲到，可以搜索一下。

自己水平有限，代码写的非常乱。。。。。。

PC端程序（PC连接相机，通过TCP socket与KUKA机器人端程序通讯）

/*
标定计算转换矩阵
识别ArUco码 按下enter键 记下点并输出到文件
识别完之后 通过socket发送point发送给机器人
机器人收到后 会将记录机器人点的文件发送过来
收到机器人的点
计算转换矩阵 并输出文件
*/#include <conio.h>
#include <librealsense2/rs.hpp>
#include<librealsense2/rsutil.h>
#include <librealsense2/hpp/rs_processing.hpp>
#include <librealsense2/hpp/rs_types.hpp>
#include <librealsense2/hpp/rs_sensor.hpp>
#include"vectorAndMat.h"
#include<iostream>
#include<fstream>
#include<winsock.h>
#include <opencv2/opencv.hpp>
#include <math.h>
#include<Windows.h>
#include<fstream>
#include <ctime>
#include <opencv2/aruco.hpp>
#pragma comment(lib,"ws2_32.lib")
using namespace std;
using namespace cv;void initialization();
int main() 
{rs2::pipeline pipe;     //Contruct a pipeline which abstracts the devicers2::config cfg;    //Create a configuration for configuring the pipeline with a non default profilecfg.enable_stream(RS2_STREAM_COLOR);cfg.enable_stream(RS2_STREAM_DEPTH);rs2::pipeline_profile selection = pipe.start(cfg);rs2::align align_to_color(RS2_STREAM_COLOR);const auto window_name = "Display Image";namedWindow(window_name, WINDOW_AUTOSIZE);//创建点的文件SYSTEMTIME sys;GetLocalTime(&sys);string filename;filename = "D:\\imagePoints_"+ to_string(sys.wYear)+ to_string(sys.wMonth)+ to_string(sys.wDay)+ to_string(sys.wHour)+ to_string(sys.wMinute)+ to_string(sys.wMinute)+ ".txt";ofstream out(filename, ofstream::app);int count = 0;while (waitKey(1) < 0 && getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0){rs2::frameset frames;frames = pipe.wait_for_frames();//Get each frameframes = align_to_color.process(frames);auto color_frame = frames.get_color_frame();auto depth_frame = frames.get_depth_frame();const int w = color_frame.as<rs2::video_frame>().get_width();const int h = color_frame.as<rs2::video_frame>().get_height();//cout << w << " " << h << endl;auto color_profile = color_frame.get_profile().as<rs2::video_stream_profile>();auto color_intrinsics = color_profile.get_intrinsics();rs2_intrinsics* in = &color_intrinsics;//create cv::Mat from rs2::frameMat color(Size(w, h), CV_8UC3, (void*)color_frame.get_data(), Mat::AUTO_STEP);cvtColor(color, color, COLOR_RGB2BGR);vector< int > markerIds;vector< vector<Point2f> > markerCorners;//cv::Ptr<cv::aruco::DetectorParameters> parameters;cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);cv::aruco::detectMarkers(color, dictionary, markerCorners, markerIds);Point2f center;if (markerCorners.size() > 0){vector< int > markerIds;center.x = (markerCorners[0][0].x + markerCorners[0][1].x + markerCorners[0][2].x + markerCorners[0][3].x) / 4;center.y = (markerCorners[0][0].y + markerCorners[0][1].y + ma