OpenCV纹理对象的实时姿态估计
现在，增强现实是计算机视觉和机器人领域的顶尖研究课题之一。增强现实中最重要的问题是在计算机视觉区域的情况下估计对象的摄像机姿态，以后做一些3D渲染，或者在机器人获取对象姿态以便掌握对象并进行某些操作的情况下。然而，由于图像处理中最常见的问题是应用大量算法或数学运算来解决对于人类是基本的和直接的问题的计算成本，所以这不是一个简单的问题。
目标
在本教程中解释了如何构建实时应用程序来估计摄像机姿态，以便在给定2D图像及其3D纹理模型的情况下跟踪具有六个自由度的纹理对象。
应用程序将具有以下部分：
- 阅读3D纹理对象模型和对象网格。
- 从相机或视频输入。
- 从场景中提取ORB特征和描述符。
- 使用Flann匹配器匹配具有模型描述符的场景描述符。
- 使用PnP + Ransac进行姿态估计。
- 线性卡尔曼滤波器用于不良姿势排斥。
理论
在计算机视觉估计相机姿态从n 3D到2D对应点是基本的和易于理解的问题。问题的最普遍版本需要估计姿态的六个自由度和五个校准参数：焦距，主点，纵横比和偏斜。可以使用众所周知的直接线性变换（DLT）算法，至少建立6个对应关系。然而，有几个简化的问题变成了广泛的不同算法列表，提高了DLT的准确性。
最常见的简化是假定已知的校准参数是所谓的Perspective-*n*-Point 问题：
问题制定：给定在世界参考帧中表示的3D点在图像上的2D投影之间的一组对应关系，我们寻求检索摄像机拍摄世界焦点的长度f的姿态 （R and t)
OpenCV提供四种不同的方法来解决返回R 和 t的Perspective-* n * -Point问题。然后，使用以下公式，可以将3D点投影到图像平面中：
使用该方程式进行管理的完整文档在相机校准和3D重建中。
源代码
您可以samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/在OpenCV源库的文件夹中找到本教程的源代码。
该教程由两个主要程序组成：
型号注册
此应用程序是不具有要检测对象的3D纹理模型的独家。您可以使用此程序创建自己的纹理3D模型。此程序仅适用于平面对象，则如果要对具有复杂形状的对象建模，则应使用复杂的软件来创建它。
应用程序需要要注册的对象的输入图像及其3D网格。我们还提供摄像机的内在参数，使用该参数拍摄输入图像。需要使用绝对路径或应用程序工作目录中的相对路径指定所有文件。如果没有指定文件，程序将尝试打开提供的默认参数。
应用程序启动从输入图像中提取ORB特征和描述符，然后使用网格与Möller-Trumbore交集算法来计算找到的特征的3D坐标。最后，3D点和描述符以YAML格式的文件存储在不同的列表中，每个行是不同的点。有关如何存储文件的技术背景，请参见使用XML和YAML文件的文件输入和输出教程。
模型检测
该应用的目的是实时估计其3D纹理模型的对象姿势。
应用程序以与模型注册程序中解释的相同结构启动以YAML文件格式加载3D纹理模型。从现场，ORB特征和描述符被检测和提取。然后，使用cv :: FlannBasedMatcher与cv :: flann :: GenericIndex进行场景描述符和模型描述符之间的匹配。使用找到的匹配以及cv :: solvePnPRansac函数R和t相机进行计算。最后，应用卡尔曼滤波器来排除不良姿态。
在使用示例编译OpenCV的情况下，可以在opencv / build / bin / cpp-tutorial-pnp_detection中找到它。然后可以运行应用程序并更改一些参数：
This program shows how to detect an object given its 3D textured model. You can choose to use a recorded video or the webcam.
Usage:
./cpp-tutorial-pnp_detection -help
Keys:
'esc' - to quit.
--------------------------------------------------------------------------
Usage: cpp-tutorial-pnp_detection [params]
-c, --confidence (value:0.95)
RANSAC confidence
-e, --error (value:2.0)
RANSAC reprojection errror
-f, --fast (value:true)
use of robust fast match
-h, --help (value:true)
print this message
--in, --inliers (value:30)
minimum inliers for Kalman update
--it, --iterations (value:500)
RANSAC maximum iterations count
-k, --keypoints (value:2000)
number of keypoints to detect
--mesh
path to ply mesh
--method, --pnp (value:0)
PnP method: (0) ITERATIVE - (1) EPNP - (2) P3P - (3) DLS
--model
path to yml model
-r, --ratio (value:0.7)
threshold for ratio test
-v, --video
path to recorded video例如，您可以运行更改pnp方法的应用程序：
./cpp-tutorial-pnp_detection --method = 2
说明
这里详细说明了实时应用程序的代码：
- 阅读3D纹理对象模型和对象网格。为了加载纹理模型，我实现了具有打开YAML文件的函数load（）的类 Model，并使用其对应的描述符获取存储的3D点。您可以在其中找到3D纹理模型的示例。samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml
/* Load a YAML file using OpenCV */
void Model::load(const std::string path)
{
cv::Mat points3d_mat;
cv::FileStorage storage(path, cv::FileStorage::READ);
storage["points_3d"] >> points3d_mat;
storage["descriptors"] >> descriptors_;
points3d_mat.copyTo(list_points3d_in_);
storage.release();
}在主程序中，模型加载如下：
Model model; // instantiate Model object
model.load(yml_read_path); // load a 3D textured object model为了读取模型网格，我实现了一个类 Mesh，它有一个函数load（），它打开一个 .ply文件，并存储对象的3D点以及组合的三角形。您可以在其中找到模型网格的示例。*samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply
/* Load a CSV with *.ply format */
void Mesh::load(const std::string path)
{
// Create the reader
CsvReader csvReader(path);
// Clear previous data
list_vertex_.clear();
list_triangles_.clear();
// Read from .ply file
csvReader.readPLY(list_vertex_, list_triangles_);
// Update mesh attributes
num_vertexs_ = list_vertex_.size();
num_triangles_ = list_triangles_.size();
}在主程序中，网格加载如下：
Mesh mesh; // instantiate Mesh object
mesh.load(ply_read_path); // load an object mesh您还可以加载不同的模型和网格：
./cpp-tutorial-pnp_detection --mesh = / absolute_path_to_your_mesh.ply --model = / absolute_path_to_your_model.yml
- 从相机或视频输入
检测是必要的捕获视频。它通过传递其位于机器中的绝对路径来加载录制的视频。为了测试应用程序，您可以在其中找到录制的视频samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.mp4。
cv::VideoCapture cap; // instantiate VideoCapture
cap.open(video_read_path); // open a recorded video
if(!cap.isOpened()) // check if we succeeded
{
std::cout << "Could not open the camera device" << std::endl;
return -1;
}那么该算法是每帧计算的帧：
cv::Mat frame, frame_vis;
while(cap.read(frame) && cv::waitKey(30) != 27) // capture frame until ESC is pressed
{
frame_vis = frame.clone(); // refresh visualisation frame
// MAIN ALGORITHM
}您还可以加载不同的录制视频：
./cpp-tutorial-pnp_detection --video=/absolute_path_to_your_video.mp4- 从场景中提取ORB特征和描述符
下一步是检测场景特征并提取其描述符。对于这个任务，我实现了一个RobustMatcher 类 ，它具有关键点检测和特征提取的功能。你可以找到它。在您的RobusMatch对象中，您可以使用OpenCV的任何二维特征检测器。在这种情况下，我使用cv :: ORB功能，因为基于cv :: FAST来检测关键点和cv :: xfeatures2d :: BriefDescriptorExtractor来提取描述符，这意味着对于旋转是快速和健康的。您可以在文档中找到有关ORB的更多详细信息。samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobusMatcher.cpp
以下代码是如何实例化和设置特征检测器和描述符提取器：
RobustMatcher rmatcher; // instantiate RobustMatcher
cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints); // instatiate ORB feature detector
cv::DescriptorExtractor * extractor = new cv::OrbDescriptorExtractor(); // instatiate ORB descriptor extractor
rmatcher.setFeatureDetector(detector); // set feature detector
rmatcher.setDescriptorExtractor(extractor); // set descriptor extractor功能和描述符将由匹配函数内的RobustMatcher计算。
- 使用Flann匹配器匹配具有模型描述符的场景描述符
这是我们的检测算法的第一步。主要思想是将场景描述符与我们的模型描述符进行匹配，以便将找到的特征的3D坐标知道到当前场景中。
首先，我们必须设置我们要使用的匹配器。在这种情况下，使用cv :: FlannBasedMatcher匹配器，它的计算成本比cv :: BFMatcher匹配器快，因为我们增加了训练有素的特征集合。然后，对于FlannBased匹配器，创建的索引是多探针LSH：由ORB描述符引起的用于高维相似性搜索的高效索引是二进制的。
您可以调整LSH和搜索参数以提高匹配效率：
cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters
cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>(50); // instantiate flann search parameters
cv::DescriptorMatcher * matcher = new cv::FlannBasedMatcher(indexParams, searchParams); // instantiate FlannBased matcher
rmatcher.setDescriptorMatcher(matcher); // set matcher其次，我们必须通过使用robustMatch（）或fastRobustMatch（）函数调用匹配器。使用这两个功能的差异在于其计算成本。第一种方法是较慢，但是在过滤良好的匹配时更稳健，因为使用两个比率测试和对称性测试。相比之下，第二种方法更快，但不太稳健，因为仅对比赛应用单一比例测试。
以下代码是获取模型3D点及其描述符，然后在主程序中调用匹配器：
// Get the MODEL INFO
std::vector<cv::Point3f> list_points3d_model = model.get_points3d(); // list with model 3D coordinates
cv::Mat descriptors_model = model.get_descriptors(); // list with descriptors of each 3D coordinate// -- Step 1: Robust matching between model descriptors and scene descriptors
std::vector<cv::DMatch> good_matches; // to obtain the model 3D points in the scene
std::vector<cv::KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
if(fast_match)
{
rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);
}
else
{
rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model);
}以下代码对应于属于RobustMatcher类的robustMatch（）函数。该函数使用给定图像来检测关键点并提取描述符，使用给定模型描述符的两个最近邻提取描述符进行匹配，反之亦然。然后，将比例测试应用于两个方向匹配，以便去除其第一和第二最佳匹配之间的距离比大于给定阈值的这些匹配。最后，按照去除非对称匹配来应用对称性测试。
void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
std::vector<cv::KeyPoint>& keypoints_frame,
const std::vector<cv::KeyPoint>& keypoints_model, 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
int removed1 = ratioTest(matches12);
// clean image 2 -> image 1 matches
int removed2 = ratioTest(matches21);
// 4. Remove non-symmetrical matches
symmetryTest(matches12, matches21, good_matches);
}在匹配滤波后，我们必须使用获得的DMatches矢量从所找到的场景关键点和我们的3D模型中减去2D和3D对应。有关cv :: DMatch的更多信息，请查看文档。
// -- Step 2: Find out the 2D/3D correspondences
std::vector<cv::Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
std::vector<cv::Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
{
cv::Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 3D point from model
cv::Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 2D point from the scene
list_points3d_model_match.push_back(point3d_model); // add 3D point
list_points2d_scene_match.push_back(point2d_scene); // add 2D point
}您还可以更改比例测试阈值，要检测的关键点数以及使用或不使用稳健匹配器：
./cpp-tutorial-pnp_detection --ratio = 0.8 --keypoints = 1000 --fast = false
使用PnP + Ransac进行姿态估计
一旦与2D和3D对应，我们必须应用PnP算法来估计相机姿态。我们必须使用cv :: solvePnPRansac而不是cv :: solvePnP的原因是因为在匹配之后，并不是所有找到的对应关系都是正确的，并且如同没有，有错误的对应关系或也称为异常值。该随机抽样一致性或RANSAC是估计从观察到的数据来产生aproximate结果的数学模型的参数作为迭代次数增加一非确定性迭代方法。雷神之后，所有的异常值 将被消除，然后以一定概率估计摄像机姿势以获得良好的解决方案。
对于相机姿态估计，我已经实现了类 PnPProblem。这个类有4个atributes：一个给定的校准矩阵，旋转矩阵，平移矩阵和旋转，平移矩阵。您用于估计姿势的相机的固有校准参数是必要的。为了获得参数，您可以使用方形棋盘和相机校准来检查相机校准使用OpenCV教程。
以下代码是如何在主程序中声明PnPProblem类：
// Intrinsic camera parameters: UVC WEBCAM
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
PnPProblem pnp_detection(params_WEBCAM); // instantiate PnPProblem class以下代码是PnPProblem类如何初始化其属性：
// Custom constructor given the intrinsic camera parameters
PnPProblem::PnPProblem(const double params[])
{
_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_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // rotation matrix
_t_matrix = cv::Mat::zeros(3, 1, CV_64FC1); // translation matrix
_P_matrix = cv::Mat::zeros(3, 4, CV_64FC1); // rotation-translation matrix
}OpenCV提供四种PnP方法：ITERATIVE，EPNP，P3P和DLS。根据应用类型，估算方法将不同。在我们想要实时应用的情况下，EPNP和P3P的适用方法比ITERATIVE和DLS更快找到最佳解决方案。然而，EPNP和P3P在平面之前并不是特别坚固，有时姿态估计似乎具有镜像效果。因此，在本教程中使用ITERATIVE方法由于要检测的对象具有平面表面。
OpenCV Ransac实现希望您提供三个参数：直到停止算法的最大迭代次数，观察点和计算点投影之间的最大允许距离，以将其视为一个非常高的值，并获得一个良好的结果的置信度。您可以调整这些参数，以提高算法性能。增加迭代次数，您将获得更准确的解决方案，但需要更多时间才能找到解决方案。增加重新投影错误将减少计算时间，但是您的解决方案将不正确。减少您的算法更快的信心，但获得的解决方案将不正确。
以下参数适用于此应用程序：
// RANSAC parameters
int iterationsCount = 500; // number of Ransac iterations.
float reprojectionError = 2.0; // maximum allowed distance to consider it an inlier.
float confidence = 0.95; // ransac successful confidence.以下代码对应于属于PnPProblem类的estimatePoseRANSAC（）函数。该函数估计给定一组2D / 3D对应关系的旋转和平移矩阵，使用的所需PnP方法，输出内容容器和Ransac参数：
// 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, // list with model 3D coordinates
const std::vector<cv::Point2f> &list_points2d, // list with scene 2D coordinates
int flags, cv::Mat &inliers, int iterationsCount, // PnP method; inliers container
float reprojectionError, float confidence ) // Ransac parameters
{
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1); // vector of distortion coefficients
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector
bool useExtrinsicGuess = false; // if true the function uses the provided rvec and tvec values as
// initial approximations of the rotation and translation vectors
cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
inliers, flags );
Rodrigues(rvec,_R_matrix); // converts Rotation Vector to Matrix
_t_matrix = tvec; // set translation matrix
this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix
}下面的代码是主算法的第3和第4步。第一个，调用上述函数，第二个采用Ransac的输出内容向量获取2D场景点进行绘图。如代码中所示，我们必须确保应用Ransac，如果我们有匹配，在另一种情况下，函数cv :: solvePnPRansac由于任何OpenCV 错误而崩溃。
if(good_matches.size() > 0) // None matches, then RANSAC crashes
{
// -- Step 3: Estimate the pose using RANSAC approach
pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match,
pnpMethod, inliers_idx, iterationsCount, reprojectionError, confidence );
// -- 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); // i-inlier
cv::Point2f point2d = list_points2d_scene_match[n]; // i-inlier point 2D
list_points2d_inliers.push_back(point2d); // add i-inlier to list
}最后，一旦估计了相机姿态，我们可以使用R and t以便使用理论上显示的公式将2D投影计算到在世界参考系中表示的给定3D点的图像上。
以下代码对应于属于PnPProblem类的backproject3DPoint （）函数。函数backproject将给定的3D点在世界参考帧中表示为2D图像：
// 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;
// Normalization of [u v]'
cv::Point2f point2d;
point2d.x = point2d_vec.at<double>(0) / point2d_vec.at<double>(2);
point2d.y = point2d_vec.at<double>(1) / point2d_vec.at<double>(2);
return point2d;
}上述函数用于计算对象网格的所有3D点，以显示对象的姿态。
您还可以更改RANSAC参数和PnP方法：
./cpp-tutorial-pnp_detection --error=0.25 --confidence=0.90 --iterations=250 --method=3
- 线性卡尔曼滤波器用于不良姿态排斥
在计算机视觉或机器人领域常见的是，在应用检测或跟踪技术之后，由于某些传感器错误而获得了不良的结果。为了避免这些不良检测在本教程中解释如何实现一个线性卡尔曼滤波器。卡尔曼滤波器将在检测到一定数量的内部值之后被应用。
您可以找到更多有关卡尔曼滤波器的信息。在本教程中，它使用基于线性卡尔曼滤波器的cv :: KalmanFilter的OpenCV实现，用于位置和方向跟踪，以设置动态和测量模型。
首先，我们必须定义我们的状态向量，它将具有18个状态：位置数据（x，y，z）以其第一和第二个导数（速度和加速度），然后旋转以三个欧拉角（滚动，俯仰，下巴）以及它们的第一和第二导数（角速度和加速度）
其次，我们必须定义将是6的度量的数量：从R and t 我们可以提取 (x,y,z)和 (ψ,θ,ϕ).。此外，我们必须定义要应用于系统的控制动作的数量，在这种情况下，系统将为零。最后，我们必须定义在这种情况下为测量之间的差分时间1/T，其中 T是视频的帧速率。
cv::KalmanFilter KF; // instantiate Kalman Filter
int nStates = 18; // the number of states
int nMeasurements = 6; // the number of measured states
int nInputs = 0; // the number of action control
double dt = 0.125; // time between measurements (1/FPS)
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function以下代码对应于卡尔曼滤波器初始化。首先，设置过程噪声，测量噪声和误差协方差矩阵。其次，设定作为动态模型的过渡矩阵，最后设定作为测量模型的测量矩阵。
您可以调整过程和测量噪声，以提高卡尔曼滤波器的性能。随着测量噪声的减小，在不良测量前面，算法敏感度越高，会越快。
void initKalmanFilter(cv::KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt)
{
KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter
cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5)); // set process noise
cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-4)); // set measurement noise
cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1)); // error covariance
/* DYNAMIC MODEL */
// [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);
/* MEASUREMENT MODEL */
// [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
}下面的代码是主算法的第5步。当所获得的Ransac之后的内联值超过阈值时，填充测量矩阵，然后更新卡尔曼滤波器：
// -- Step 5: Kalman Filter
// GOOD MEASUREMENT
if( inliers_idx.rows >= minInliersKalman )
{
// Get the measured translation
cv::Mat translation_measured(3, 1, CV_64F);
translation_measured = pnp_detection.get_t_matrix();
// Get the measured rotation
cv::Mat rotation_measured(3, 3, CV_64F);
rotation_measured = pnp_detection.get_R_matrix();
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
}
// Instantiate estimated translation and rotation
cv::Mat translation_estimated(3, 1, CV_64F);
cv::Mat rotation_estimated(3, 3, CV_64F);
// update the Kalman filter with good measurements
updateKalmanFilter( KF, measurements,
translation_estimated, rotation_estimated);以下代码对应于fillMeasurements（）函数，它将测量的旋转矩阵转换为欧拉角，并将测量矩阵与测量的平移向量相加：
void fillMeasurements( cv::Mat &measurements,
const cv::Mat &translation_measured, const cv::Mat &rotation_measured)
{
// Convert rotation matrix to euler angles
cv::Mat measured_eulers(3, 1, CV_64F);
measured_eulers = rot2euler(rotation_measured);
// 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
}以下代码对应于更新卡尔曼滤波器的updateKalmanFilter（）函数，并设置估计的旋转矩阵和平移向量。估计的旋转矩阵来自估计的欧拉角与旋转矩阵。
void updateKalmanFilter( cv::KalmanFilter &KF, cv::Mat &measurement,
cv::Mat &translation_estimated, cv::Mat &rotation_estimated )
{
// First predict, to update the internal statePre variable
cv::Mat prediction = KF.predict();
// The "correct" phase that is going to use the predicted value and our measurement
cv::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
cv::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);
// Convert estimated quaternion to rotation matrix
rotation_estimated = euler2rot(eulers_estimated);
}第6步设置估计旋转平移矩阵：
// -- Step 6: Set estimated projection matrix
pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);最后一个可选步骤是绘制发现的姿势。为了做到这一点，我实现了一个函数来绘制所有的网格3D点和一个额外的参考轴：
// -- Step X: Draw pose
drawObjectMesh(frame_vis, &mesh, &pnp_detection, green); // draw current pose
drawObjectMesh(frame_vis, &mesh, &pnp_detection_est, yellow); // draw estimated pose
double l = 5;
std::vector<cv::Point2f> pose_points2d;
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,0,0))); // axis center
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(l,0,0))); // axis x
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,l,0))); // axis y
pose_points2d.push_back(pnp_detection_est.backproject3DPoint(cv::Point3f(0,0,l))); // axis z
draw3DCoordinateAxes(frame_vis, pose_points2d); // draw axes您还可以修改最低内容以更新卡尔曼滤波器：
./cpp-tutorial-pnp_detection --inliers=20结果
以下视频是使用以下参数使用所述检测算法实时进行姿态估计的结果：
// Robust Matcher parameters
int numKeyPoints = 2000; // number of detected keypoints
float ratio = 0.70f; // ratio test
bool fast_match = true; // fastRobustMatch() or robustMatch()
// RANSAC parameters
int iterationsCount = 500; // number of Ransac iterations.
int reprojectionError = 2.0; // maximum allowed distance to consider it an inlier.
float confidence = 0.95; // ransac successful confidence.
// Kalman Filter parameters
int minInliersKalman = 30; // Kalman threshold updating