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
// ２d-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; // 匹配的对应场景图片中 的 ２ｄ点
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
}