- 參考文獻
- 參考文獻1：OpenCV手眼标定（calibrateHandeye()）
- 參考文獻2:【OpenCV3學習筆記 】相機标定函數 calibrateCamera( ) 使用詳解（附相機标定程式和資料）
- 思路：
- 按照參考文獻1的做法，首先要拿到标定闆的位姿與機器人TCP的位姿；TCP的位姿是由機器人上位機軟體或API給出的，是以第一步是先想辦法拿到标定闆的位姿
- 标定闆位姿：直接跑參考文獻2的代碼即可，我稍作了修改，代碼如下：
- 拿到位姿之後，進一步用參考文獻二中的方法，标定出手眼變換矩陣
- 一些參數的問題(比如标定闆的size)：
/// 标定闆位姿解算程式
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <vector>
using namespace cv;
using namespace std;
/*
@param File_Directory 為檔案夾目錄
@param FileType 為需要查找的檔案類型
@param FilesName 為存放檔案名的容器
@other 目前的寫法是按1.png,2.png這樣來讀取的，請按需要修改
*/
void getFilesName(string &File_Directory, string &FileType, vector<string>&FilesName)
{
int count = 1;//初始圖檔索引
while (true) {
fstream _file;
string pic_name = File_Directory + "/" + to_string(count) + FileType;
_file.open(pic_name,ios::in);
if(!_file) {
cout<<pic_name<<"沒有被建立\n";
break;
} else {
cout<<pic_name<<"已經存在\n";
FilesName.push_back(pic_name);
}
count++;
}
}
void m_calibration(vector<string> &FilesName, Size board_size, Size square_size, Mat &cameraMatrix, Mat &distCoeffs, vector<Mat> &rvecsMat, vector<Mat> &tvecsMat)
{
ofstream fout("caliberation_result.txt"); // 儲存标定結果的檔案
cout << "開始提取角點………………" << endl;
int image_count = 0; // 圖像數量
Size image_size; // 圖像的尺寸
vector<Point2f> image_points; // 緩存每幅圖像上檢測到的角點
vector<vector<Point2f>> image_points_seq; // 儲存檢測到的所有角點
for (int i = 0;i < FilesName.size();i++)
{
image_count++;
// 用于觀察檢驗輸出
cout << "image_count = " << image_count << endl;
Mat imageInput = imread(FilesName[i]);
if (image_count == 1) //讀入第一張圖檔時擷取圖像寬高資訊
{
image_size.width = imageInput.cols;
image_size.height = imageInput.rows;
cout << "image_size.width = " << image_size.width << endl;
cout << "image_size.height = " << image_size.height << endl;
}
/* 提取角點 */
bool ok = findChessboardCorners(imageInput, board_size, image_points, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
if (0 == ok)
{
cout <<"第"<< image_count <<"張照片提取角點失敗，請删除後，重新标定！"<<endl; //找不到角點
imshow("失敗照片", imageInput);
waitKey(0);
}
else
{
Mat view_gray;
cout << "imageInput.channels()=" << imageInput.channels() << endl;
cvtColor(imageInput, view_gray, CV_RGB2GRAY);
/* 亞像素精确化 */
//find4QuadCornerSubpix(view_gray, image_points, Size(5, 5)); //對粗提取的角點進行精确化
cv::cornerSubPix(view_gray, image_points, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 20, 0.01));
image_points_seq.push_back(image_points); //儲存亞像素角點
/* 在圖像上顯示角點位置 */
drawChessboardCorners(view_gray, board_size, image_points, true);
//imshow("Camera Calibration", view_gray);//顯示圖檔
//waitKey(100);//暫停0.1S
}
}
cout << "角點提取完成！！！" << endl;
/*棋盤三維資訊*/
vector<vector<Point3f>> object_points_seq; // 儲存标定闆上角點的三維坐标
for (int t = 0;t < image_count;t++)
{
vector<Point3f> object_points;
for (int i = 0;i < board_size.height;i++)
{
for (int j = 0;j < board_size.width;j++)
{
Point3f realPoint;
/* 假設标定闆放在世界坐标系中z=0的平面上 */
realPoint.x = i*square_size.width;
realPoint.y = j*square_size.height;
realPoint.z = 0;
object_points.push_back(realPoint);
}
}
object_points_seq.push_back(object_points);
}
/* 運作标定函數 */
double err_first = calibrateCamera(object_points_seq, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, CV_CALIB_FIX_K3);
fout << "重投影誤差1：" << err_first << "像素" << endl << endl;
cout << "标定完成！！！" << endl;
cout << "開始評價标定結果………………";
double total_err = 0.0; // 所有圖像的平均誤差的總和
double err = 0.0; // 每幅圖像的平均誤差
double totalErr = 0.0;
double totalPoints = 0.0;
vector<Point2f> image_points_pro; // 儲存重新計算得到的投影點
for (int i = 0;i < image_count;i++)
{
projectPoints(object_points_seq[i], rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points_pro); //通過得到的錄影機内外參數，對角點的空間三維坐标進行重新投影計算
err = norm(Mat(image_points_seq[i]), Mat(image_points_pro), NORM_L2);
totalErr += err*err;
totalPoints += object_points_seq[i].size();
err /= object_points_seq[i].size();
//fout << "第" << i + 1 << "幅圖像的平均誤差：" << err << "像素" << endl;
total_err += err;
}
fout << "重投影誤差2：" << sqrt(totalErr / totalPoints) << "像素" << endl << endl;
fout << "重投影誤差3：" << total_err / image_count << "像素" << endl << endl;
//儲存定标結果
cout << "開始儲存定标結果………………" << endl;
Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 儲存每幅圖像的旋轉矩陣 */
fout << "相機内參數矩陣：" << endl;
fout << cameraMatrix << endl << endl;
fout << "畸變系數：\n";
fout << distCoeffs << endl << endl << endl;
for (int i = 0; i < image_count; i++)
{
fout << "第" << i + 1 << "幅圖像的旋轉向量：" << endl;
fout << rvecsMat[i] << endl;
/* 将旋轉向量轉換為相對應的旋轉矩陣 */
Rodrigues(rvecsMat[i], rotation_matrix);
fout << "第" << i + 1 << "幅圖像的旋轉矩陣：" << endl;
fout << rotation_matrix << endl;
fout << "第" << i + 1 << "幅圖像的平移向量：" << endl;
fout << tvecsMat[i] << endl << endl;
}
cout << "定标結果完成儲存！！！" << endl;
fout << endl;
}
void m_undistort(vector<string> &FilesName, Size image_size, Mat &cameraMatrix, Mat &distCoeffs)
{
Mat mapx = Mat(image_size, CV_32FC1); //X 坐标重映射參數
Mat mapy = Mat(image_size, CV_32FC1); //Y 坐标重映射參數
Mat R = Mat::eye(3, 3, CV_32F);
cout << "儲存矯正圖像" << endl;
string imageFileName; //校正後圖像的儲存路徑
stringstream StrStm;
string temp;
for (int i = 0; i < FilesName.size(); i++)
{
Mat imageSource = imread(FilesName[i]);
Mat newimage = imageSource.clone();
//方法一：使用initUndistortRectifyMap和remap兩個函數配合實作
//initUndistortRectifyMap(cameraMatrix,distCoeffs,R, Mat(),image_size,CV_32FC1,mapx,mapy);
// remap(imageSource,newimage,mapx, mapy, INTER_LINEAR);
//方法二：不需要轉換矩陣的方式，使用undistort函數實作
undistort(imageSource, newimage, cameraMatrix, distCoeffs);
StrStm << i + 1;
StrStm >> temp;
imageFileName = "矯正後圖像//" + temp + "_d.jpg";
imwrite(imageFileName, newimage);
StrStm.clear();
imageFileName.clear();
}
std::cout << "儲存結束" << endl;
}
int main()
{
string File_Directory1 = "."; //檔案夾目錄1
string FileType = ".png"; // 需要查找的檔案類型
vector<string>FilesName1; //存放檔案名的容器
getFilesName(File_Directory1, FileType, FilesName1); // 标定所用圖像檔案的路徑
Size board_size = Size(11, 8); // 标定闆上每行、列的角點數
Size square_size = Size(30, 30); // 實際測量得到的标定闆上每個棋盤格的實體尺寸，機關mm
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); // 錄影機内參數矩陣
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); // 錄影機的5個畸變系數：k1,k2,p1,p2,k3
vector<Mat> rvecsMat; // 存放所有圖像的旋轉向量，每一副圖像的旋轉向量為一個mat
vector<Mat> tvecsMat; // 存放所有圖像的平移向量，每一副圖像的平移向量為一個mat
m_calibration(FilesName1, board_size, square_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
//m_undistort(FilesName1, image_size, cameraMatrix, distCoeffs);
return 0;
}
//
// Created by ch on 2020/11/25.
//
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <thread>
#include <ui/robot_feedback.h>
#include <ros/ros.h>
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "ClimberKinematics5D.h"
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp> // Include OpenCV API
#include <opencv2/calib3d.hpp>
using namespace std;
using namespace cv;
enum CONTROL_STATUS {RECORD, WAIT, QUIT} CONTROL_FLAG;
bool to_record = false;//互動變量，true->準備記錄一組資料
vector<double> ENCODER_DATA(5, 0);//編碼器資料，用來計算TCP
void control_thread() {//互動線程，輸入1則記錄1組資料
bool to_quit = false;
while (!to_quit) {
switch (CONTROL_FLAG) {
case CONTROL_STATUS::WAIT : {
int i = 0;
cout << "Input 1 for record, input 0 for stop recording.\n";
cin >> i;
if (i == 0) {
CONTROL_FLAG = CONTROL_STATUS::QUIT;
break;
} else if (i == 1) CONTROL_FLAG = CONTROL_STATUS::RECORD;
break;
}
case CONTROL_STATUS::QUIT : to_quit = true; break;
case CONTROL_STATUS::RECORD : break;
}
}
}
void GetEncoderData(const ui::robot_feedbackConstPtr& data) {//ROS回調函數，擷取編碼器資料
ENCODER_DATA[0] = data->feedbackPosData[0];
ENCODER_DATA[1] = data->feedbackPosData[1];
ENCODER_DATA[2] = data->feedbackPosData[2];
ENCODER_DATA[3] = data->feedbackPosData[3];
ENCODER_DATA[4] = data->feedbackPosData[4];
}
void ros_thread() {//ROS線程，如果不另開線程的話,main裡面會有兩個while(一個是realsense的while， 一個是ros的spin)
//ros
ros::NodeHandle n;
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe("/low_level/joints_point_feedback", 100, GetEncoderData);
ros::spin();
}
void m_calibration(vector<string> &FilesName, const Size& board_size, const Size& square_size, Mat &cameraMatrix, Mat &distCoeffs, vector<Mat> &rvecsMat, vector<Mat> &tvecsMat)
{
ofstream fout("caliberation_result.txt"); // 儲存标定結果的檔案
cout << "開始提取角點………………" << endl;
int image_count = 0; // 圖像數量
Size image_size; // 圖像的尺寸
vector<Point2f> image_points; // 緩存每幅圖像上檢測到的角點
vector<vector<Point2f>> image_points_seq; // 儲存檢測到的所有角點
for (int i = 0;i < FilesName.size();i++)
{
image_count++;
// 用于觀察檢驗輸出
cout << "image_count = " << image_count << endl;
Mat imageInput = imread(FilesName[i]);
if (image_count == 1) //讀入第一張圖檔時擷取圖像寬高資訊
{
image_size.width = imageInput.cols;
image_size.height = imageInput.rows;
cout << "image_size.width = " << image_size.width << endl;
cout << "image_size.height = " << image_size.height << endl;
}
/* 提取角點 */
bool ok = findChessboardCorners(imageInput, board_size, image_points, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
if (0 == ok)
{
cout <<"第"<< image_count <<"張照片提取角點失敗，請删除後，重新标定！"<<endl; //找不到角點
imshow("失敗照片", imageInput);
waitKey(0);
}
else
{
Mat view_gray;
cout << "imageInput.channels()=" << imageInput.channels() << endl;
cvtColor(imageInput, view_gray, COLOR_RGB2GRAY);
/* 亞像素精确化 */
//find4QuadCornerSubpix(view_gray, image_points, Size(5, 5)); //對粗提取的角點進行精确化
cv::cornerSubPix(view_gray, image_points, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, 0.01));
image_points_seq.push_back(image_points); //儲存亞像素角點
/* 在圖像上顯示角點位置 */
drawChessboardCorners(view_gray, board_size, image_points, true);
imshow("Camera Calibration", view_gray);//顯示圖檔
waitKey(0);//暫停0.1S
}
}
cout << "角點提取完成！！！" << endl;
/*棋盤三維資訊*/
vector<vector<Point3f>> object_points_seq; // 儲存标定闆上角點的三維坐标
for (int t = 0;t < image_count;t++)
{
vector<Point3f> object_points;
for (int i = 0;i < board_size.height;i++)
{
for (int j = 0;j < board_size.width;j++)
{
Point3f realPoint;
/* 假設标定闆放在世界坐标系中z=0的平面上 */
realPoint.x = i*square_size.width;
realPoint.y = j*square_size.height;
realPoint.z = 0;
object_points.push_back(realPoint);
}
}
object_points_seq.push_back(object_points);
}
/* 運作标定函數 */
double err_first = calibrateCamera(object_points_seq, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, CALIB_FIX_K3);
fout << "重投影誤差1：" << err_first << "像素" << endl << endl;
cout << "标定完成！！！" << endl;
cout << "開始評價标定結果………………";
double total_err = 0.0; // 所有圖像的平均誤差的總和
double err = 0.0; // 每幅圖像的平均誤差
double totalErr = 0.0;
double totalPoints = 0.0;
vector<Point2f> image_points_pro; // 儲存重新計算得到的投影點
for (int i = 0;i < image_count;i++)
{
projectPoints(object_points_seq[i], rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points_pro); //通過得到的錄影機内外參數，對角點的空間三維坐标進行重新投影計算
err = norm(Mat(image_points_seq[i]), Mat(image_points_pro), NORM_L2);
totalErr += err*err;
totalPoints += object_points_seq[i].size();
err /= object_points_seq[i].size();
//fout << "第" << i + 1 << "幅圖像的平均誤差：" << err << "像素" << endl;
total_err += err;
}
fout << "重投影誤差2：" << sqrt(totalErr / totalPoints) << "像素" << endl << endl;
fout << "重投影誤差3：" << total_err / image_count << "像素" << endl << endl;
//儲存定标結果
cout << "開始儲存定标結果………………" << endl;
Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 儲存每幅圖像的旋轉矩陣 */
fout << "相機内參數矩陣：" << endl;
fout << cameraMatrix << endl << endl;
fout << "畸變系數：\n";
fout << distCoeffs << endl << endl << endl;
for (int i = 0; i < image_count; i++)
{
fout << "第" << i + 1 << "幅圖像的旋轉向量：" << endl;
fout << rvecsMat[i] << endl;
/* 将旋轉向量轉換為相對應的旋轉矩陣 */
Rodrigues(rvecsMat[i], rotation_matrix);
fout << "第" << i + 1 << "幅圖像的旋轉矩陣：" << endl;
fout << rotation_matrix << endl;
fout << "第" << i + 1 << "幅圖像的平移向量：" << endl;
fout << tvecsMat[i] << endl << endl;
}
cout << "定标結果完成儲存！！！" << endl;
fout << endl;
}
/**************************************************
* @brief 将旋轉矩陣與平移向量合成為齊次矩陣
* @note
* @param Mat& R 3*3旋轉矩陣
* @param Mat& T 3*1平移矩陣
* @return Mat 4*4齊次矩陣
**************************************************/
Mat R_T2HomogeneousMatrix(const Mat& R,const Mat& T)
{
Mat HomoMtr;
Mat_<double> R1 = (Mat_<double>(4, 3) <<
R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
0, 0, 0);
Mat_<double> T1 = (Mat_<double>(4, 1) <<
T.at<double>(0,0),
T.at<double>(1,0),
T.at<double>(2,0),
1);
cv::hconcat(R1, T1, HomoMtr); //矩陣拼接
return HomoMtr;
}
/**************************************************
* @brief 齊次矩陣分解為旋轉矩陣與平移矩陣
* @note
* @param const Mat& HomoMtr 4*4齊次矩陣
* @param Mat& R 輸出旋轉矩陣
* @param Mat& T 輸出平移矩陣
* @return
**************************************************/
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T)
{
//Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值
//Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3));
//R_HomoMtr.copyTo(R);
//T_HomoMtr.copyTo(T);
/*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R);
HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/
Rect R_rect(0, 0, 3, 3);
Rect T_rect(3, 0, 1, 3);
R = HomoMtr(R_rect);
T = HomoMtr(T_rect);
}
int main(int argc, char * argv[]) try
{
//realsense initialization
rs2::colorizer color_map;
rs2::pipeline pipe;
pipe.start();
//other initialization
ClimbotKinematics *kinematics = new ClimberKinematics5D();//運動學
std::thread control(control_thread);
std::thread ros_t(ros_thread);
vector<Eigen::Matrix4d> TCP_poses;
vector<string> pic_names;
int count = 0;
//capture pictures
const auto window_name = "Display Image";
namedWindow(window_name, WINDOW_AUTOSIZE);
bool to_quit = false;
while (waitKey(1) < 0 && getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0 && !to_quit) {
rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
rs2::frame depth = data.get_depth_frame().apply_filter(color_map);
rs2::frame rgb = data.get_color_frame();
const int w = rgb.as<rs2::video_frame>().get_width();
const int h = rgb.as<rs2::video_frame>().get_height();
Mat image(Size(w, h), CV_8UC3, (void*)rgb.get_data(), Mat::AUTO_STEP);
cvtColor(image, image, COLOR_BGR2RGB);
//record a picture
switch (CONTROL_FLAG) {
case CONTROL_STATUS::RECORD : {
string num_string = to_string(count);
cv::imwrite("./pose_" + num_string + ".jpg" , image);
pic_names.emplace_back("./pose_" + num_string + ".jpg");
auto T = kinematics->GetTransformMatrix(ClimberKinematics5D::G6, ENCODER_DATA);
TCP_poses.push_back(T);
ofstream TCP_trasform("./TCP_pose_" + num_string + ".txt", ofstream::out);
Eigen::Matrix3d R = T.block(0, 0, 3, 3);
Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0);
TCP_trasform << "Index: " << num_string << endl;
TCP_trasform << "Fixed X-Y-Z(x/y/z):\n" << euler_angles[0] / PI * 180 << " " << euler_angles[1] / PI * 180 << " " << euler_angles[2] / PI * 180 << endl;
TCP_trasform << "Position(m):\n" << T(0, 3) / 1000 << " "<< T(1, 3) / 1000 << " " << T(2, 3) / 1000 << endl << endl;
break;
}
case WAIT:
break;
case QUIT:
to_quit = true;
break;
}
count++;
// Update the window with new data
imshow(window_name, image);
}
//camera calibration and get board pose
Size board_size = Size(11, 8); // 标定闆上每行、列的角點數
Size square_size = Size(25, 25); // 實際測量得到的标定闆上每個棋盤格的實體尺寸，機關mm
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); // 錄影機内參數矩陣
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); // 錄影機的5個畸變系數：k1,k2,p1,p2,k3
vector<Mat> R_target2cam; // 存放所有圖像的旋轉向量，每一副圖像的旋轉向量為一個mat
vector<Mat> t_target2cam; // 存放所有圖像的平移向量，每一副圖像的平移向量為一個mat
m_calibration(pic_names, board_size, square_size, cameraMatrix, distCoeffs, R_target2cam, t_target2cam);
//至此，第2部分完成
//開始第3部分:矩陣格式轉換
vector<cv::Mat> R_gripper2base, t_gripper2base;
for (auto& T : TCP_poses) {
Mat cv_format_tcp_pose, cv_format_tcp_rvec, cv_format_tcp_tvec;
eigen2cv(T, cv_format_tcp_pose);
HomogeneousMtr2RT(cv_format_tcp_pose, cv_format_tcp_rvec, cv_format_tcp_tvec);
R_gripper2base.emplace_back(cv_format_tcp_rvec);
t_gripper2base.emplace_back(cv_format_tcp_tvec);
}
//至此，第3部分完成
//開始第4部分：手眼标定程式
cv::Mat R_cam2gripper,T_cam2gripper;
calibrateHandEye(R_gripper2base,t_gripper2base,R_target2cam,t_target2cam,R_cam2gripper,T_cam2gripper);
//第五部分：矩陣格式轉換
auto Homo_cam2gripper = R_T2HomogeneousMatrix(R_cam2gripper, T_cam2gripper);
cout << "Homo_cam2gripper:\n" << Homo_cam2gripper << endl;
cout << endl << "Finish.";
return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
![有關halcon标定過程中用到的.cpd檔案的解讀[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)