《視覺SLAM十四講精品總結》9:直接法和光流法
阿新 • • 發佈：2019-02-10
內容：
二者關係
光流法
直接法
一、二者關係
引出原因：關鍵點和描述子計算非常耗時，可以保留特徵點，使用光流法跟蹤特徵點運動。
關係：光流法描述畫素在影象中運動，直接法利用相機運動模型計算特徵點在下一時刻影象中位置。
使用條件：直接法利用影象的畫素灰度資訊計算相機運動，需要場景中存在明暗變化。
二、LK光流法
光流法常用來跟蹤角點的運動。之後用跟蹤的特徵點，用ICP、PnP或對極幾何估計相機運動。
適用場景：要求相機運動緩慢，採集頻率高
呼叫函式calcOpticalFlowPyrLK
void cv::calcOpticalFlowPyrLK(InputArray prevImg,nextImg,prevPts,nextPts, OutputArray status,err, Size winSize = Size(21, 21),int maxLevel = 3, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), int flags = 0,double minEigThreshold = 1e-4 )
1、對第一幀提取FAST特徵點存到keypoints中（list型別，之後還會刪除）
2、對其他幀用LK跟蹤特徵點
3. 更新keypoints列表，從prev_keypoints到next_keypoints
4. 畫出 keypoints出為圓圈
int main(int argc, char** argv) { string path_to_dataset = argv[1]; string associate_file = path_to_dataset + "/associate.txt"; //讀入txt文字到fin中 ifstream fin(associate_file); string rgb_file, depth_file, time_rgb, time_depth; // 因為要刪除跟蹤失敗的點，keypoints使用list，元素型別是Point2f，座標 // 逐幀操作的將color承接為last_color，然後color讀取新的圖 list< cv::Point2f > keypoints; cv::Mat color, depth, last_color; for (int index = 0; index<100; index++) { //讀入顏色和深度影象 fin >> time_rgb >> rgb_file >> time_depth >> depth_file; color = cv::imread(path_to_dataset + "/" + rgb_file); depth = cv::imread(path_to_dataset + "/" + depth_file, -1); // 1. 對第一幀提取FAST特徵點存到keypoints中 if (index == 0) { vector<cv::KeyPoint> kps; cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create(); detector->detect(color, kps); for (auto kp : kps) keypoints.push_back(kp.pt); last_color = color; continue; } //實際只有10張圖，後面10-99沒有資料所以一直continue掉，index秒到99 if (color.data == nullptr || depth.data == nullptr) continue; // 2. 對其他幀用LK跟蹤特徵點 vector<cv::Point2f> next_keypoints; vector<cv::Point2f> prev_keypoints; for (auto kp : keypoints) prev_keypoints.push_back(kp); //呼叫calcOpticalFlowPyrLK函式 //status匹配狀態，匹配上賦1，否則賦0； vector<unsigned char> status; vector<float> error; cv::calcOpticalFlowPyrLK(last_color, color, prev_keypoints, next_keypoints, status, error); // 3. 更新keypoints列表，從prev_keypoints到next_keypoints int i = 0; for (auto iter = keypoints.begin(); iter != keypoints.end(); i++) { //跟丟了刪除，iter保持當前位置，不會++，但i會++； //跟蹤失敗，next_keypoints中資料被跳過 if (status[i] == 0) { iter = keypoints.erase(iter); continue; } //跟蹤上的好點，才會讓iter指向， *iter = next_keypoints[i]; iter++; } cout << "tracked keypoints: " << keypoints.size() << endl; if (keypoints.size() == 0) { cout << "all keypoints are lost." << endl; break; } // 4. 畫出 keypoints出為圓圈 cv::Mat img_show = color.clone(); for (auto kp : keypoints) cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1); cv::imshow("corners", img_show); cv::waitKey(0); last_color = color; } return 0; }
- 從匹配資訊associate.txt輸入進去
fin >> time_rgb >> rgb_file >> time_depth >> depth_file;13050353.359684 rgb/13050353.359684.png 1305031453.374112 depth/1305031453.374112.png- list型別的keypoints初始化第一幀檢測角點後，將所有角點座標存入。他的更新只有減少，沒有增加(只有跟丟的點進行刪除)
- next_keypoints和prev_keypoints每幀跟蹤迴圈內被定義的，也就是每次進迴圈被定義，出迴圈被釋放.
list< Point2f > keypoints;
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;-
重點註釋：
int i = 0;
for (auto iter = keypoints.begin(); iter != keypoints.end(); i++)
{
if (status[i] == 0)
{
iter = keypoints.erase(iter);
continue;
}
*iter = next_keypoints[i];
iter++;
}iter是keypoints的迭代器，正常情況下更新keypoints為下一幀特徵點座標，直接*iter=next_keypoints[i]即可。i++,iter++二者同步更新；
特殊情況下，出現跟蹤失敗的特徵點，在keypoints先把這個位置刪除，同時.erase()迭代器返回的是下一個的位置。與此同時這裡的continue能結束本次迴圈，重新下一次迴圈i++，這樣可以跳過這個數值，下一次*iter=next_keypoints[i]時，iter已經指向了下一個位置，同時i++了，完美賦值；
三、直接法
1、隨著一批不需提取特徵的方法，如SVO（選取關鍵點來採用直接法，這類方法稱為稀疏方法（sparse））；LSD（選取整幅影象中有梯度的部分來採用直接法，這種方法稱為半稠密方法（simi-dense）），直接法漸露其自身優勢。DSO（Direct Sparse Odometry，稀疏直接運動估計演算法），DSO的前端和LSD-SLAM相似，後端則拋棄了圖優化的框架。
2、直接法將資料關聯（data association）與位姿估計（pose estimation）放在了一個統一的非線性優化問題中，最小化光度誤差。而特徵點法則分步求解，即，先通過匹配特徵點求出資料之間關聯，再根據關聯來估計位姿。這兩步通常是獨立的，在第二步中，可以通過重投影誤差來判斷資料關聯中的外點，也可以用於修正匹配結果。
程式碼詳解
呼叫函式：1、cvtColor 將影象從一個顏色空間轉換為另一個顏色空間。
void cv :: cvtColor（src，dst，INT 程式碼）
//例項
Mat color, gray;
cvtColor ( color, gray, cv::COLOR_BGR2GRAY );1、直接法位姿估計的邊，構造圖優化 EdgeSE3ProjectDirect
g20中已有的節點和邊
- 相機位姿頂點類VertexSE3Expmap（在這裡直接使用）
- 3D路標點類VertexSBAPointXYZ
- 重投影誤差邊類EdgeProjectXYZ2UV
兩個虛擬函式：computeError()計算誤差，linearizeOplus()計算雅克比矩陣。
//自定義的光度誤差 邊EdgeSE3ProjectDirect，頂點為g2o中李代數位姿節點VertexSE3Expmap
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
//變數宣告，3D點、內參、灰度影象指標image
public:
Eigen::Vector3d x_world_;
float cx_ = 0, cy_ = 0, fx_ = 0, fy_ = 0;
cv::Mat* image_ = nullptr;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//建構函式
EdgeSE3ProjectDirect() {}
EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ){}
//1. 光度誤差計算，3D點投影到影象平面
virtual void computeError()
{
//v是位姿pose,x_local是3D估計值
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
//3D到2D畫素點轉換
float x = x_local[0]*fx_/x_local[2] + cx_;
float y = x_local[1]*fy_/x_local[2] + cy_;
//檢查x y是否出界，距離影象四條邊4個畫素大小內為有效區域。
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
{
_error ( 0,0 ) = 0.0;
this->setLevel ( 1 );
}
else
{
//經過在灰度圖中插值獲取得的灰度值getPixelValue（x,y）減去測量值灰度值
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
}
}
// 2. 計算線性增量，雅可比矩陣J
virtual void linearizeOplus( )
{
//先判斷是否出界，重置
if ( level() == 1 )
{
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
return;
}
//2.1 位姿估計，得到空間座標系3D座標
VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book
//2.2 3D轉換為2D畫素座標
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
float u = x*fx_*invz + cx_;
float v = y*fy_*invz + cy_;
//2.3 畫素對位姿偏導jacobian from se3 to u,v
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
//2.4 畫素梯度部分偏導，求得差分
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
//2.5 總的雅克比是二者相乘
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}
virtual bool read ( std::istream& in ) {}
virtual bool write ( std::ostream& out ) const {}
protected:
//getPixelValue函式通過雙線性差值獲得浮點座標對應插值後的畫素值
inline float getPixelValue ( float x, float y )
{
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
float xx = x - floor ( x );
float yy = y - floor ( y );
return float (
( 1-xx ) * ( 1-yy ) * data[0] +
xx* ( 1-yy ) * data[1] +
( 1-xx ) *yy*data[ image_->step ] +
xx*yy*data[image_->step+1]
);
}
};2、直接法估計相機運動poseEstimationDirect（使用g2o）
bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
// 1.初始化g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );
// 2.新增頂點相機位姿李代數pose
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
pose->setId ( 0 );
optimizer.addVertex ( pose );
// 3. 新增邊，光度誤差，一幀圖上很多畫素代表很多一元邊
int id=1;
for ( Measurement m: measurements )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
);
//這些邊（誤差），對應的頂點都是ID為0那一個pose頂點
edge->setVertex ( 0, pose );
//整個過程測量值只有第一幀的灰度值，後面的每一幀根據位姿找出畫素點，再找到灰度值，都是估計值，
edge->setMeasurement ( m.grayscale );
//資訊矩陣設定為單位陣，表徵每個邊的權重都一樣
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
//依次增加，給邊設定一個ID
edge->setId ( id++ );
optimizer.addEdge ( edge );
}
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
// 4. 開始優化
optimizer.initializeOptimization();
optimizer.optimize ( 30 );
Tcw = pose->estimate();
}3、座標轉換project2Dto3D和project3Dto2D
// 1. 一次測量的值，包括一個世界座標系下3D點與一個灰度值gray
struct Measurement
{
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}//初始化建構函式
Eigen::Vector3d pos_world;//宣告成員函式
float grayscale;
};
//2. 畫素座標到3D座標，RGBD照片d單位毫米，空間點zz單位米，scale單位換算
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
float zz = float ( d ) /scale;
float xx = zz* ( x-cx ) /fx;
float yy = zz* ( y-cy ) /fy;
return Eigen::Vector3d ( xx, yy, zz );
}
//3. 3D到畫素座標
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
float u = fx*x/z+cx;
float v = fy*y/z+cy;
return Eigen::Vector2d ( u,v );
}4、主框架
int main ( int argc, char** argv )
{
//根據時間生成隨機數
srand ( ( unsigned int ) time ( 0 ) );
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
ifstream fin ( associate_file );
string rgb_file, depth_file, time_rgb, time_depth;
cv::Mat color, depth, gray;
//Measurement類儲存世界座標點（以第一幀為參考的FAST關鍵點）和對應的灰度影象（由color->gray）的灰度值
vector<Measurement> measurements;
// 相機內參K
float cx = 325.5;
float cy = 253.5;
float fx = 518.0;
float fy = 519.0;
//RGBD相機單位毫米，3D點單位米
float depth_scale = 1000.0;
Eigen::Matrix3f K;
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
// 位姿，變換矩陣T
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
cv::Mat prev_color;
// 我們以第一個影象為參考，對後續影象和參考影象做直接法
//每一副影象都會與第一幀影象做直接法計算第一幀到當前幀的R,t。
// 參考幀一直是第一幀，而不是迴圈流動當前幀的上一幀為參考幀
for ( int index=0; index<10; index++ )
{
cout<<"*********** loop "<<index<<" ************"<<endl;
//讀入顏色和深度影象
fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
color = cv::imread ( path_to_dataset+"/"+rgb_file );
depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );
if ( color.data==nullptr || depth.data==nullptr )
continue;
//轉換後的灰度圖為g2o優化需要的邊提供灰度值
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
// 1. 對第一幀提取FAST特徵點
//遍歷第一幀特徵點陣列，篩選，把3D座標和灰度值放到measurement中
if ( index ==0 )
{
vector<cv::KeyPoint> keypoints;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect ( color, keypoints );
for ( auto kp:keypoints )
{
// 1.1去掉鄰近邊緣處的點
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
continue;
// 1.2 深度圖上關鍵點的深度值d ，cvRound()返回整數
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
if ( d==0 )
continue;
// 找到深度值d後，2D點投影到3D世界座標系
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
//1.3 灰度圖上關鍵點灰度值
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
//得到三維3D座標和對應的灰度值，作為測量值
measurements.push_back ( Measurement ( p3d, grayscale ) );
}
//第一幀，賦值給pre_color，為什麼用clone？？？
prev_color = color.clone();
continue;
}
// 2. 使用直接法計算相機運動T
//measurements是不變的，之後不斷讀入的fray灰度圖變化
poseEstimationDirect ( measurements, &gray, K, Tcw );
cout<<"Tcw="<<Tcw.matrix() <<endl;
// 3.構建一張圖畫出後續的幀跟第一幀對比的效果
cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
//擺完兩張圖後畫上直接法跟蹤的關鍵點和連線
for ( Measurement m:measurements )
{
if ( rand() > RAND_MAX/5 )
continue;
//空間點3D座標，及在第一幀的畫素座標
Eigen::Vector3d p = m.pos_world;
Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
//座標變換T後，
Eigen::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
continue;
//隨機色使用
float b = 255*float ( rand() ) /RAND_MAX;
float g = 255*float ( rand() ) /RAND_MAX;
float r = 255*float ( rand() ) /RAND_MAX;
//畫跟蹤的特徵點圓和匹配直線
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
}
cv::imshow ( "result", img_show );
cv::waitKey ( 0 );
}
return 0;
}