opencv 纹理对象的实时姿态估计 PLY网格模型 三维纹理 ORB鲁棒匹配 PnPRansac 卡尔曼滤波去除错误的姿态估计
github
博文末尾支持二维码赞赏哦 _
# 纹理对象的实时姿态估计
[官方参考](https://docs.opencv.org/trunk/dc/d2c/tutorial_real_time_pose.html)
# 运行
一、生成物体三维纹理模型数据 ====
首先 Data/box.ply mesh 文件
提供了 单张图片中 盒子 8个定点的 3d坐标点信息(以某个定点为世界坐标系原点)
以及 6个 长方形面 构成的 12个三角形
我们首先需要 生成其 3d描述 信息,需要运行 src/main_registration.cpp 获取
1. 手动指定 图像中 物体顶点的位置(得到相机像素坐标系下的2d坐标 对应 ply文件中是3D位置)
2. 求取 欧式变换矩阵
由对应的 2d-3d点对关系
u
v = K × [R t] X
1 Y
Z
1
K 为图像拍摄时 相机的内参数
世界坐标中的三维点(以文件中坐标为(0,0,0)某个定点为世界坐标系原点)
经过 旋转矩阵R 和平移向量t 变换到相机坐标系下
在通过相机内参数 变换到 相机的图像平面上
由 PnP 算法可解的 旋转矩阵R 和平移向量t
3. 把从图像中得到的纹理信息 加入到 物体的三维纹理模型中
在图像中提取 ORB特征点 和 对应的描述子
利用 内参数K 、 旋转矩阵R 和平移向量t 反向投影2d像素点 到 三维空间,获取3d坐标
标记 该反投影的3d点 是否在三维物体的 某个平面上
4. 将 2d-3d点对 、关键点 以及 关键点描述子 存 入 物体的三维纹理模型中
Data/cookies_ORB.yml
运行:
./pnp_registration
二、使用数据库对视频/拍摄图像 进行实时 检测 目标定位
主程序 src/main_detection.cpp
1. 读取网格数据文件Data/box.ply 和 三维纹理数据文件Data/cookies_ORB.yml (上一步获取) 获取3d描述数据库
2. 对真实场景(视频文件帧/摄像头拍摄数据) 提取特征点2D 及其 描述子
3. 与模型库中的 3d点带有的 描述子进行匹配,得到 2d-3d匹配点
4. 使用PnP + Ransac 估计 当前物体的姿态 (R,t)
5. 显示 PNP求解后 得到的内点
6. 使用线性卡尔曼滤波去除错误的姿态估计变换矩阵 (R,t)
7. 更新pnp 的 变换矩阵
8. 将数据库中的 8个3D顶点 使用(R,t) 反投影到 图像2D平面上,绘制3D框,显示姿态
运行
./pnp_detection
# 项目分析
## 1 PLY网格模型,CSV格式的ply文件类
src/CsvReader.cpp
src/CsvWriter.cpp
## 2 三维纹理
src/Mesh.cpp 实际CsvReader类读取了文件
src/Model.cpp 模型读取保存 添加关键点、描述子、外点
物体网格模型 物体的 三维纹理 模型文件
包含:2d-3d点对 特征点 特征点描述子
## 3 ORB鲁棒匹配
src/RobustMatcher.cpp
鲁棒匹配 两者相互看对眼了
图像1匹配到图像2的点 和图像2 匹配 图像1的点相互对应 才算是匹配
## 4 PnPRansac 2d-3d点对匹配 Rt变换求解器
src/PnPProblem.cpp
## 5 由简单的 长方体 顶点 面 描述的ply文件 和 物体的彩色图像 生产 物体的三维纹理模型文件
src/ModelRegistration.cpp 二维图像创建 三维数据 textured 3D model.
程序执行,首先从输入图像中提取 ORB特征描述子,
然后使用网格数据和 Möller–Trumbore intersection 算法
来计算特征的3D坐标系。
最后,3D坐标点和特征描述子存在YAML格式文件的不同列表中,
每一行存储一个不同的点.
src/main_registration.cpp
由简单的 长方体 顶点 面 描述的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点对 、关键点 以及 关键点描述子 存入物体的三维纹理模型中
## 6 纹理对象的实时姿态估计
src/main_detection.cpp
【0】 读取网格数据文件 和 三维纹理数据文件 获取3d描述数据库
设置特征检测器 描述子提取器 描述子匹配器
【1】 场景中提取特征点 描述子 在 模型库(3d描述子数据库
3d points +description )中提取 匹配点
【2】 获取场景图片中 和 模型库中匹配的2d-3d点对
【3】 使用PnP + Ransac进行姿态估计
【4】 显示 PNP求解后 得到的内点
【5】 使用线性卡尔曼滤波去除错误的姿态估计
【6】 更新pnp 的 变换矩阵
【7】 显示位姿 轴 帧率 可信度
【8】显示调试数据
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
}