\[s_1p_1 = kp
\]\[s_2p_2 = k(rp+t)
\]\[_2^t}rk} = 0
\]可以通過8點法求解本質矩陣進而得到\(r\)，\(t\)
主要基於視覺slam14講的**，稍微改動的測試，儘管能夠求解姿態但是並不十分準確，後續考慮使用雙目相機實現定位功能
#include #include #include #include #include "opencv2/features2d/features2d.hpp"
#include #include #include #include //#include "stdafx.h"
using namespace cv;
using namespace std;
void find_feature_matches(
const mat& img_1, const mat& img_2,
std::vector& keypoints_1,
std::vector& keypoints_2,
std::vector< dmatch >& matches);
void pose_estimation_2d2d(
std::vectorkeypoints_1,
std::vectorkeypoints_2,
std::vector< dmatch > matches,
mat& r, mat& t);
// 畫素座標轉相機歸一化座標
point2d pixel2cam(const point2d& p, const mat& k);
int main()
//將攝像頭從640*480改成320*240，速度從200ms提公升至50ms
320);
240);
320);
240);
//namedwindow("video", 1);
//namedwindow("video", 2);
//namedwindow("pts", 3);
//mat frame;
mat img_1;
mat img_2;
while (1)
vectorkeypoints_1, keypoints_2;
vectormatches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
//cout << "一共找到了" << matches.size() << "組匹配點" << endl;
//-- 估計兩張影象間運動
mat r, t;
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, r, t);
//cout << "r:" << endl << r << endl;
//cout << "t:" << endl << t << endl;
////-- 驗證e=t^r*scale
//mat t_x = (mat_(3, 3) <<
// 0, -t.at(2, 0), t.at(1, 0),
// t.at(2, 0), 0, -t.at(0, 0),
// -t.at(1.0), t.at(0, 0), 0);
//cout << "t^r=" << endl << t_x*r << endl;
////-- 驗證對極約束
//mat k = (mat_(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
//for (dmatch m : matches)
//waitkey(1);
} cap1.release();
return 0;
}void find_feature_matches(const mat& img_1, const mat& img_2,
std::vector& keypoints_1,
std::vector& keypoints_2,
std::vector< dmatch >& matches)
//printf("-- max dist : %f \n", max_dist);
//printf("-- min dist : %f \n", min_dist);
//當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設定乙個經驗值30作為下限.
for (int i = 0; i < descriptors_1.rows; i++) }
}point2d pixel2cam(const point2d& p, const mat& k)
void pose_estimation_2d2d(std::vectorkeypoints_1,
std::vectorkeypoints_2,
std::vector< dmatch > matches,
mat& r, mat& t)
//-- 計算基礎矩陣
mat fundamental_matrix;
fundamental_matrix = findfundamentalmat(points1, points2, cv_fm_8point);
//cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;
//-- 計算本質矩陣
point2d principal_point(409.717933143672, 69.8389243159229); //相機光心, 標定值
double focal_length = 1296.76842892674; //相機焦距, 標定值
mat essential_matrix;
essential_matrix = findessentialmat(points1, points2, focal_length, principal_point);
//cout << "essential_matrix is " << endl << essential_matrix << endl;
//-- 計算單應矩陣
mat homography_matrix;
homography_matrix = findhomography(points1, points2, ransac, 3);
//cout << "homography_matrix is " << endl << homography_matrix << endl;
//-- 從本質矩陣中恢復旋轉和平移資訊.
recoverpose(essential_matrix, points1, points2, r, t, focal_length, principal_point);
//cout << "r is " << endl << r << endl;
//cout << "t is " << endl << t << endl;
cout << r << endl;
}
視覺里程計01 ORB特徵提取與跟蹤
概念網上描述的很詳細，這裡簡單說一下 總的來說該演算法比surf還要快，而且準確率也很不錯。主體部分來自高翔的視覺slam14講的 在開頭加入了攝像頭讀取的 替換掉固定的演示。這裡用雙目攝像頭來進行匹配，實際上視覺里程計採用單目即可。include include include include i...
視覺慣性里程計中，速度的修正原理
只有視覺或者只有imu的資料的時候，絕對的速度都是乙個不可觀測量，但神奇的是，如果把兩者結合在一起，絕對速度就變成可觀測的了。原理可以大概這麼解釋 假設imu觀察到乙個加速度，那麼在加速之前和加速之後的量幀之間的移動距離的比例和絕對速度有關。速度需要至少三幀影象才能被觀測。視覺能直接觀察到三幀之間距...
imu預積分 視覺慣性里程計的IMU預積分模型
為什麼工程實踐中我們使用視覺與imu融合的解決方案即視覺慣性里程計 vio 來估計運動而不是簡單地使用視覺里程計 vo 視覺慣性里程計的感測器主要包括相機和慣性測量單元 imu 兩種感測器各有優缺點，vio的優勢就在於imu與相機的互補性。視覺感測器在大多數紋理豐富的場景中效果很好，但是遇到玻璃或白...