github
1 PLY网格模型
CSV格式的ply文件类 PLY是一种电脑档案格式,全名为多边形档案(Polygon File Format)或 斯坦福三角形档案(Stanford Triangle Format)。 该格式主要用以储存立体扫描结果的三维数值,透过多边形片面的集合描述三维物体, 与其他格式相较之下这是较为简单的方法。它可以储存的资讯包含颜色、 透明度、表面法向量、材质座标与资料可信度,并能对多边形的正反两面设定不同的属性。 在档案内容的储存上PLY有两种版本,分别是纯文字(ASCII)版本与二元码(binary)版本, 其差异在储存时是否以ASCII编码表示元素资讯。 每个PLY档都包含档头(header),用以设定网格模型的“元素”与“属性”, 以及在档头下方接着一连串的元素“数值资料”。 一般而言,网格模型的“元素” 就是顶点(vertices)、 面(faces),另外还可能包含有 边(edges)、 深度图样本(samples of range maps)与 三角带(triangle strips)等元素。 无论是纯文字与二元码的PLY档,档头资讯都是以ASCII编码编写, 接续其后的数值资料才有编码之分。PLY档案以此行: ply 开头作为PLY格式的识别。接着第二行是版本资讯,目前有三种写法: format ascii 1.0 format binary_little_endian 1.0 format binary_big_endian 1.0 // 其中ascii, binary_little_endian, binary_big_endian是档案储存的编码方式, // 而1.0是遵循的标准版本(现阶段仅有PLY 1.0版)。 comment This is a comment! // 使用'comment'作为一行的开头以编写注解 comment made by anonymous comment this file is a cube element vertex 8 // 描述元素 element <element name> <number in file> 8个顶点 // 以下 接续的6行property描述构成vertex元素的数值字段顺序代表的意义,及其资料形态。 property float32 x // 描述属性 property <data_type> <property name 1> property float32 y // 每个顶点使用3个 float32 类型浮点数(x,y,z)代表点的坐标 property float32 z property uchar blue // 使用3个unsigned char代表顶点颜色,颜色顺序为 (B, G, R) property uchar green property uchar red element face 12 property list uint8 int32 vertex_index // 12 个面(6*2) 另一个常使用的元素是面。 // 由于一个面是由3个以上的顶点所组成,因此使用一个“顶点列表”即可描述一个面, // PLY格式使用一个特殊关键字'property list'定义之。 end_header // 最后,标头必须以此行结尾: // 档头后接着的是元素资料(端点座标、拓朴连结等)。在ASCII格式中各个端点与面的资讯都是以独立的一行描述 0 0 0 // 8个顶点 索引 0~7 0 25.8 0 18.9 0 0 18.9 25.8 0 0 0 7.5 0 25.8 7.5 18.9 0 7.5 18.9 25.8 7.5 3 5 1 0 // 前面的3表示3点表示的面 有的一个面 它用其中的三个点 表示了两次 6*2=12 3 5 4 0 // 后面是上面定点的 索引 0~7 3 4 0 2 3 4 6 2 3 7 5 4 3 7 6 4 3 3 2 1 3 1 2 0 3 5 7 1 3 7 1 3 3 7 6 3 3 6 3 2
#include "CsvReader.h" // 默认构造函数 CsvReader::CsvReader(const string &path, const char &separator){ _file.open(path.c_str(), ifstream::in);//打开文件 _separator = separator; } // 读取ply文件 得到 顶点列表 和三角形面 列表 void CsvReader::readPLY(vector<Point3f> &list_vertex, vector<vector<int> > &list_triangles) { std::string line, tmp_str, n; int num_vertex = 0, num_triangles = 0; int count = 0; bool end_header = false; bool end_vertex = false; // Read the whole *.ply file while (getline(_file, line)) {//读取文件的每一行 stringstream liness(line); // 读取ply文件头 read header if(!end_header) { getline(liness, tmp_str, _separator);//分割每一行 if( tmp_str == "element" )// 描述元素 element <element name> <number in file> { getline(liness, tmp_str, _separator);// getline(liness, n); if(tmp_str == "vertex") num_vertex = StringToInt(n);// 顶点vertices 8个顶点 element vertex 8 if(tmp_str == "face") num_triangles = StringToInt(n);// 面face 12个三角形面 element face 12 } if(tmp_str == "end_header") end_header = true;// 标头必须以此 end_header 行结尾 } // read file content else if(end_header) { // 顶点vertices // read vertex and add into 'list_vertex' if(!end_vertex && count < num_vertex) { string x, y, z;// 0 25.8 0 getline(liness, x, _separator); getline(liness, y, _separator); getline(liness, z); cv::Point3f tmp_p;// 三维空间点 tmp_p.x = (float)StringToFloat(x);// Utils.h tmp_p.y = (float)StringToFloat(y); tmp_p.z = (float)StringToFloat(z); list_vertex.push_back(tmp_p); count++; if(count == num_vertex) { count = 0; end_vertex = !end_vertex; } } // 面face // read faces and add into 'list_triangles' else if(end_vertex && count < num_triangles) { string num_pts_per_face, id0, id1, id2; getline(liness, num_pts_per_face, _separator); getline(liness, id0, _separator);// 索引 getline(liness, id1, _separator); getline(liness, id2); std::vector<int> tmp_triangle(3); tmp_triangle[0] = StringToInt(id0); tmp_triangle[1] = StringToInt(id1); tmp_triangle[2] = StringToInt(id2); list_triangles.push_back(tmp_triangle); count++; } } } }
2 三维纹理
物体网格模型 物体的 三维纹理 模型文件
包含:2d-3d点对 特征点 特征点描述子
// 保存 物体的 三维纹理 模型文件 *.yaml void Model::save(const std::string path) { cv::Mat points3dmatrix = cv::Mat(list_points3d_in_);//2d点 vector 保存成 Mat cv::Mat points2dmatrix = cv::Mat(list_points2d_in_);//3d点 vector 保存成 Mat //cv::Mat keyPointmatrix = cv::Mat(list_keypoints_); cv::FileStorage storage(path, cv::FileStorage::WRITE);//文件保存 storage << "points_3d" << points3dmatrix; storage << "points_2d" << points2dmatrix; storage << "keypoints" << list_keypoints_; storage << "descriptors" << descriptors_; storage.release();// 释放文件句柄 } // 载入 物体的 三维纹理 模型文件 *.yaml void Model::load(const std::string path) { cv::Mat points3d_mat;// 3d点是按 Mat存储的 cv::FileStorage storage(path, cv::FileStorage::READ);//打开读取文件 storage["points_3d"] >> points3d_mat; storage["descriptors"] >> descriptors_;// 3d点 描述子就是按 vector存储的 points3d_mat.copyTo(list_points3d_in_);// 2d点 Mat -> vector storage.release();// 释放文件句柄 }
3 ORB鲁棒匹配
鲁棒匹配 两者相互看对眼了 图像1匹配到图像2的点 和图像2 匹配 图像1的点相互对应 才算是匹配
// 最匹配 和 次匹配 的比值大于 一个阈值 我认为这个匹配比较好 int RobustMatcher::ratioTest(std::vector<std::vector<cv::DMatch> > &matches) { int removed = 0; // 对于每一个匹配点对 matches for ( std::vector<std::vector<cv::DMatch> >::iterator matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) { // 如果检测到了两个最近点 if 2 NN has been identified if (matchIterator->size() > 1) { // 检测最近的距离和次近的距离的比值是否超过阈值 if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_) { matchIterator->clear(); //超过阈值 不好 最匹配距离和 次匹配距离差不多 这个匹配不好 removed++; } } else { // 没有两个匹配点 直接排除 does not have 2 neighbours matchIterator->clear(); // remove match removed++; } } return removed; } // 返回 相互匹配的 匹配点对 // image 1 -> image 2 == image 2 -> image 1 void RobustMatcher::symmetryTest( const std::vector<std::vector<cv::DMatch> >& matches1, const std::vector<std::vector<cv::DMatch> >& matches2, std::vector<cv::DMatch>& symMatches ) { // for all matches image 1 -> image 2 for (std::vector<std::vector<cv::DMatch> >::const_iterator matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1) { // ignore deleted matches if (matchIterator1->empty() || matchIterator1->size() < 2) continue; // for all matches image 2 -> image 1 for (std::vector<std::vector<cv::DMatch> >::const_iterator matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2) { // ignore deleted matches if (matchIterator2->empty() || matchIterator2->size() < 2) continue; // Match symmetry test if ((*matchIterator1)[0].queryIdx == (*matchIterator2)[0].trainIdx && (*matchIterator2)[0].queryIdx == (*matchIterator1)[0].trainIdx) { // add symmetrical match symMatches.push_back( cv::DMatch((*matchIterator1)[0].queryIdx, (*matchIterator1)[0].trainIdx, (*matchIterator1)[0].distance)); break; // next match in image 1 -> image 2 } } } } // 鲁棒匹配 两者相互看对眼了 // 图像1匹配到图像2的点 和图像2 匹配 图像1的点相互对应 才算是匹配 void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model ) { // 1a. Detection of the ORB features this->computeKeyPoints(frame, keypoints_frame); // 1b. Extraction of the ORB descriptors cv::Mat descriptors_frame; this->computeDescriptors(frame, keypoints_frame, descriptors_frame); // 2. Match the two image descriptors std::vector<std::vector<cv::DMatch> > matches12, matches21; // 2a. From image 1 to image 2 matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours // 2b. From image 2 to image 1 matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours // 3. Remove matches for which NN ratio is > than threshold // clean image 1 -> image 2 matches ratioTest(matches12); // clean image 2 -> image 1 matches ratioTest(matches21); // 4. Remove non-symmetrical matches symmetryTest(matches12, matches21, good_matches); } // 快速鲁棒匹配不需要相互看对眼 单相思也可以 void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches, std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model ) { good_matches.clear(); // 1a. Detection of the ORB features this->computeKeyPoints(frame, keypoints_frame); // 1b. Extraction of the ORB descriptors cv::Mat descriptors_frame; this->computeDescriptors(frame, keypoints_frame, descriptors_frame); // 2. Match the two image descriptors std::vector<std::vector<cv::DMatch> > matches; matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2); // 3. Remove matches for which NN ratio is > than threshold ratioTest(matches); // 4. Fill good matches container for ( std::vector<std::vector<cv::DMatch> >::iterator matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) { if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]); } }
4 PnPRansac 2d-3d点对匹配 Rt变换求解器
/* * PnPProblem.cpp * 2d-3d点对匹配 Rt变换求解器 */ #include <iostream> #include <sstream> #include "PnPProblem.h" #include "Mesh.h" #include <opencv2/calib3d/calib3d.hpp> /* Functions headers */ cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);//向量叉乘 double DOT(cv::Point3f v1, cv::Point3f v2);//向量点乘法 cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);//向量减法 // 找最近的3d点 cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin); //=================向量 叉乘=============================== // Functions for Möller–Trumbore intersection algorithm // (v1.x v1.y v1.z) // 得到向量 叉乘 // (v2.x v2.y v2.z) // ====> // v1.y * v2.z - v1.z * v2.y // v1.z * v2.x - v1.x * v2.z // v1.x * v2.y - v1.y * v2.x //=====>写成叉乘矩阵 // // 0 -v1.z v1.y v2.x // v1.z 0 -v1.x * v2.y // -v1.y v1.x 0 v2.z cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2) { cv::Point3f tmp_p; tmp_p.x = v1.y*v2.z - v1.z*v2.y; tmp_p.y = v1.z*v2.x - v1.x*v2.z; tmp_p.z = v1.x*v2.y - v1.y*v2.x; return tmp_p; } //=========向量点乘===================== //得到常数 对应元素相乘后加在一起 double DOT(cv::Point3f v1, cv::Point3f v2) { return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; } //========向量相减==================== //得到向量 对应元素相减 cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2) { cv::Point3f tmp_p; tmp_p.x = v1.x - v2.x; tmp_p.y = v1.y - v2.y; tmp_p.z = v1.z - v2.z; return tmp_p; } //End functions for Möller–Trumbore intersection algorithm // 在给定的3d点容器里(就存储了两个点) 找一个与 特定3d点 最相邻 的点 // Function to get the nearest 3D point to the Ray origin // 注意这里 3d点容器 为引用类型 避免拷贝 cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin) { cv::Point3f p1 = points_list[0];// 第一个点 cv::Point3f p2 = points_list[1];// 第二个点 double d1 = std::sqrt( std::pow(p1.x-origin.x, 2) + std::pow(p1.y-origin.y, 2) + std::pow(p1.z-origin.z, 2) ); double d2 = std::sqrt( std::pow(p2.x-origin.x, 2) + std::pow(p2.y-origin.y, 2) + std::pow(p2.z-origin.z, 2) ); if(d1 < d2)//与第一个点最近 { return p1; } else//与第二个点最近 { return p2; } } // Custom constructor given the intrinsic camera parameters // PnP求解器类 默认构造函数 PnPProblem::PnPProblem(const double params[]) { // PnP求解器参数初始化 // 相机内参数矩阵 _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // intrinsic camera parameters _A_matrix.at<double>(0, 0) = params[0]; // [ fx 0 cx ] _A_matrix.at<double>(1, 1) = params[1]; // [ 0 fy cy ] _A_matrix.at<double>(0, 2) = params[2]; // [ 0 0 1 ] _A_matrix.at<double>(1, 2) = params[3]; _A_matrix.at<double>(2, 2) = 1; // 旋转矩阵 R 需要求解的 _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // rotation matrix // 平移向量 t _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1); // translation matrix // 变换矩阵 P = [R t] _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1); // rotation-translation matrix } // PnP求解器类 默认析构函数 PnPProblem::~PnPProblem() { // TODO Auto-generated destructor stub } // 由 旋转矩阵R 和 平移向量t 构造变换矩阵P = [R t] void PnPProblem::set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix) { // Rotation-Translation Matrix Definition _P_matrix.at<double>(0,0) = R_matrix.at<double>(0,0); _P_matrix.at<double>(0,1) = R_matrix.at<double>(0,1); _P_matrix.at<double>(0,2) = R_matrix.at<double>(0,2); _P_matrix.at<double>(1,0) = R_matrix.at<double>(1,0); _P_matrix.at<double>(1,1) = R_matrix.at<double>(1,1); _P_matrix.at<double>(1,2) = R_matrix.at<double>(1,2); _P_matrix.at<double>(2,0) = R_matrix.at<double>(2,0); _P_matrix.at<double>(2,1) = R_matrix.at<double>(2,1); _P_matrix.at<double>(2,2) = R_matrix.at<double>(2,2); _P_matrix.at<double>(0,3) = t_matrix.at<double>(0); _P_matrix.at<double>(1,3) = t_matrix.at<double>(1); _P_matrix.at<double>(2,3) = t_matrix.at<double>(2); } // 2D-3D 单个点对 估计变换矩阵 cv::solvePnP() 得到 旋转向量 ----> 旋转矩阵 // Estimate the pose given a list of 2D/3D correspondences and the method to use bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d, const std::vector<cv::Point2f> &list_points2d, int flags) { cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);// distCoeffs是4×1、1×4、5×1或1×5的相机畸变向量 cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // 3*1 旋转向量 方向表示旋转轴 大小表示 旋转角度 cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // 3*1 平移向量 bool useExtrinsicGuess = false;//按初始值不断优化 // 变量useExtrinsicGuess的值为true,函数将使用所提供的rvec和tvec向量作为旋转和平移的初始值,然后会不断优化它们。 // 函数按最小化重投影误差来计算摄像机的变换,即让所得的投影向量imagePoints与被投影向量之间的距离平方和最小。 // Pose estimation bool correspondence = cv::solvePnP( list_points3d,// objectPoints 对象所在空间的三维点 list_points2d,// imagePoints是相应图像点(或投影) _A_matrix, // cameraMatrix是3×3的摄像机内部参数矩阵 distCoeffs, // distCoeffs是4×1、1×4、5×1或1×5的相机畸变向量 rvec, // rvec是输出的旋转向量,该向量将点由模型坐标系统 转换为 摄像机坐标系统 worid 到camera相机 tvec, // tvec是输出的平移向量 useExtrinsicGuess, flags); // 欧拉角的表示方式里,三个旋转轴一般是随着刚体在运动 row pitch yaw // 旋转向量 转到 旋转矩阵(旋转矩阵 也叫 方向余弦矩阵(Direction Cosine Matrix),简称DCM ) // 直观来讲,一个四维向量(theta,x,y,z)就可以表示出三维空间任意的旋转。 // 注意,这里的三维向量(x,y,z)只是用来表示旋转轴axis的方向朝向, // 因此更紧凑的表示方式是用一个单位向量来表示方向axis, // 而用该三维向量的长度来表示角度值theta。 // 这样以来,可以用一个三维向量(theta*x, theta*y, theta*z)就可以表示出三维空间任意的旋转, // 前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式, // ===============四元素========================================== // 同上,假设(x,y,z)是axis方向的单位向量,theta是绕axis转过的角度, // 那么四元数可以表示为[cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)]。 Rodrigues(rvec,_R_matrix);// 旋转向量 ----> 旋转矩阵 _t_matrix = tvec;// 平移向量 // 调用类的函数 设置 变换矩阵 Set projection matrix this->set_P_matrix(_R_matrix, _t_matrix); return correspondence; } // 随机序列采样 一致性 估计 变换矩阵 // 得到符合变换的内点外点数量找到 内点比例最大的 一个变换 // Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, // 对象所在空间的三维点 列表 const std::vector<cv::Point2f> &list_points2d, // 相应图像二维点(或投影) 列表 int flags, cv::Mat &inliers, int iterationsCount,// PnP method; inliers container float reprojectionError, double confidence ) // Ransac parameters { cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);// distCoeffs是4×1、1×4、5×1或1×5的相机畸变向量 cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // 3*1 旋转向量 方向表示旋转轴 大小表示 旋转角度 cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // 3*1 平移向量 bool useExtrinsicGuess = false; // 变量useExtrinsicGuess的值为true,函数将使用所提供的rvec和tvec向量作为旋转和平移的初始值,然后会不断优化它们。 // 函数按最小化重投影误差来计算摄像机的变换,即让所得的投影向量imagePoints与被投影向量之间的距离平方和最小。 cv::solvePnPRansac( list_points3d, // 对象所在空间的三维点 列表 list_points2d, // 相应图像二维点(或投影) 列表 _A_matrix, // cameraMatrix是3×3的摄像机内部参数矩阵 distCoeffs, // distCoeffs是4×1、1×4、5×1或1×5的相机畸变向量 rvec, // rvec是输出的旋转向量,该向量将点由模型坐标系统 转换为 摄像机坐标系统 worid 到camera相机 tvec, // tvec是输出的平移向量 useExtrinsicGuess, // 使用初始优化? iterationsCount, // 随机采样迭代计数 reprojectionError, // 投影误差 confidence, // 内点所占比例 可信度 inliers, // 内点数量 flags ); Rodrigues(rvec,_R_matrix);// 旋转向量 ----> 旋转矩阵 _t_matrix = tvec; // 平移向量 // 调用类的函数 设置 变换矩阵 Set projection matrix this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix } // 网格文件 顶点 三维坐标 --> 2d // Given the mesh, backproject the 3D points to 2D to verify the pose estimation std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh) { std::vector<cv::Point2f> verified_points_2d; for( int i = 0; i < mesh->getNumVertices(); i++) { cv::Point3f point3d = mesh->getVertex(i); // 网格 顶点 三维坐标 cv::Point2f point2d = this->backproject3DPoint(point3d);//3d点反投影到2d点 verified_points_2d.push_back(point2d);//3d --> 2d } return verified_points_2d; } // 反投影 3d --> 2d [u v 1]' = K * P * [x y z 1]' 再归一化 得到二维像素点坐标[u v]' // Backproject a 3D point to 2D using the estimated pose parameters cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d) { // 三维点 的 齐次表达式 3D point vector [x y z 1]' cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1); point3d_vec.at<double>(0) = point3d.x; point3d_vec.at<double>(1) = point3d.y; point3d_vec.at<double>(2) = point3d.z; point3d_vec.at<double>(3) = 1; // 二维点 的 齐次表达式2D point vector [u v 1]' cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1); point2d_vec = _A_matrix * _P_matrix * point3d_vec; // 将第三个量归一化 得到2d 点 Normalization of [u v]' cv::Point2f point2d; point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2)); point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2)); return point2d; } // 反投影 2d -> 3d 查看是否在 为物体网格模型的平面上 // Back project a 2D point to 3D and returns if it's on the object surface bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d) { // 三角形面 顶点 索引 n*3 std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList(); double lambda = 8; double u = point2d.x;// 二维像素点坐标[u v]' double v = point2d.y; // 二维点 的 齐次表达式 cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1 point2d_vec.at<double>(0) = u * lambda; point2d_vec.at<double>(1) = v * lambda; point2d_vec.at<double>(2) = lambda; // camera相机 coordinates cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1 // Point in world coordinates cv::Mat X_w = _R_matrix.inv() * ( X_c - _t_matrix ); // 3x1 // Center of projection cv::Mat C_op = cv::Mat(_R_matrix.inv()).mul(-1) * _t_matrix; // 3x1 // Ray direction vector cv::Mat ray = X_w - C_op; // 3x1 ray = ray / cv::norm(ray); // 3x1 // Set up Ray Ray R((cv::Point3f)C_op, (cv::Point3f)ray); // A vector to store the intersections found std::vector<cv::Point3f> intersections_list; // Loop for all the triangles and check the intersection for (unsigned int i = 0; i < triangles_list.size(); i++) { cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]); cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]); cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]); Triangle T(i, V0, V1, V2); double out; if(this->intersect_MollerTrumbore(R, T, &out)) { cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D intersections_list.push_back(tmp_pt); } } // If there are intersection, find the nearest one if (!intersections_list.empty()) { point3d = get_nearest_3D_point(intersections_list, R.getP0()); return true; } else { return false; } } // Möller–Trumbore intersection algorithm bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out) { const double EPSILON = 0.000001; cv::Point3f e1, e2; cv::Point3f P, Q, T; double det, inv_det, u, v; double t; cv::Point3f V1 = Triangle.getV0(); // Triangle vertices cv::Point3f V2 = Triangle.getV1(); cv::Point3f V3 = Triangle.getV2(); cv::Point3f O = Ray.getP0(); // Ray origin cv::Point3f D = Ray.getP1(); // Ray direction //Find vectors for two edges sharing V1 e1 = SUB(V2, V1); e2 = SUB(V3, V1); // Begin calculation determinant - also used to calculate U parameter P = CROSS(D, e2); // If determinant is near zero, ray lie in plane of triangle det = DOT(e1, P); //NOT CULLING if(det > -EPSILON && det < EPSILON) return false; inv_det = 1.f / det; //calculate distance from V1 to ray origin T = SUB(O, V1); //Calculate u parameter and test bound u = DOT(T, P) * inv_det; //The intersection lies outside of the triangle if(u < 0.f || u > 1.f) return false; //Prepare to test v parameter Q = CROSS(T, e1); //Calculate V parameter and test bound v = DOT(D, Q) * inv_det; //The intersection lies outside of the triangle if(v < 0.f || u + v > 1.f) return false; t = DOT(e2, Q) * inv_det; if(t > EPSILON) { //ray intersection *out = t; return true; } // No hit, no win return false; }
5 由简单的 长方体 顶点 面 描述的ply文件 和 物体的彩色图像 生产 物体的三维纹理模型文件
/* 由简单的 长方体 顶点 面 描述的ply文件 和 物体的彩色图像 生产 物体的三维纹理模型文件 2d-3d点配准 【1】手动指定 图像中 物体顶点的位置(得到二维像素值位置) ply文件 中有物体定点的三维坐标 由对应的 2d-3d点对关系 u v = K × [R t] X 1 Y Z 1 K 为图像拍摄时 相机的内参数 世界坐标中的三维点(以文件中坐标为(0,0,0)某个定点为世界坐标系原点) 经过 旋转矩阵R 和平移向量t 变换到相机坐标系下 在通过相机内参数 变换到 相机的图像平面上 【2】由 PnP 算法可解的 旋转矩阵R 和平移向量t 【3】把从图像中得到的纹理信息 加入到 物体的三维纹理模型中 在图像中提取特征点 和对应的描述子 利用 内参数K 、 旋转矩阵R 和平移向量t 反向投影到三维空间 标记 该反投影的3d点 是否在三维物体的 某个平面上 【4】将 2d-3d点对 、关键点 以及 关键点描述子 存入物体的三维纹理模型中 */ // C++ #include <iostream> // OpenCV #include <opencv2/core/core.hpp> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/calib3d/calib3d.hpp> #include <opencv2/features2d/features2d.hpp> // PnP Tutorial #include "Mesh.h"// 物体模型 读取 ply 文件 顶点坐标和 平面的顶点组成关系 #include "Model.h"// 物体网格模型 物体的 三维纹理 模型文件,包含:2d-3d点对 特征点 特征点描述子 #include "PnPProblem.h"// 2d-3d点对匹配 Rt变换求解器 #include "RobustMatcher.h"// #include "ModelRegistration.h"// 2d - 3d 匹配点对记录 类 #include "Utils.h"// 画图 打印图片 显示文字等操作 using namespace cv; using namespace std; // 全局变量 string tutorial_path = "../"; // path to tutorial string img_path = tutorial_path + "Data/resized_IMG_3875.JPG"; // 需要配准的图像 找到器三维空间位置模型 string ply_read_path = tutorial_path + "Data/box.ply"; // 物体模型对象 object mesh 简单的 ply // ply文件格式详细说明 https://www.cnblogs.com/liangliangdetianxia/p/4000295.html // ply文件格式详细说明 https://blog.csdn.net/huapenguag/article/details/69831350 string write_path = tutorial_path + "Data/cookies_ORB.yml"; // 输出文件 物体的 三维纹理 模型文件,包含:2d-3d点对 特征点 特征点描述子 output file // 手动指定二位点 2d-3d 点对 配准是否完成标志 bool end_registration = false; // 相机内参数 K Intrinsic camera parameters: UVC WEBCAM double f = 45; // 焦距 毫米单位 focal length in mm double sx = 22.3, sy = 14.9; double width = 2592, height = 1944;//图像尺寸 double params_CANON[] = { width*f/sx, // fx height*f/sy, // fy width/2, // cx height/2}; // cy // ply中指定了 8个顶点 的 长方体 int n = 8;//8个顶点 int pts[] = {1, 2, 3, 4, 5, 6, 7, 8}; // 长方体 顶点 序列 // 颜色 Scalar red(0, 0, 255); Scalar green(0,255,0); Scalar blue(255,0,0); Scalar yellow(0,255,255); ModelRegistration registration;//模型配准对象 记录2d-3d点对 Model model;// 物体网格模型 物体的 三维纹理 模型文件,包含:2d-3d点对 特征点 特征点描述子 Mesh mesh; // 物体模型 读取 ply 文件 顶点坐标和 平面的顶点组成关系 PnPProblem pnp_registration(params_CANON);// 2D-3D 点对匹配 Rt变换求解器 // 帮助函声明 void help(); // 鼠标点击响应 回调函数 模型配置 // 手动指定 图像中 物体顶点的位置(得到二维像素值位置) static void onMouseModelRegistration( int event, int x, int y, int, void* ) { if ( event == EVENT_LBUTTONUP )//鼠标按下 { int n_regist = registration.getNumRegist();// 当前配准的点数 int n_vertex = pts[n_regist];// 顶点 Point2f point_2d = Point2f((float)x,(float)y);// 鼠标点击的 像素位置 Point3f point_3d = mesh.getVertex(n_vertex-1);// 对应mesh中的 三维坐标值 0~7 // 判断当前是否还需要 确定匹配点 当前已经匹配的点对数 < 所需匹配点对数时 就还需要 bool is_registrable = registration.is_registrable(); if (is_registrable) { registration.registerPoint(point_2d, point_3d);//模型配准类 中添加 2d-3d配准点对 if( registration.getNumRegist() == registration.getNumMax() ) // 记录手动确定的点数 满8个就匹配完毕 end_registration = true; } } } // 主函数 int main() { // 帮助信息 help(); // 载入物体网格模型文件 *.ply mesh.load(ply_read_path); // 设置最大 关键点个数 int numKeyPoints = 10000; //鲁棒匹配器 detector, extractor, matcher RobustMatcher rmatcher; Ptr<FeatureDetector> detector = ORB::create(numKeyPoints);//ORB特征点检测器 rmatcher.setFeatureDetector(detector);//匹配器的特征 提取方法 // 创建显示窗口 // Create & Open Window namedWindow("MODEL REGISTRATION", WINDOW_KEEPRATIO); // 鼠标点击事件 Set up the mouse events setMouseCallback("MODEL REGISTRATION", onMouseModelRegistration, 0 ); // 读取图像进行匹配 Open the image to register Mat img_in = imread(img_path, IMREAD_COLOR); Mat img_vis = img_in.clone(); if (!img_in.data) { cout << "Could not open or find the image" << endl; return -1; } // 设置需要手动 配准的 2d-3d点对数 int num_registrations = n;//初始最大需要配置的 点对数 registration.setNumMax(num_registrations); cout << "Click the box corners ..." << endl; cout << "Waiting ..." << endl; //======循环直到 完成配准 Loop until all the points are registered while ( waitKey(30) < 0 ) { // Refresh debug image img_vis = img_in.clone();//更新图像 // 得到当前配准的点对 2d-3d Current registered points vector<Point2f> list_points2d = registration.get_points2d(); vector<Point3f> list_points3d = registration.get_points3d(); // 显示当前配准的点对 2d-3d Draw current registered points drawPoints(img_vis, list_points2d, list_points3d, red);// 红色 2d点 if (!end_registration)//配准未完成 显示还需要配准的3d点 { // Draw debug text int n_regist = registration.getNumRegist();// 已经匹配的点 int n_vertex = pts[n_regist];//对应三维点的 索引 Point3f current_poin3d = mesh.getVertex(n_vertex-1);//对应ply文件中 三维点的坐标 drawQuestion(img_vis, current_poin3d, green);// 绿色 3d反投影的点 drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red); } else//已经配准完成 结束循环 { // Draw debug text drawText(img_vis, "END REGISTRATION", green); drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green); break;// 结束循环 } // Show the image imshow("MODEL REGISTRATION", img_vis); } //=====配准完成 进行相机位姿 计算============== cout << "COMPUTING POSE ..." << endl; // 匹配的 2d-3d点对 vector<Point2f> list_points2d = registration.get_points2d(); vector<Point3f> list_points3d = registration.get_points3d(); //=====pnp 估计变换矩阵 ========== bool is_correspondence = pnp_registration.estimatePose(list_points3d, list_points2d, SOLVEPNP_ITERATIVE); if ( is_correspondence ) { cout << "Correspondence found" << endl; //=====使用得到的 变换位姿 将 三维网格中的3d点 投影到 2d点 并显示=========== vector<Point2f> list_points2d_mesh = pnp_registration.verify_points(&mesh); draw2DPoints(img_vis, list_points2d_mesh, green);//显示3d -> 2d点 绿色 } else { cout << "Correspondence not found" << endl << endl; } // 显示图像 imshow("MODEL REGISTRATION", img_vis); // ESC 键 结束 waitKey(0); //===计算图像2d特征点,使用变换矩阵得到3d点 =================== vector<KeyPoint> keypoints_model;//关键点 Mat descriptors;//描述子 rmatcher.computeKeyPoints(img_in, keypoints_model);// 检测关键点 rmatcher.computeDescriptors(img_in, keypoints_model, descriptors);//计算关键点的描述子 for (unsigned int i = 0; i < keypoints_model.size(); ++i) { Point2f point2d(keypoints_model[i].pt);//每一个检测出的 2d 特征点 Point3f point3d;// 按变换关系 转换成的三维点 bool on_surface = pnp_registration.backproject2DPoint(&mesh, point2d, point3d); if (on_surface)// 转换得到的3d点 在物体表面上 { model.add_correspondence(point2d, point3d);// 模型文件添加2d-3d点 model.add_descriptor(descriptors.row(i));//描述子 model.add_keypoint(keypoints_model[i]);//关键点 } else// 转换得到的3d点 不在物体表面上 { model.add_outlier(point2d);// 模型文件添加2d外点 } } // 保存模型文件 *.yaml model.save(write_path); // 输出图像 img_vis = img_in.clone(); // 2d - 3d 点 vector<Point2f> list_points_in = model.get_points2d_in(); vector<Point2f> list_points_out = model.get_points2d_out(); // 显示一些文字 内点 和 外点数量 string num = IntToString((int)list_points_in.size()); string text = "There are " + num + " inliers";//内点 drawText(img_vis, text, green); num = IntToString((int)list_points_out.size()); text = "There are " + num + " outliers";//外点 drawText2(img_vis, text, red); // 画物体三维网格 绿色 drawObjectMesh(img_vis, &mesh, &pnp_registration, blue); //显示找到的 关键点 内点显示绿色 外点显示红色 draw2DPoints(img_vis, list_points_in, green); draw2DPoints(img_vis, list_points_out, red); // 显示最后的图像 imshow("MODEL REGISTRATION", img_vis); // ESC 键 结束 waitKey(0); // 是否窗口 destroyWindow("MODEL REGISTRATION"); cout << "GOODBYE" << endl; } /**********************************************************************************************************/ void help() { cout << "--------------------------------------------------------------------------" << endl << "This program shows how to create your 3D textured model. " << endl << "Usage:" << endl << "./cpp-tutorial-pnp_registration" << endl << "--------------------------------------------------------------------------" << endl << endl; }
6 纹理对象的实时姿态估计
/* 【0】 读取网格数据文件 和 三维纹理数据文件 获取3d描述数据库 设置特征检测器 描述子提取器 描述子匹配器 【1】 场景中提取特征点 描述子 在 模型库(3d描述子数据库 3d points +description )中提取 匹配点 【2】 获取场景图片中 和 模型库中匹配的2d-3d点对 【3】 使用PnP + Ransac进行姿态估计 【4】 显示 PNP求解后 得到的内点 【5】 使用线性卡尔曼滤波去除错误的姿态估计 【6】 更新pnp 的 变换矩阵 【7】显示位姿 轴 帧率 可信度 【8】显示调试数据 */ // C++ #include <iostream> #include <time.h> // OpenCV #include <opencv2/core/core.hpp> #include <opencv2/core/utility.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/calib3d/calib3d.hpp> #include <opencv2/video/tracking.hpp> // PnP Tutorial #include "Mesh.h" #include "Model.h" #include "PnPProblem.h" #include "RobustMatcher.h" #include "ModelRegistration.h" #include "Utils.h" // using namespace cv; using namespace std; // 全局变量 string tutorial_path = "../"; // build 上一个目录 string video_read_path = tutorial_path + "Data/box.mp4"; // 如要检测的视频 可以使用实时视频 string yml_read_path = tutorial_path + "Data/cookies_ORB.yml"; // 物体的三维纹理 模型文件 2d-3d 描述子 3dpts + descriptors string ply_read_path = tutorial_path + "Data/box.ply"; // 物体 顶点 面 网格文件 // 相机内参数 K double f = 55; // focal length in mm double sx = 22.3, sy = 14.9; // sensor size double width = 640, height = 480; // image size double params_WEBCAM[] = { width*f/sx, // fx height*f/sy, // fy width/2, // cx height/2}; // cy // 颜色 Scalar red(0, 0, 255); Scalar green(0,255,0); Scalar blue(255,0,0); Scalar yellow(0,255,255); //鲁棒匹配器 参数 parameters int numKeyPoints = 2000; // 每幅图像检测的关键点的最大个数 float ratioTest = 0.70f; // 匹配点 质量评价阈值 最匹配和次匹配比值大于这个值会被剔除 bool fast_match = true; // fastRobustMatch()(仅考虑比率) robustMatch()(考虑比率+相互匹配) // 随机序列采样一致性算法参数 parameters int iterationsCount = 500; // 最大迭代次数 float reprojectionError = 2.0; // 是否是内点的阈值 投影后和匹配点的误差 double confidence = 0.95; // 算法成功的可信度 阈值 内点/总点 > 0.95 // 卡尔曼滤波内点数量阈值 Kalman Filter parameters int minInliersKalman = 30; // Kalman threshold updating // 2d-3d匹配点对求解变换矩阵算法 参数 PnP parameters int pnpMethod = SOLVEPNP_ITERATIVE; // 函数声明 void help(); // 初始化卡尔曼滤波器 void initKalmanFilter( KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt); // 更新卡尔曼滤波器 void updateKalmanFilter( KalmanFilter &KF, Mat &measurements, Mat &translation_estimated, Mat &rotation_estimated ); // 设置卡尔曼滤波器测量数据 void fillMeasurements( Mat &measurements, const Mat &translation_measured, const Mat &rotation_measured); /** Main program **/ int main(int argc, char *argv[]) { help(); const String keys = "{help h | | print this message }" "{video v | | path to recorded video }"// 需要识别的视频 "{model | | path to yml model }"// 模型的三维纹理文件 "{mesh | | path to ply mesh }"// 模型的 网格数据文件 "{keypoints k |2000 | number of keypoints to detect }"// 关键点 "{ratio r |0.7 | threshold for ratio test }"// 匹配点好坏 评判阈值 "{iterations it |500 | RANSAC maximum iterations count }"// 迭代算法 总最大迭代次数 "{error e |2.0 | RANSAC reprojection errror }"// 内点阈值 "{confidence c |0.95 | RANSAC confidence }"// 可行度阈值 "{inliers in |30 | minimum inliers for Kalman update }"// 最少内点个数 卡尔曼滤波器 "{method pnp |0 | PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS}"// pnp算法 "{fast f |true | use of robust fast match }"// 快速鲁棒匹配 还是 鲁棒匹配 ; CommandLineParser parser(argc, argv, keys);// 命令行参数 解析 if (parser.has("help")) { parser.printMessage(); return 0; } else { video_read_path = parser.get<string>("video").size() > 0 ? parser.get<string>("video") : video_read_path; yml_read_path = parser.get<string>("model").size() > 0 ? parser.get<string>("model") : yml_read_path; ply_read_path = parser.get<string>("mesh").size() > 0 ? parser.get<string>("mesh") : ply_read_path; numKeyPoints = !parser.has("keypoints") ? parser.get<int>("keypoints") : numKeyPoints; ratioTest = !parser.has("ratio") ? parser.get<float>("ratio") : ratioTest; fast_match = !parser.has("fast") ? parser.get<bool>("fast") : fast_match; iterationsCount = !parser.has("iterations") ? parser.get<int>("iterations") : iterationsCount; reprojectionError = !parser.has("error") ? parser.get<float>("error") : reprojectionError; confidence = !parser.has("confidence") ? parser.get<float>("confidence") : confidence; minInliersKalman = !parser.has("inliers") ? parser.get<int>("inliers") : minInliersKalman; pnpMethod = !parser.has("method") ? parser.get<int>("method") : pnpMethod; } PnPProblem pnp_detection(params_WEBCAM);// pnp求解类 得到的 [Rt] PnPProblem pnp_detection_est(params_WEBCAM);// pnp求解后 再使用卡尔曼滤波器滤波得到的位姿 // 读取3D纹理对象 加载textured model实现了Model类(class),其中的函数 load() 载入3d点 和 对应的 描述子 Model model; // instantiate Model object model.load(yml_read_path); // load a 3D textured object model // 加载网格模型 来打开 .ply 格式的文件 得到 三维顶点 和 面 Mesh mesh; // instantiate Mesh object mesh.load(ply_read_path); // load an object mesh // 鲁棒匹配器 给图像 和需要匹配的描述子集合 // 对图像提取 orb 描述子 和 需要匹配的描述子集合 做匹配 相互看对眼 单相思 RobustMatcher rmatcher;// instantiate RobustMatcher // 特征点检测器 Ptr<FeatureDetector> orb = ORB::create(); // 鲁棒匹配器设置 特征点检测器 rmatcher.setFeatureDetector(orb);// set feature detector // 鲁棒匹配器设置 描述子提取器 rmatcher.setDescriptorExtractor(orb);// set descriptor extractor // 鲁棒匹配器设置 描述子匹配器 Ptr<cv::flann::IndexParams> indexParams = makePtr<flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters Ptr<cv::flann::SearchParams> searchParams = makePtr<flann::SearchParams>(50); // instantiate flann search parameters // instantiate FlannBased matcher Ptr<DescriptorMatcher> matcher = makePtr<FlannBasedMatcher>(indexParams, searchParams); rmatcher.setDescriptorMatcher(matcher);// 鲁棒匹配器设置 描述子匹配器 rmatcher.setRatio(ratioTest); // 设置匹配器 匹配好坏阈值 // 卡尔曼滤波器 及其参数 KalmanFilter KF; // instantiate Kalman Filter int nStates = 18; // the number of states 状态 维度 int nMeasurements = 6; // the number of measured states 测量 维度 [x y z row pitch yaw] int nInputs = 0; // the number of control actions 控制 无 double dt = 0.125; // time between measurements (1/FPS) 时间 // 初始化 initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // 卡尔曼滤波器初始化函数 Mat measurements(nMeasurements, 1, CV_64F); // 测量数据 数据 [x y z row pitch yaw] measurements.setTo(Scalar(0)); bool good_measurement = false; // 物体三维点 列表和 其对应的 二维描述子模型库 descriptors_model vector<Point3f> list_points3d_model = model.get_points3d();// 3d Mat descriptors_model = model.get_descriptors();// 描述子 descriptors // 创建显示窗口 namedWindow("REAL TIME DEMO", WINDOW_KEEPRATIO); // 捕获视频留 VideoCapture cap; // 实例化对象 VideoCapture cap.open(video_read_path); // 打开视频 if(!cap.isOpened()) // 检查是否打开成功 { cout << "Could not open the camera device" << endl; return -1; } // 开始和结束时间 time_t start, end; // 帧率 和 经过的时间 double fps, sec; // 帧 记录 int counter = 0; // 记录开始时间 start time(&start); Mat frame, frame_vis;//原始帧 和 检测位置修改后的帧 while(cap.read(frame) && waitKey(30) != 27) // 按 ESC键 结束 { frame_vis = frame.clone(); // 处理的帧 //=======【1】Step 1: 场景中提取特征点 在 模型库中提取 匹配点 ============================= vector<DMatch> good_matches; // 匹配的模型中的三维点(模型中记录了描述子) to obtain the 3D points of the model vector<KeyPoint> keypoints_scene; // 场景的关键点 // 基于Flann算法对ORB特征描述子进行匹配 if(fast_match) {// 快速匹配 仅仅 最有匹配距离/次优匹配距离 < 阈值就认为这个匹配可以 rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model); } else {//相互匹配对 物体三维点对应的 二维描述子模型库 descriptors_model rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model); } //======【2】Step 2: 获取场景图片中 和 模型库中匹配的2d-3d点对============================= vector<Point3f> list_points3d_model_match; // 3D点 来自模型 文件中 vector<Point2f> list_points2d_scene_match; // 匹配的对应场景图片中 的 2d点 for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)//匹配点对 { Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ];// 物体模型 3D点 Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 匹配对应的 场景2D点 list_points3d_model_match.push_back(point3d_model); // add 3D point list_points2d_scene_match.push_back(point2d_scene); // add 2D point } // 显示场景中 检测到的 匹配的 2D点 所有点 draw2DPoints(frame_vis, list_points2d_scene_match, red); // 根据2d-3d点对 用pnp 求解 变换矩阵 [R t] Mat inliers_idx; vector<Point2f> list_points2d_inliers; if(good_matches.size() > 0) // None matches, then RANSAC crashes { //=======【3】Step 3: 使用PnP + Ransac进行姿态估计 Estimate the pose using RANSAC approach================== // 使用PnP + Ransac进行姿态估计(Pose estimation using PnP + Ransac) pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match, pnpMethod, inliers_idx, iterationsCount, reprojectionError, confidence ); //======【4】显示 PNP求解后 得到的内点 2D Step 4: Catch the inliers keypoints to draw=========================== for(int inliers_index = 0; inliers_index < inliers_idx.rows; ++inliers_index) { int n = inliers_idx.at<int>(inliers_index); // 2D点符合变换的 内点 索引 Point2f point2d = list_points2d_scene_match[n]; // 2D点符合变换的 内点 list_points2d_inliers.push_back(point2d); // 2D点符合变换的 内点序列 } // 显示 PNP求解后 得到的内点 2D draw2DPoints(frame_vis, list_points2d_inliers, blue); //======【5】使用线性卡尔曼滤波去除错误的姿态估计 Step 5: Kalman Filter============================== // 使用线性卡尔曼滤波去除错误的姿态估计(Linear Kalman Filter for bad poses rejection) good_measurement = false; // 随机序列采样 PNP之后 的内点数量 > 卡尔曼滤波所需的内点数量 就比较好 if( inliers_idx.rows >= minInliersKalman ) { // 平移向量 t Mat translation_measured(3, 1, CV_64F); translation_measured = pnp_detection.get_t_matrix(); // 旋转矩阵 R Mat rotation_measured(3, 3, CV_64F); rotation_measured = pnp_detection.get_R_matrix(); // 卡尔曼滤波测量矩阵 fillMeasurements(measurements, translation_measured, rotation_measured); good_measurement = true;//前期初匹配较好 } // 卡尔曼估计的 [R t] Mat translation_estimated(3, 1, CV_64F); Mat rotation_estimated(3, 3, CV_64F); // 更新卡尔曼滤波器 更新预测值 update the Kalman filter with good measurements updateKalmanFilter( KF, measurements, translation_estimated, rotation_estimated); //==== 【6】更新pnp 的 变换矩阵 Step 6: Set estimated projection matrix pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated); } //=====【7】显示位姿 轴 帧率 可信度 Step X: Draw pose=================================================== if(good_measurement) { drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // 原来PNP求解得到的位姿 //drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // 再经过卡尔曼滤波后得到的位姿 } else//匹配量较少的时候 我在使用 卡尔曼滤波得到的位姿 { drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose //drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // 原来PNP求解得到的位姿 } // 显示坐标轴 float l = 5;//轴长度 vector<Point2f> pose_points2d; pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,0))); // 世界坐标 原点axis center pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(l,0,0))); // x轴 axis x pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,l,0))); // y轴 axis y pose_points2d.push_back(pnp_detection_est.backproject3DPoint(Point3f(0,0,l))); // z轴 axis z draw3DCoordinateAxes(frame_vis, pose_points2d);// 显示坐标轴 // FRAME RATE // see how much time has elapsed time(&end);//结束时间 当前时间 // calculate current FPS ++counter;// 处理了一张图像 帧数+1 sec = difftime (end, start);//处理的总时间 fps = counter / sec;//帧率 drawFPS(frame_vis, fps, yellow);// 显示帧率 frame ratio double detection_ratio = ((double)inliers_idx.rows/(double)good_matches.size())*100;//显示内点数量所占比例 drawConfidence(frame_vis, detection_ratio, yellow);//显示内点所占比例 //==== 【8】显示调试数据 Step X: Draw some debugging text // Draw some debug text int inliers_int = inliers_idx.rows;// 内点数量 int outliers_int = (int)good_matches.size() - inliers_int;// string inliers_str = IntToString(inliers_int);// 内点数量 string outliers_str = IntToString(outliers_int); string n = IntToString((int)good_matches.size()); string text = "Found " + inliers_str + " of " + n + " matches";// string text2 = "Inliers: " + inliers_str + " - Outliers: " + outliers_str; drawText(frame_vis, text, green); drawText2(frame_vis, text2, red); imshow("REAL TIME DEMO", frame_vis); } // Close and Destroy Window destroyWindow("REAL TIME DEMO"); cout << "GOODBYE ..." << endl; } /**********************************************************************************************************/ void help() { cout << "--------------------------------------------------------------------------" << endl << "This program shows how to detect an object given its 3D textured model. You can choose to " << "use a recorded video or the webcam." << endl << "Usage:" << endl << "./cpp-tutorial-pnp_detection -help" << endl << "Keys:" << endl << "'esc' - to quit." << endl << "--------------------------------------------------------------------------" << endl << endl; } /**********************************************************************************************************/ // 初始化卡尔曼滤波器 void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt) { // cv 内置初始化 状态维度 测量维度 状态控制无 KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); // 过程噪声 set process noise setIdentity(KF.measurementNoiseCov, Scalar::all(1e-3)); // 测量噪声 set measurement noise 1e-2 setIdentity(KF.errorCovPost, Scalar::all(1)); // 协方差矩阵 单位阵 // 状态协方差矩阵18*18 各个状态之间的相关关系 状态维度 18 // //Fk : KF.transitionMatrix 状态转移矩阵 //Hk : KF.measurementMatrix 测量矩阵 //Qk : KF.processNoiseCov 过程噪声 方差 //Rk : KF.measurementNoiseCov 测量噪声 方差 //Pk : KF.errorCovPost 协方差矩阵 //有时也需要定义Bk : KF.controlMatrix 控制传递矩阵 // Xk = Fk * Xk-1 + () + Wk ; 状态过程传递 Wk为 过程噪声 ~N(0, Qk) // Zk = Hk * Xk + VK ; 测量值的传递 Hk为测量矩阵 VK为 测量过程噪声 ~N(0, Rk) //协方差矩阵的求解 // Pk = Fk * Pk-1 * Fk转置 + Qk // 卡尔曼增益 // K = Pk * H转置 * (H * Pk * H转置 + Rk)逆 // 更新状态 // Xk = Xk + K (真实测量值 - Hk * Xk ) // 更新 协方差矩阵 // Pk = Pk + K * H * Pk = (I + K * H )* Pk // 转移矩阵 A // A * X // [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0] // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] // [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0] // [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0] // [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2] // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1] // position KF.transitionMatrix.at<double>(0,3) = dt; KF.transitionMatrix.at<double>(1,4) = dt; KF.transitionMatrix.at<double>(2,5) = dt; KF.transitionMatrix.at<double>(3,6) = dt; KF.transitionMatrix.at<double>(4,7) = dt; KF.transitionMatrix.at<double>(5,8) = dt; KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2); // orientation KF.transitionMatrix.at<double>(9,12) = dt; KF.transitionMatrix.at<double>(10,13) = dt; KF.transitionMatrix.at<double>(11,14) = dt; KF.transitionMatrix.at<double>(12,15) = dt; KF.transitionMatrix.at<double>(13,16) = dt; KF.transitionMatrix.at<double>(14,17) = dt; KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2); // 测量矩阵 // [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0] // [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0] // [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0] KF.measurementMatrix.at<double>(0,0) = 1; // x KF.measurementMatrix.at<double>(1,1) = 1; // y KF.measurementMatrix.at<double>(2,2) = 1; // z KF.measurementMatrix.at<double>(3,9) = 1; // roll KF.measurementMatrix.at<double>(4,10) = 1; // pitch KF.measurementMatrix.at<double>(5,11) = 1; // yaw } /**********************************************************************************************************/ // 更新卡尔曼滤波器 卡尔曼增益K 协方差矩阵 更新状态矩阵 void updateKalmanFilter( KalmanFilter &KF, Mat &measurement, Mat &translation_estimated, Mat &rotation_estimated ) { // First predict, to update the internal statePre variable Mat prediction = KF.predict();// 状态传递 预测 // The "correct" phase that is going to use the predicted value and our measurement Mat estimated = KF.correct(measurement);// 得到误差 // 估计的平移矩阵 Estimated translation translation_estimated.at<double>(0) = estimated.at<double>(0); translation_estimated.at<double>(1) = estimated.at<double>(1); translation_estimated.at<double>(2) = estimated.at<double>(2); // 估计的欧拉角 Estimated euler angles Mat eulers_estimated(3, 1, CV_64F); eulers_estimated.at<double>(0) = estimated.at<double>(9); eulers_estimated.at<double>(1) = estimated.at<double>(10); eulers_estimated.at<double>(2) = estimated.at<double>(11); // 欧拉角转换到旋转矩阵 rotation_estimated = euler2rot(eulers_estimated); } /**********************************************************************************************************/ // 设置卡尔曼滤波器的 测量数据 旋转矩阵 + 平移向量 void fillMeasurements( Mat &measurements, const Mat &translation_measured, const Mat &rotation_measured) { // 旋转矩阵转换到欧拉角 Mat measured_eulers(3, 1, CV_64F);//欧拉角 row pitch yaw measured_eulers = rot2euler(rotation_measured);// // 设置6维的 测量数据 Set measurement to predict measurements.at<double>(0) = translation_measured.at<double>(0); // x measurements.at<double>(1) = translation_measured.at<double>(1); // y measurements.at<double>(2) = translation_measured.at<double>(2); // z measurements.at<double>(3) = measured_eulers.at<double>(0); // roll measurements.at<double>(4) = measured_eulers.at<double>(1); // pitch measurements.at<double>(5) = measured_eulers.at<double>(2); // yaw }