現(xiàn)在,增強現(xiàn)實是計算機視覺和機器人領(lǐng)域的頂尖研究課題之一。增強現(xiàn)實中最重要的問題是在計算機視覺區(qū)域的情況下估計對象的攝像機姿態(tài),以后做一些3D渲染,或者在機器人獲取對象姿態(tài)以便掌握對象并進行某些操作的情況下。然而,由于圖像處理中最常見的問題是應(yīng)用大量算法或數(shù)學運算來解決對于人類是基本的和直接的問題的計算成本,所以這不是一個簡單的問題。
在本教程中解釋了如何構(gòu)建實時應(yīng)用程序來估計攝像機姿態(tài),以便在給定2D圖像及其3D紋理模型的情況下跟蹤具有六個自由度的紋理對象。
應(yīng)用程序?qū)⒕哂幸韵虏糠郑?/p>
在計算機視覺估計相機姿態(tài)從n 3D到2D對應(yīng)點是基本的和易于理解的問題。問題的最普遍版本需要估計姿態(tài)的六個自由度和五個校準參數(shù):焦距,主點,縱橫比和偏斜??梢允褂帽娝苤闹苯泳€性變換(DLT)算法,至少建立6個對應(yīng)關(guān)系。然而,有幾個簡化的問題變成了廣泛的不同算法列表,提高了DLT的準確性。
最常見的簡化是假定已知的校準參數(shù)是所謂的Perspective-*n*-Point 問題:
問題制定:給定在世界參考幀中表示的3D點在圖像上的2D投影之間的一組對應(yīng)關(guān)系,我們尋求檢索攝像機拍攝世界焦點的長度f的姿態(tài) (R and t)
OpenCV提供四種不同的方法來解決返回R 和 t的Perspective-* n * -Point問題。然后,使用以下公式,可以將3D點投影到圖像平面中:
使用該方程式進行管理的完整文檔在相機校準和3D重建中。
您可以samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/在OpenCV源庫的文件夾中找到本教程的源代碼。
該教程由兩個主要程序組成:
此應(yīng)用程序是不具有要檢測對象的3D紋理模型的獨家。您可以使用此程序創(chuàng)建自己的紋理3D模型。此程序僅適用于平面對象,則如果要對具有復(fù)雜形狀的對象建模,則應(yīng)使用復(fù)雜的軟件來創(chuàng)建它。
應(yīng)用程序需要要注冊的對象的輸入圖像及其3D網(wǎng)格。我們還提供攝像機的內(nèi)在參數(shù),使用該參數(shù)拍攝輸入圖像。需要使用絕對路徑或應(yīng)用程序工作目錄中的相對路徑指定所有文件。如果沒有指定文件,程序?qū)L試打開提供的默認參數(shù)。
應(yīng)用程序啟動從輸入圖像中提取ORB特征和描述符,然后使用網(wǎng)格與M?ller-Trumbore交集算法來計算找到的特征的3D坐標。最后,3D點和描述符以YAML格式的文件存儲在不同的列表中,每個行是不同的點。有關(guān)如何存儲文件的技術(shù)背景,請參見使用XML和YAML文件的文件輸入和輸出教程。
該應(yīng)用的目的是實時估計其3D紋理模型的對象姿勢。
應(yīng)用程序以與模型注冊程序中解釋的相同結(jié)構(gòu)啟動以YAML文件格式加載3D紋理模型。從現(xiàn)場,ORB特征和描述符被檢測和提取。然后,使用cv :: FlannBasedMatcher與cv :: flann :: GenericIndex進行場景描述符和模型描述符之間的匹配。使用找到的匹配以及cv :: solvePnPRansac函數(shù)R和t相機進行計算。最后,應(yīng)用卡爾曼濾波器來排除不良姿態(tài)。
在使用示例編譯OpenCV的情況下,可以在opencv / build / bin / cpp-tutorial-pnp_detection中找到它。然后可以運行應(yīng)用程序并更改一些參數(shù):
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方法的應(yīng)用程序:
./cpp-tutorial-pnp_detection --method = 2
這里詳細說明了實時應(yīng)用程序的代碼:
/* 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
為了讀取模型網(wǎng)格,我實現(xiàn)了一個類 Mesh,它有一個函數(shù)load(),它打開一個 .ply文件,并存儲對象的3D點以及組合的三角形。您可以在其中找到模型網(wǎng)格的示例。*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();
}
在主程序中,網(wǎng)格加載如下:
Mesh mesh; // instantiate Mesh object
mesh.load(ply_read_path); // load an object mesh
您還可以加載不同的模型和網(wǎng)格:
./cpp-tutorial-pnp_detection --mesh = / absolute_path_to_your_mesh.ply --model = / absolute_path_to_your_model.yml
檢測是必要的捕獲視頻。它通過傳遞其位于機器中的絕對路徑來加載錄制的視頻。為了測試應(yīng)用程序,您可以在其中找到錄制的視頻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
下一步是檢測場景特征并提取其描述符。對于這個任務(wù),我實現(xiàn)了一個RobustMatcher 類 ,它具有關(guān)鍵點檢測和特征提取的功能。你可以找到它。在您的RobusMatch對象中,您可以使用OpenCV的任何二維特征檢測器。在這種情況下,我使用cv :: ORB功能,因為基于cv :: FAST來檢測關(guān)鍵點和cv :: xfeatures2d :: BriefDescriptorExtractor來提取描述符,這意味著對于旋轉(zhuǎn)是快速和健康的。您可以在文檔中找到有關(guān)ORB的更多詳細信息。samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobusMatcher.cpp
以下代碼是如何實例化和設(shè)置特征檢測器和描述符提取器:
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
功能和描述符將由匹配函數(shù)內(nèi)的RobustMatcher計算。
這是我們的檢測算法的第一步。主要思想是將場景描述符與我們的模型描述符進行匹配,以便將找到的特征的3D坐標知道到當前場景中。
首先,我們必須設(shè)置我們要使用的匹配器。在這種情況下,使用cv :: FlannBasedMatcher匹配器,它的計算成本比cv :: BFMatcher匹配器快,因為我們增加了訓練有素的特征集合。然后,對于FlannBased匹配器,創(chuàng)建的索引是多探針LSH:由ORB描述符引起的用于高維相似性搜索的高效索引是二進制的。
您可以調(diào)整LSH和搜索參數(shù)以提高匹配效率:
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()函數(shù)調(diào)用匹配器。使用這兩個功能的差異在于其計算成本。第一種方法是較慢,但是在過濾良好的匹配時更穩(wěn)健,因為使用兩個比率測試和對稱性測試。相比之下,第二種方法更快,但不太穩(wěn)健,因為僅對比賽應(yīng)用單一比例測試。
以下代碼是獲取模型3D點及其描述符,然后在主程序中調(diào)用匹配器:
// 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);
}
以下代碼對應(yīng)于屬于RobustMatcher類的robustMatch()函數(shù)。該函數(shù)使用給定圖像來檢測關(guān)鍵點并提取描述符,使用給定模型描述符的兩個最近鄰提取描述符進行匹配,反之亦然。然后,將比例測試應(yīng)用于兩個方向匹配,以便去除其第一和第二最佳匹配之間的距離比大于給定閾值的這些匹配。最后,按照去除非對稱匹配來應(yīng)用對稱性測試。
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矢量從所找到的場景關(guān)鍵點和我們的3D模型中減去2D和3D對應(yīng)。有關(guān)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
}
您還可以更改比例測試閾值,要檢測的關(guān)鍵點數(shù)以及使用或不使用穩(wěn)健匹配器:
./cpp-tutorial-pnp_detection --ratio = 0.8 --keypoints = 1000 --fast = false
使用PnP + Ransac進行姿態(tài)估計
一旦與2D和3D對應(yīng),我們必須應(yīng)用PnP算法來估計相機姿態(tài)。我們必須使用cv :: solvePnPRansac而不是cv :: solvePnP的原因是因為在匹配之后,并不是所有找到的對應(yīng)關(guān)系都是正確的,并且如同沒有,有錯誤的對應(yīng)關(guān)系或也稱為異常值。該隨機抽樣一致性或RANSAC是估計從觀察到的數(shù)據(jù)來產(chǎn)生aproximate結(jié)果的數(shù)學模型的參數(shù)作為迭代次數(shù)增加一非確定性迭代方法。雷神之后,所有的異常值 將被消除,然后以一定概率估計攝像機姿勢以獲得良好的解決方案。
對于相機姿態(tài)估計,我已經(jīng)實現(xiàn)了類 PnPProblem。這個類有4個atributes:一個給定的校準矩陣,旋轉(zhuǎn)矩陣,平移矩陣和旋轉(zhuǎn),平移矩陣。您用于估計姿勢的相機的固有校準參數(shù)是必要的。為了獲得參數(shù),您可以使用方形棋盤和相機校準來檢查相機校準使用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。根據(jù)應(yīng)用類型,估算方法將不同。在我們想要實時應(yīng)用的情況下,EPNP和P3P的適用方法比ITERATIVE和DLS更快找到最佳解決方案。然而,EPNP和P3P在平面之前并不是特別堅固,有時姿態(tài)估計似乎具有鏡像效果。因此,在本教程中使用ITERATIVE方法由于要檢測的對象具有平面表面。
OpenCV Ransac實現(xiàn)希望您提供三個參數(shù):直到停止算法的最大迭代次數(shù),觀察點和計算點投影之間的最大允許距離,以將其視為一個非常高的值,并獲得一個良好的結(jié)果的置信度。您可以調(diào)整這些參數(shù),以提高算法性能。增加迭代次數(shù),您將獲得更準確的解決方案,但需要更多時間才能找到解決方案。增加重新投影錯誤將減少計算時間,但是您的解決方案將不正確。減少您的算法更快的信心,但獲得的解決方案將不正確。
以下參數(shù)適用于此應(yīng)用程序:
// 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.
以下代碼對應(yīng)于屬于PnPProblem類的estimatePoseRANSAC()函數(shù)。該函數(shù)估計給定一組2D / 3D對應(yīng)關(guān)系的旋轉(zhuǎn)和平移矩陣,使用的所需PnP方法,輸出內(nèi)容容器和Ransac參數(shù):
// 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步。第一個,調(diào)用上述函數(shù),第二個采用Ransac的輸出內(nèi)容向量獲取2D場景點進行繪圖。如代碼中所示,我們必須確保應(yīng)用Ransac,如果我們有匹配,在另一種情況下,函數(shù)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
}
最后,一旦估計了相機姿態(tài),我們可以使用R and t以便使用理論上顯示的公式將2D投影計算到在世界參考系中表示的給定3D點的圖像上。
以下代碼對應(yīng)于屬于PnPProblem類的backproject3DPoint ()函數(shù)。函數(shù)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;
}
上述函數(shù)用于計算對象網(wǎng)格的所有3D點,以顯示對象的姿態(tài)。
您還可以更改RANSAC參數(shù)和PnP方法:
./cpp-tutorial-pnp_detection --error=0.25 --confidence=0.90 --iterations=250 --method=3
在計算機視覺或機器人領(lǐng)域常見的是,在應(yīng)用檢測或跟蹤技術(shù)之后,由于某些傳感器錯誤而獲得了不良的結(jié)果。為了避免這些不良檢測在本教程中解釋如何實現(xiàn)一個線性卡爾曼濾波器??柭鼮V波器將在檢測到一定數(shù)量的內(nèi)部值之后被應(yīng)用。
您可以找到更多有關(guān)卡爾曼濾波器的信息。在本教程中,它使用基于線性卡爾曼濾波器的cv :: KalmanFilter的OpenCV實現(xiàn),用于位置和方向跟蹤,以設(shè)置動態(tài)和測量模型。
首先,我們必須定義我們的狀態(tài)向量,它將具有18個狀態(tài):位置數(shù)據(jù)(x,y,z)以其第一和第二個導(dǎo)數(shù)(速度和加速度),然后旋轉(zhuǎn)以三個歐拉角(滾動,俯仰,下巴)以及它們的第一和第二導(dǎo)數(shù)(角速度和加速度)
其次,我們必須定義將是6的度量的數(shù)量:從R and t 我們可以提取 (x,y,z)和 (ψ,θ,?).。此外,我們必須定義要應(yīng)用于系統(tǒng)的控制動作的數(shù)量,在這種情況下,系統(tǒng)將為零。最后,我們必須定義在這種情況下為測量之間的差分時間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
以下代碼對應(yīng)于卡爾曼濾波器初始化。首先,設(shè)置過程噪聲,測量噪聲和誤差協(xié)方差矩陣。其次,設(shè)定作為動態(tài)模型的過渡矩陣,最后設(shè)定作為測量模型的測量矩陣。
您可以調(diào)整過程和測量噪聲,以提高卡爾曼濾波器的性能。隨著測量噪聲的減小,在不良測量前面,算法敏感度越高,會越快。
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之后的內(nèi)聯(lián)值超過閾值時,填充測量矩陣,然后更新卡爾曼濾波器:
// -- 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);
以下代碼對應(yīng)于fillMeasurements()函數(shù),它將測量的旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角,并將測量矩陣與測量的平移向量相加:
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
}
以下代碼對應(yīng)于更新卡爾曼濾波器的updateKalmanFilter()函數(shù),并設(shè)置估計的旋轉(zhuǎn)矩陣和平移向量。估計的旋轉(zhuǎn)矩陣來自估計的歐拉角與旋轉(zhuǎn)矩陣。
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步設(shè)置估計旋轉(zhuǎn)平移矩陣:
// -- Step 6: Set estimated projection matrix
pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);
最后一個可選步驟是繪制發(fā)現(xiàn)的姿勢。為了做到這一點,我實現(xiàn)了一個函數(shù)來繪制所有的網(wǎng)格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
您還可以修改最低內(nèi)容以更新卡爾曼濾波器:
./cpp-tutorial-pnp_detection --inliers=20
以下視頻是使用以下參數(shù)使用所述檢測算法實時進行姿態(tài)估計的結(jié)果:
// 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
更多建議: