网站建设的总结,中国公共招聘网,网站开发所需要的时间 知乎,专业的手机网站建设公司2D3D融合
概述
截止目前为止#xff0c;我们学习了机器人学#xff0c;学习了2D和3D视觉算法。我们也学习了2D相机(图像数据的来源)和3D相机(点云数据的来源)工作原理。
实际上#xff0c;我们最终要做的#xff0c;是一个手眼机器人系统。在这个系统里#xff0c…2D3D融合
概述
截止目前为止我们学习了机器人学学习了2D和3D视觉算法。我们也学习了2D相机(图像数据的来源)和3D相机(点云数据的来源)工作原理。
实际上我们最终要做的是一个手眼机器人系统。在这个系统里相机与机器人构成了两个非常关键的部分它们之间需要密切配合因此它们之间的关系也就非常重要。确定相机与机器人之间的关系这是手眼标定要解决的问题。
另一方面在很多场合为了增强算法的鲁棒性我们通常同时使用图像数据与点云数据这又涉及到2D与3D配准的问题。
相机配准 方式一
通过双重循环遍历
/*** 将彩色图和深度图合并成点云* param matrix 相机内参矩阵3x3* param rgb 彩色图* param depth 深度图* param cloud 输出点云*/
static void convert(Mat matrix, Mat rgb, Mat depth, PointCloud::Ptr cloud) {double camera_fx matrix.atdouble(0, 0);double camera_fy matrix.atdouble(1, 1);double camera_cx matrix.atdouble(0, 2);double camera_cy matrix.atdouble(1, 2);cout fx: camera_fx endl;cout fy: camera_fy endl;cout cx: camera_cx endl;cout cy: camera_cy endl;// 遍历深度图for (int v 0; v depth.rows; v)for (int u 0; u depth.cols; u) {// 获取深度图中(m,n)处的值ushort d depth.ptrushort(v)[u];// d 可能没有值若如此跳过此点if (isnan(d) abs(d) 0.0001)continue;// d 存在值则向点云增加一个点PointT p;// 计算这个点的空间坐标p.z double(d) / 1000; //单位是米p.x (u - camera_cx) * p.z / camera_fx;p.y (v - camera_cy) * p.z / camera_fy;// 从rgb图像中获取它的颜色// rgb是三通道的BGR格式图所以按下面的顺序获取颜色Vec3b bgr rgb.atVec3b(v, u);p.b bgr[0];p.g bgr[1];p.r bgr[2];// 把p加入到点云中cloud-points.push_back(p);//cout cloud-points.size() endl;}// 设置并保存点云cloud-height 1;cloud-width cloud-points.size();cout point cloud size cloud-points.size() endl;cloud-is_dense false;
}
int main(){cv::Mat cameraMatrix; // 从文件加载相机内参cv::Mat rgb; // 从相机得到RGB彩色图cv::Mat depth; // 从相机得到depth深度图PointCloud::Ptr pCloud PointCloud::Ptr(new PointCloud);convert(cameraMatrix, rgb, depth, pCloud);
}方式二
通过指针遍历并提前准备好计算矩阵
#include iostream
#include opencv2/opencv.hpp
#include sstream
#include cstdlib
#include pcl/io/io.husing namespace std;
using namespace cv;float qnan_ std::numeric_limitsfloat::quiet_NaN();
const char *cameraInCailFile ./assets/3DCameraInCailResult.xml;Eigen::Matrixfloat, 1920, 1 colmap;
Eigen::Matrixfloat, 1080, 1 rowmap;
//const short w 512, h 424;
const short w 1920, h 1080;void prepareMake3D(const double cx, const double cy,const double fx, const double fy) {float *pm1 colmap.data();float *pm2 rowmap.data();for (int i 0; i w; i) {*pm1 (i - cx 0.5) / fx;}for (int i 0; i h; i) {*pm2 (i - cy 0.5) / fy;}
}
/*** 根据内参合并RGB彩色图和深度图到点云* param cloud* param depthMat* param rgbMat*/
void getCloud(pcl::PointCloudpcl::PointXYZRGB::Ptr cloud, Mat depthMat, Mat rgbMat) {const float *itD0 (float *) depthMat.ptr();const char *itRGB0 (char *) rgbMat.ptr();if (cloud-size() ! w * h)cloud-resize(w * h);pcl::PointXYZRGB *itP cloud-points[0];bool is_dense true;for (size_t y 0; y h; y) {const unsigned int offset y * w;const float *itD itD0 offset;const char *itRGB itRGB0 offset * 4;const float dy rowmap(y);for (size_t x 0; x w; x, itP, itD, itRGB 4) {const float depth_value *itD / 1000.0f;if (!isnan(depth_value) abs(depth_value) 0.0001) {const float rx colmap(x) * depth_value;const float ry dy * depth_value;itP-z depth_value;itP-x rx;itP-y ry;itP-b itRGB[0];itP-g itRGB[1];itP-r itRGB[2];} else {itP-z qnan_;itP-x qnan_;itP-y qnan_;itP-b qnan_;itP-g qnan_;itP-r qnan_;is_dense false;}}}cloud-is_dense is_dense;
}int main(){Mat cameraMatrix cv::Mat_double(3, 3);FileStorage paramFs(cameraInCailFile, FileStorage::READ);paramFs[cameraMatrix] cameraMatrix;// 内参数据double fx cameraMatrix.atdouble(0, 0);double fy cameraMatrix.atdouble(1, 1);double cx cameraMatrix.atdouble(0, 2);double cy cameraMatrix.atdouble(1, 2);// 提前准备计算所需参数prepareMake3D(cx, cy, fx, fy);cv::Mat rgbMat; // 从相机得到RGB彩色图cv::Mat depthMat; // 从相机得到depth深度图pcl::PointCloudpcl::PointXYZRGB::Ptr cloud(new pcl::PointCloudpcl::PointXYZRGB());getCloud(cloud, depthMat, rgbMat)
}
手眼标定(外参标定)
一、手眼标定的原理
图例说明
{b}base基座标系{g}gripper抓手坐标系{t}target标定板坐标系{c}camera相机坐标系 二、手眼标定的操作¶ 将标定板固定在机械臂末端开启机械臂开启摄像头在距离摄像头40、60、80cm的距离上在摄像头可见范围内使用各种角度各拍照15-20张照片一共45-60张。同时保存照片以及对应拍照时机械臂位姿准备好之前标定的相机内参执行手眼标定API得到相机在基坐标系的表达旋转矩阵R平移向量t
三、自己动手实现手眼标定及验证¶
从文件及图片读取照片
// Created by poplar on 19-7-25.
#include iostream
#include opencv2/core.hpp
#include opencv2/calib3d.hpp#include boost/filesystem.hpp // includes all needed Boost.Filesystem declarations
#include boost/algorithm/string/predicate.hpp
#include opencv2/imgcodecs.hpp#include tinyxml/tinyxml2.h
#include map// Eigen 部分
#include Eigen/Core
// 稠密矩阵的代数运算逆特征值等
#include Eigen/Dense
// Eigen 几何模块
#include Eigen/Geometry#include rw/math/Rotation3D.hpp
#include rw/math/Vector3D.hpp
#include rw/math/RPY.hpp#include opencv/cxeigen.hpp
#include opencv/cv.hpp
#include utils/Rotation3DUtils.husing namespace boost::filesystem; // for ease of tutorial presentation;
// a namespace alias is preferred practice in real codeusing namespace tinyxml2;
using namespace Eigen;
using namespace cv;
using namespace std;using namespace rw::math;// Eigen
// OpenCV
// RobWorkconst string prefix_path /home/ty/Workspace/Robot/calibration-single;
const string intrinsicsPath prefix_path /CaliResult/3DCameraInCailResult.xml;const string pic_dir_path prefix_path /ImageFromCamera;
const string exten bmp;
const string extrinsic_params prefix_path /extrinsic_input_param.xml;
// const string extrinsic_params /home/poplar/Lesson/Cobot/Aubo/calibration-single/extrinsic_input_param_t.xml;const string exCailFilePath prefix_path /CaliResult/3DCameraExCailResult.xml;enum Pattern {CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID
};void printPose(const vectordouble pose);void calcChessboardCorners(const Size boardSize, float squareSize, vectorPoint3f corners,Pattern patternType CHESSBOARD) {corners.resize(0);switch (patternType) {case CHESSBOARD:case CIRCLES_GRID:for (int i 0; i boardSize.height; i) // 9for (int j 0; j boardSize.width; j) // 6corners.emplace_back(float(j * squareSize),float(i * squareSize), 0);break;case ASYMMETRIC_CIRCLES_GRID:for (int i 0; i boardSize.height; i)for (int j 0; j boardSize.width; j)corners.emplace_back(float((2 * j i % 2) * squareSize),float(i * squareSize), 0);break;default:CV_Error(Error::StsBadArg, Unknown pattern type\n);}
}/*** 通过图片集合 填充 旋转矩阵平移矩阵* param R_target2cam* param t_target2cam* param imgPaths*/
bool fillFromImages(vectorMat R_target2cam, std::vectorMat t_target2cam, std::vectorpath imgPaths) {const Size patternSize(6, 9);const float squareSize 20;//! [compute-poses]std::vectorPoint3f objectPoints;
// [
// [0, 0 , 0]
// [0, 20, 0]
// [0, 40, 0]
// ...
// [20, 0, 0]
// ...
// ]calcChessboardCorners(patternSize, squareSize, objectPoints);// 通过内参进行矫正// 检测角点// 计算变换矩阵旋转矩阵平移矩阵cv::FileStorage fs(intrinsicsPath, FileStorage::READ);Mat cameraMatrix, distCoeffs;fs[cameraMatrix] cameraMatrix;fs[distCoeffs] distCoeffs;// 遍历图片for (const auto path: imgPaths) {const string s_path path.string();
// std::cout s_path std::endl;const Mat img_mat imread(s_path, IMREAD_UNCHANGED);// 查找图片所有角点std::vectorPoint2f corners;bool isFound cv::findChessboardCorners(img_mat, patternSize, corners);if (!isFound) {std::cerr Image not found corners: s_path std::endl;return false;}
// std::cout corners.size() std::endl;cv::Mat rvec cv::Mat::zeros(3, 1, CV_64FC1);cv::Mat tvec cv::Mat::zeros(3, 1, CV_64FC1);// solveP3P// 根据// objectPoints角点行列信息大小信息// corners所有角点信息// 内参// 得到// 旋转向量平移向量solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec);// raux.convertTo(Rvec, CV_32F); //旋转向量
// taux.convertTo(Tvec, CV_32F); //平移向量Mat R; // 旋转矩阵 - 旋转向量// Transforms Rotation Vector to MatrixRodrigues(rvec, R); // solvePnP返回的是旋转向量故用罗德里格斯变换变成旋转矩阵cout rotation vector rvec \n rvec endl;cout rotation R \n R endl;cout translation vector tvec \n tvec std::endl;const Vec3f oulerAngles rotationMatrixToEulerAngles(R);std::cout oulerAngles \n oulerAngles * RA2DE std::endl; // zyx (RPY)
// Rotation3Ddouble rot(R);std::cout Image path: s_path std::endl;R_target2cam.push_back(R);
// t_target2cam.push_back(tvec);t_target2cam.push_back(tvec / 1000);// const Mat img_mat imread(s_path, IMREAD_UNCHANGED);
// Mat smallImg;
// resize( img_mat, smallImg, Size(), 0.5, 0.5, INTER_LINEAR_EXACT);
// cv::imshow(img_chess, smallImg);
// std::cout s_path std::endl;
// waitKey(0);}return true;}/*** 求齐次矩阵的逆矩阵* param T* return*/
static Mat homogeneousInverse(const Mat T) {CV_Assert(T.rows 4 T.cols 4);Mat R T(Rect(0, 0, 3, 3));Mat t T(Rect(3, 0, 1, 3));Mat Rt R.t();Mat tinv -Rt * t;Mat Tinv Mat::eye(4, 4, T.type());Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));return Tinv;
}/*** 外参标定** 输入* 60组t2c* 标定板在相机坐标系的表达标定板到相机的转换矩阵-旋转矩阵R平移向量t* 内参用于求相机在标定板坐标系的表达** 60组b2g (g2b求逆)* 末端gripper的x,y,z,r,p,y- 旋转矩阵R平移向量t, 笛卡尔RPY转旋转矩阵* 求逆(转置)正交矩阵两个计算结果一致** 输出* 外参 :* 相机在Base坐标系的表达 (轴角对平移向量t) (旋转矩阵R平移向量t)** 验证* 通过现有图片及标定结果进行验证* return*/
int main() {// 相机坐标系下标定板目标的表达 (通过 彩图深度图内参 获得) ---------------1std::vectorMat R_target2cam, t_target2cam;// 读取照片深度图if (!exists(pic_dir_path)) {std::cout pic_dir_path not exist std::endl;return 0;}int counter{0};// 将所有外参标定的照片路径存到imgPathsvectorpath imgPaths;directory_iterator end_itr;for (directory_iterator itr(pic_dir_path); itr ! end_itr; itr) {if (!is_directory(itr-status())) {path file_path itr-path();const path filename file_path.filename();if (boost::starts_with(filename.string(), raw_color_extrinsic_pose)) {
// std::cout filename.string() std::endl;imgPaths.push_back(file_path);// counter;
// if (counter 5){
// break;
// }}}}// 通过识别图像及角点得到相机到标定板的变换矩阵 (内参)bool rst fillFromImages(R_target2cam, t_target2cam, imgPaths);if (!rst) {return -1;}std::cout R_target2cam: R_target2cam.size() std::endl;std::cout t_target2cam: t_target2cam.size() std::endl;std::cout --------------------------------------------- 相机坐标系下标定板目标的表达 OK -------------------------------------------- ↑ std::endl;// 基坐标Base下末端TCP(gripper)的表达 (通过设备获得) ---------------2std::vectorMat R_gripper2base, t_gripper2base;// 轴角对平移 - 旋转矩阵平移矩阵XMLDocument doc;doc.LoadFile(extrinsic_params.c_str());XMLElement *root doc.RootElement();XMLElement *extrinsics root-FirstChildElement(extrinsic);mapstd::string, vectordouble map;while (extrinsics) {const char *image_path extrinsics-FirstChildElement(Limage_color_path)-GetText();string img_path std::string(image_path);string img_name img_path.substr(img_path.find_last_of(/) 1, -1);// std::cout image_path name: img_name std::endl;double pose1 atof(extrinsics-FirstChildElement(pose1)-GetText());double pose2 atof(extrinsics-FirstChildElement(pose2)-GetText());double pose3 atof(extrinsics-FirstChildElement(pose3)-GetText());double pose4 atof(extrinsics-FirstChildElement(pose4)-GetText());double pose5 atof(extrinsics-FirstChildElement(pose5)-GetText());double pose6 atof(extrinsics-FirstChildElement(pose6)-GetText());vectordouble pose{pose1, pose2, pose3, pose4, pose5, pose6};// 字典map保存的图片文件名及对应的位姿map[img_name] pose;extrinsics extrinsics-NextSiblingElement();}// 将对应图片的机械臂笛卡尔空间坐标pose转成 旋转矩阵平移矩阵for (const path p: imgPaths) {std::string f_name p.filename().string();try {// 取出每个图片对应的位姿vectordouble pose map.at(f_name);if (pose.empty()) {std::cerr pose empty std::endl;return -1;}
// std::cout f_name - ;printPose(pose);cv::Vec3f eulerAngles(pose[3],pose[4],pose[5]);const Mat R eulerAnglesToRotationMatrix(eulerAngles);cout rotation matrix3 eulerAngles \n eulerAngles endl;cout rotation matrix3 R \n R endl;cv::Mat t (cv::Mat_double(3,1) pose[0], pose[1], pose[2]);cout translation matrix3 t \n t endl;R_gripper2base.push_back(R);
// t_gripper2base.push_back(t);t_gripper2base.push_back(t / 1000);// const string s_path p.string();
// const Mat img_mat imread(s_path, IMREAD_UNCHANGED);
// Mat smallImg;
// resize( img_mat, smallImg, Size(), 0.5, 0.5, INTER_LINEAR_EXACT);
// cv::imshow(img_chess, smallImg);
// std::cout s_path std::endl;
// waitKey(0);} catch (const std::out_of_range e) {std::cerr f_name was not found. std::endl;}}std::cout --------------------------------------------- 基坐标Base下末端TCP(gripper)的表达 -------------------------------------------- ↑ std::endl;// return 0;// std::cout map[raw_color_extrinsic_pose_07_26_17_01_59_965.bmp].size() std::endl;// TCP坐标系下基坐标的表达std::vectorMat R_base2gripper, t_base2gripper;// 转换成逆矩阵unsigned long size R_gripper2base.size();R_base2gripper.reserve(size);t_base2gripper.reserve(size);for (size_t i 0; i size; i) {// 获取每个抓手的姿态旋转矩阵Mat R R_gripper2base[i];Mat Rt R.t(); // 转置R_base2gripper.push_back(Rt);// 获取每个抓手的位置Mat t t_gripper2base[i];Mat tinv -Rt * t;t_base2gripper.push_back(tinv);cout base2gripper Rt\n Rt endl;cout base2gripper tinv \n tinv endl;}std::cout --------------------------------------------- 末端TCP坐标下Base的表达 -------------------------------------------- ↑ std::endl;std::cout R_target2cam.size() : t_target2cam.size() \n R_base2gripper.size() : t_base2gripper.size() std::endl;std::cout ---------------------calibrateHandEye start !--------------------- std::endl;// 求Base基坐标下相机Cam的表达Mat R_cam2base_est, t_cam2base_est;// 进行手眼标定(外参)cv::calibrateHandEye(R_base2gripper, t_base2gripper,R_target2cam, t_target2cam,R_cam2base_est, t_cam2base_est,HandEyeCalibrationMethod::CALIB_HAND_EYE_DANIILIDIS);cout 旋转矩阵 est \n R_cam2base_est endl;cout 平移矩阵 est \n t_cam2base_est * 1000 endl;double angle 0;double axisX 0;double axisY 0;double axisZ 0;double translationX 0;double translationY 0;double translationZ 0;// 使用opencv读取文件cv::FileStorage fs(exCailFilePath, cv::FileStorage::READ);fs[Angle] angle;fs[AxisX] axisX;fs[AxisY] axisY;fs[AxisZ] axisZ;fs[TranslationX] translationX;fs[TranslationY] translationY;fs[TranslationZ] translationZ;// 轴角对Vector3d axisMatrix(axisX, axisY, axisZ);AngleAxisd angleAxisd(angle, axisMatrix);// 获取旋转矩阵const Eigen::AngleAxisdouble::Matrix3 rotationMatrix angleAxisd.toRotationMatrix();
// cout 旋转矩阵e rotationMatrix endl;// 获取平移矩阵Vector3d t_cam2base_eigen(translationX, translationY, translationZ);// 获取输出结果
// cout 平移向量e: t_cam2base_eigen endl;// 真实值eigen转成cvcv::Mat_double R_cam2base_true(3, 3);cv::eigen2cv(rotationMatrix,R_cam2base_true);cv::Mat_double t_cam2base_true(3, 1);cv::eigen2cv(t_cam2base_eigen,t_cam2base_true);cout 旋转矩阵 true \n R_cam2base_true endl;cout 平移矩阵 true \n t_cam2base_true endl;// 估算的 旋转矩阵-旋转向量
// Mat rvec_cam2base_est;
// cv::Rodrigues(R_cam2base_est, rvec_cam2base_est);// 真实的 旋转矩阵-旋转向量
// Mat rvec_cam2base_true;
// cv::Rodrigues(R_cam2base_true, rvec_cam2base_true);
// cout 旋转矩阵 est \n rvec_cam2base_est endl;
// cout 平移向量 est \n t_cam2base_true endl;// double rvecDiff norm(rvec_cam2base_true, rvec_cam2base_est, NormTypes::NORM_L2);
// double tvecDiff norm(t_cam2base_true, t_cam2base_est, NormTypes::NORM_L2);
// std::cout rvecDiff: rvecDiff tvecDiff: tvecDiff std::endl;return 0;
}void printPose(const vectordouble pose) {cout [ pose[0] pose[1] pose[2] pose[3] pose[4] pose[5] ] endl;
}
2D与3D融合实践¶
根据模板抓取指定物体
制作模板并计算取得相机到模板的变换矩阵T1根据实时相机中拍到的物体进行模板匹配得到变换矩阵T0最后和相机的外参矩阵Tc进行矩阵相乘得到目标在世界坐标系的表示从而进行抓取操作。
一、制作模板求T1¶ Kinect相机拍照得到RGB图和深度图 01_PhotoCapture.cpp 检测抓取位置(u,v)根据内参及深度信息得到三个空间中的点坐标 02_PointLocator.cpp 03_TemplateMaker.cpp 构建坐标系得到旋转矩阵T1转成RPY进行抓取测试 04_TestGrabTemplate.cpp 生成点云图用于模板匹配进行直通滤波及降采样 05_CreatePclCloud.cpp 验证变换矩阵 06_TemplateCloudFilter.cpp 生成剪切后的模板 实时的拍照得到RGB和深度图合成目标点云图通过直通滤波框定范围得到感兴趣区域将感兴趣区域进行降采样提高模板匹配效率
二、使用模板求T0¶
准备好切割后的点云template.pcd以及对应的变换矩阵T1可以有多个 Kinect相机拍照得到RGB图和深度图 01_PhotoCapture.cpp 生成目标点云图 07_TargetCloudFilter.cpp 进行模板与目标点云图匹配统一进行直通滤波及降采样得到变换矩阵T0 08_TemplateAlignment.cpp
三、进行抓取测试¶ 09_TestGrabTarget.cpp√ T0 为目标在模板坐标系的表达 T1 为模板在相机坐标系的表达 Tc 为相机在基坐标系的表达 待优化事宜¶
安全位置判定将盒子抓取到指定位置放置不间断抓取多个盒子准备多个模板提高模板匹配姿态识别度设置间隔实时进行模板匹配设置目标位置抓取动态延时自动避障其他
自己封装内外参标定工具¶
命令行版Qt版
