視覺SLAM十四講 讀書編程筆記 Chapter8 視覺里程計2

原創 Leo-Ma 2020-06-27 04:18

Chapter8 視覺里程計2

直接法的引出

特徵點法的缺點：

  1. 關鍵點的提取與描述子的計算非常耗時
  2. 使用特徵點時，忽略了除特徵點以外的所有信息
  3. 相機有時會運動到特徵缺失的地方

直接法可以有效克服特徵點法的缺點：

  • 在特徵點法估計相機運動時，我們把特徵點看作固定在三維空間的不動點。通過它們在相機中的投影位置，通過最小化重投影誤差來優化相機運動。
  • 在直接法中，我們並不知道點與點之間的對應關係，而是通過最小化光度誤差來求得它們。

光流(Optical Flow)

LK光流原理及其python實現，參考我這篇文章LK光流算法及其opencv的自定義實現

實踐：LK光流

使用TUM公開數據集

完整的TUM數據集下載地址：https://vision.in.tum.de/data/datasets/rgbd-dataset/download
書中程序使用了一部分"freburg1_desk"數據集中的圖像，在書中源代碼已經提供。
書中數據集位於本章目錄的data/下，以壓縮包形式提供(data.tar.gz),解壓後，將看到如下這些文件：

  1. rgb.txt和depth.txt記錄了各文件的採集時間和對應的文件名
  2. rgb/和depth/目錄存放着採集到的PNG格式圖像文件。彩色圖像爲8位3通道，深度圖爲16位單通道圖像。文件名即採集時間
  3. groundtruth.txt爲外部運動捕捉系統採集到的相機位姿，格式爲(time,tx,ty,tz,qx,qy,qz,qw),我們可以把它看成標準軌跡。

注意：彩色圖、深度圖和標準軌跡的採集都是獨立的，軌跡的採集頻率比圖像高很多。在使用數據之前，需要根據採集時間對數據進行一次時間上的對齊，以便對彩色圖和深度圖進行配對。原則上，我們可以把採集時間相近於一個閾值的數據，看成是一對圖像。並把相近時間的位姿，看作是該圖像的真實採集位置。TUM提供了一個python腳本“associatepy”幫我們完成這件事：把這個腳本放到數據集目錄下，運行：

python associate.py rgb.txt depth.txt > associate.txt

這段腳本會根據輸入的兩個文件中的採集時間進行配對，最後輸出到文件associate.txt。輸出文件含有配對後的兩幅圖像的時間、文件名信息，可以作爲後續處理的來源。

使用LK光流

我們對第一幅圖像提取FAST角點，對後面出現的每一幀圖像都，都用LK光流跟蹤它們，並畫在圖中。

完整代碼：
LKFlow/LKFlow.cpp

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>

int main(int argc, char** argv)
{
	if(argc != 2)
	{
		cout<<"usage::LKFlow path_to_dataset"<<endl;
		return 1;
	}

	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;
	list<cv::Point2f> keypoints;//因爲要刪除跟蹤失敗的點，使用list
	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);

		if(index==0)
		{
			//對第一幀提取FAST特徵點
			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;
		}

		if(color.data==nullptr || depth.data==nullptr)continue;
		
		//對其他幀用LK跟蹤特徵點
		vector<cv::Point2f> next_keypoints;
		vector<cv::Point2f> prev_keypoints;

		for(auto kp:keypoints)
		{
			prev_keypoints.push_back(kp);
		}
		vector<unsigned char>status;
		vector<float> error;
		
		chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
		cv::calcOpticalFlowPyrLK(last_color,color,prev_keypoints,next_keypoints,status,error);
		chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
		chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
		cout<<"LK flow use time: "<<time_used.count() <<" seconds."<<endl;
				
		//把跟丟的點刪掉
		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++;
		}
		cout <<"tracked keypoints:"<<keypoints.size()<<endl;
		if(keypoints.size() == 0)
		{
			cout<<"all keypoints are lost."<<endl;
			break;
		}
		//畫出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;

}

LKFlow/CMakeLists.txt

#2019.08.10
# 添加C++標準文件
set(CMAKE_CXX_FLAGS "-std=c++11")

#尋找opencv庫
find_package(OpenCV REQUIRED)

#添加頭文件
include_directories(${OpenCV_INCLUDE_DIRS})


add_executable(LKFlow LKFlow.cpp)

#鏈接OpenCV庫
target_link_libraries(LKFlow ${OpenCV_LIBS})

運行結果
在這裏插入圖片描述
圖像中大部分特徵點都能夠順利跟蹤到，但也有特徵點會丟失。丟失的特徵點或是被移出了視野外，或是被其他物體擋住了。如果我們不提取新的特徵點，那麼光流的跟蹤會越來越少。
在實踐中發現位於物體角點處的特徵更加穩定。邊緣處的特徵會沿着邊緣"滑動"，這主要是由於沿着邊緣移動時特徵塊的內容基本不變，因此程序容易認爲是一個地方。
角點具有最好的辨識度，邊緣次之，區塊最少。
總結： 光流法可以加速基於特徵點的視覺里程計法，避免計算和匹配描述子的過程，但要求相機運動較慢(或採集頻率較高)

直接法(Direct Method)

直接法的推導

假設某個空間點P,它的世界座標爲[X,Y,Z],它在兩個相機上成像，記非齊次像素座標爲p1,p2.以第一個相機作爲參照系，設第二個相機的旋轉和平移爲R,t.同時，兩相機的內參相同，記爲K。
在這裏插入圖片描述
列寫完整的投影方程如下：
在這裏插入圖片描述
其中，Z1是P在第一個相機座標系下的深度，Z2是P在第二個相機座標系下的深度，也就是RP+t的第三個座標值。

在特徵點法中，我們通過匹配描述子知道了p1,p2的像素位置，所以可以計算重投影的位置。但在直接法中，由於沒有特徵匹配，我們無從知道哪一個p2與p1對應着同一個點。
直接法的思路是根據當前相機的位姿估計值來尋找 p2 的位置。但若相機位姿不夠好，p2外觀和p1會有明顯差別。於是，爲了減小這個差別，我們優化相機的位姿，來尋找與p1更相似的p2.這同樣可以解一個優化問題完成，但此時最小化的不是重投影誤差，而是光度誤差，也就是P的兩個像素的亮度誤差，詳細推導如下：

在這裏插入圖片描述
在這裏插入圖片描述

直接法的討論

根據P的來源，可以將直接法進行分類：

  1. P來自於稀疏關鍵點，稱之爲稀疏直接法
  2. P來自部分像素。可以考慮只使用帶有梯度的像素點，捨棄像素梯度不明顯的地方。這稱爲半稠密直接法。
  3. P爲所有像素，稱爲稠密直接法。

實踐：RGB-D的直接法

稀疏直接法

把直接法抽象成一個圖優化問題。顯然，直接法是由以下節點和邊組成的：

  1. 優化變量爲一個相機位姿，因此需要一個位姿頂點，這裏使用李代數SE(3)位姿頂點，即使用VertexSE3Expmap作爲相機位姿
  2. 誤差項爲單個像素的光度誤差。由於整個優化過程中**I1(p1)保持不變，我們可以把它當成一個固定的預設值，然後調整相機位姿，使I2(p2)**接近這個值。於是，這種邊只連接一個頂點，爲一元邊。由於g2o中本身沒有計算光度誤差的邊，我們需要自己定義一種新的邊。

定義直接法的邊

我們的邊繼承自g2o::BaseUnaryEdge.在繼承時，需要在模板參數裏填入測量值的維度、類型，以及連接此邊的頂點，同時，我們把空間點P、相機內參和圖像存儲在該邊的成員變量中。爲了讓g2o優化該邊對應的誤差，我們需要覆寫兩個虛函數：用computeError()計算誤差值，用linearizeOplus()計算雅克比矩陣。可以看到，這裏的雅克比矩陣與前面推導的是一致的。我們在程序中誤差計算裏使用了I2(p2)-I1(p1)的形式，因此，前面色負號可以省去，只需把像素梯度乘以像素到李代數的梯度即可。
在程序中，相機位姿是用浮點數表示的，投影導像素座標也是浮點形式。爲了更精確地計算像素亮度，我們要對像素插值。我們這裏採用了簡單的雙線性插值。

// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
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 )
    {}

    virtual void computeError()
    {
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

        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_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        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_;

        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;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    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]
               );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};

使用直接法估計相機運動

讀取數據集的RGB-D圖像序列。以第一幅圖像爲參考幀，然後用直接法求解後續圖像位姿。在參考幀中，對一副圖像提取FAST關鍵點(不需要描述子)，並使用直接法估計這些關鍵點在後續圖像中的位置，這就構成了一種簡單的稀疏直接法。
完整代碼：
direct_sparse.cpp

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;
using namespace g2o;

/********************************************
 * 本節演示了RGBD上的稀疏直接法 
 ********************************************/

// 一次測量的值，包括一個世界座標系下三維點與一個灰度值
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
    Eigen::Vector3d pos_world;
    float grayscale;
};

//像素座標系轉換到相機座標系系
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 );
}


//相機座標系轉換到像素座標系
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 );
}

// 直接法估計位姿
// 輸入：測量值（空間點的灰度），新的灰度圖，相機內參； 輸出：相機位姿
// 返回：true爲成功，false失敗
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );


// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
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 )
    {}

    virtual void computeError()
    {
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

        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_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        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_;

        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;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    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]
               );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    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;
    vector<Measurement> measurements;
    // 相機內參
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;
    // 我們以第一個圖像爲參考，對後續圖像和參考圖像做直接法
    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; 
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
        if ( index ==0 )
        {
            // 對第一幀提取FAST特徵點
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect ( color, keypoints );
            for ( auto kp:keypoints )
            {
                // 去掉鄰近邊緣處的點
                if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }
        // 使用直接法計算相機運動
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect ( measurements, &gray, K, Tcw );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix() <<endl;

        // plot the feature points
        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;
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            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;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化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 );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加邊
    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
        );
        edge->setVertex ( 0, pose );
        edge->setMeasurement ( m.grayscale );
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 30 );
    Tcw = pose->estimate();
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( directMethod )

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模塊路徑
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )

find_package( G2O )
include_directories( ${G2O_INCLUDE_DIRS} ) 

include_directories( "/usr/include/eigen3" )

set( G2O_LIBS 
    g2o_core g2o_types_sba g2o_solver_csparse g2o_stuff g2o_csparse_extension 
)

add_executable( direct_sparse direct_sparse.cpp )
target_link_libraries( direct_sparse ${OpenCV_LIBS} ${G2O_LIBS} )

運行結果：

半稠密直接法

只需要將參考幀中提取FAST特徵點變成提取梯度較明顯的像素，其餘部分不變，改動如下：

            // select the pixels with high gradiants 
            for ( int x=10; x<gray.cols-10; x++ )
                for ( int y=10; y<gray.rows-10; y++ )
                {
                    Eigen::Vector2d delta (
                        gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], 
                        gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
                    );
                    if ( delta.norm() < 50 )
                        continue;
                    ushort d = depth.ptr<ushort> (y)[x];
                    if ( d==0 )
                        continue;
                    Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
                    float grayscale = float ( gray.ptr<uchar> (y) [x] );
                    measurements.push_back ( Measurement ( p3d, grayscale ) );
                }

完整代碼：
direct_semidense.cpp

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;
using namespace g2o;

/********************************************
 * 本節演示了RGBD上的半稠密直接法 
 ********************************************/

// 一次測量的值，包括一個世界座標系下三維點與一個灰度值
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
    Eigen::Vector3d pos_world;
    float grayscale;
};

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 );
}

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 );
}

// 直接法估計位姿
// 輸入：測量值（空間點的灰度），新的灰度圖，相機內參； 輸出：相機位姿
// 返回：true爲成功，false失敗
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );


// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
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 )
    {}

    virtual void computeError()
    {
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

        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_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        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_;

        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;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    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]
               );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    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;
    vector<Measurement> measurements;
    // 相機內參
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;
    // 我們以第一個圖像爲參考，對後續圖像和參考圖像做直接法
    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; 
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
        if ( index ==0 )
        {
            // select the pixels with high gradiants 
            for ( int x=10; x<gray.cols-10; x++ )
                for ( int y=10; y<gray.rows-10; y++ )
                {
                    Eigen::Vector2d delta (
                        gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], 
                        gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
                    );
                    if ( delta.norm() < 50 )
                        continue;
                    ushort d = depth.ptr<ushort> (y)[x];
                    if ( d==0 )
                        continue;
                    Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
                    float grayscale = float ( gray.ptr<uchar> (y) [x] );
                    measurements.push_back ( Measurement ( p3d, grayscale ) );
                }
            prev_color = color.clone();
            cout<<"add total "<<measurements.size()<<" measurements."<<endl;
            continue;
        }
        // 使用直接法計算相機運動
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect ( measurements, &gray, K, Tcw );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix() <<endl;

        // plot the feature points
        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;
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            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 = 0;
            float g = 250;
            float r = 0;
            img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3] = b;
            img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+1] = g;
            img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+2] = r;
            
            img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3] = b;
            img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+1] = g;
            img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+2] = r;
            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 4, cv::Scalar ( b,g,r ), 2 );
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 4, cv::Scalar ( b,g,r ), 2 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );

    }
    return 0;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化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 );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加邊
    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
        );
        edge->setVertex ( 0, pose );
        edge->setMeasurement ( m.grayscale );
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 30 );
    Tcw = pose->estimate();
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( directMethod )

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模塊路徑
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )

find_package( G2O )
include_directories( ${G2O_INCLUDE_DIRS} ) 

include_directories( "/usr/include/eigen3" )

set( G2O_LIBS 
    g2o_core g2o_types_sba g2o_solver_csparse g2o_stuff g2o_csparse_extension 
)


add_executable( direct_semidense direct_semidense.cpp )
target_link_libraries( direct_semidense ${OpenCV_LIBS} ${G2O_LIBS} )

運行結果：
在這裏插入圖片描述

直接法的討論

根據直接法的推導結果可以看出，像素梯度引導着優化的方向。如果想要得到正確的優化結果，就必須保證大部分像素梯度能夠把優化引導到正確的方向。
在這裏插入圖片描述假設對於參考圖像，測量到一個灰度值爲229的像素，由於我們知道深度，因此可以推斷出空間點P的位置。
我們需要估計第二幅圖像相對於第一幅圖像的相機位姿變化。這個位姿是由一個初始值不斷地優化迭代得到的。假設我們的初始值比較差，在這個初始值下，空間點P投影后的像素灰度值是126。於是，這個像素的誤差爲229-126=103。爲了減小這個誤差，我們希望微調相機的位姿，使像素更亮一些。
我們需要根據局部的像素梯度來微調相機的位姿。我們在第二幅圖像中發現，沿u軸往前走一步，該處的灰度值變成123，即減去了3，沿v軸往前走一步，灰度值減了18，變成108。因此，在這個像素周圍，梯度是[-3,-18]，爲了提高亮度，我們會建議優化算法微調相機，使P的像往左上方移動。優化算法不能只聽取這個像素的一面之詞，還需要聽取其他像素的建議。綜合聽取了許多像素的意見之後，優化算法選擇了一個和我們建議的方向偏離不遠的地方，計算出一個更新量。加上更新量後，圖像從I2移動到了I2’,像素的投影位置也變到了一個更亮的位置，通過這次更新，誤差變小了，理想情況下，通過迭代優化，最後期望誤差會收斂。
而實際上，圖像通常都是一個非凸函數，我們不能保證沿着圖像梯度走時，灰度誤差會不斷下降。所以只有當相機運動很小，圖像中的梯度不會有很強的非凸性時，直接法才能成立。

直接法的優缺點總結

優點：

  • 可以省去計算特徵點、描述子的時間
  • 只要求有像素梯度即可，不需要有特徵點。因此，直接法可以在特徵缺失的場合下使用。
  • 可以構建半稠密甚至稠密的地圖，這是特徵點法無法做到的。
    缺點：
  • 非凸性。直接法完全依靠梯度搜索，降低目標函數來計算相機位姿。其目標函數中需要取像素點的灰度值，而圖像是強烈的非凸函數。這使得優化算法容易進入極小，只在運動極小時直接法才能成功。
  • 單個像素沒有區分度。因爲和它像的實在太多了，這個約束實在太弱了。於是我們要麼計算圖像塊，要麼計算複雜的相關性
  • 灰度值不變是很強的假設。這可能並不符合實際情況。
發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章

cartographer編譯過程遇到未定義的dlclose@@GLIBC_2.2.5

1. 使用的安裝過程如下： 先裝下這下面幾個依賴 sudo apt-get update sudo apt-get install -y python-wstool python-rosdep ninja-build 1. carto

莫奈的老旧三轮车 2020-07-08 09:14:34

SLAM：：g2o安裝和使用

g2o安裝和使用 安裝依賴 sudo apt-get install libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake 下載源碼 git clone https

dieju8330 2020-07-08 10:35:13

VSLAM：：[手寫VIO_課堂筆記]第三講(上)_基於優化的IMU與視覺信息融合(最小二乘問題)

1. 第三講(上)_基於優化的IMU與視覺信息融合(上) 第三講(上): (1)最小二乘問題的求解推導 (2)其中有LM算法的相關推導以及魯棒核函數的推導 1.1. 最小二乘與非線性優化 1.1.1. 最小二乘

dieju8330 2020-07-08 10:35:13

VSLAM：：[手寫VIO_課堂筆記]第一講_預備知識

1. 四元數的基本運算 主要運算 四元數乘法 乘法性質 滿足結合律 不滿足交換律 乘積的模等於模的乘積 乘積的逆等於各個四元數的逆以相反的順序相乘 其他運算 *四元數部分參考：旋轉矩陣、歐拉角、四元數理論

dieju8330 2020-07-08 10:35:13

利用Robosense 16線雷達跑cartographer 2D

首先，根據不同的雷達，瞭解雷達發佈的點雲消息。（IMU也是一樣，瞭解IMU發佈的點雲topic）。 本文使用的是Robosense 16線激光雷達，暫時未使用IMU（建議6軸，再加上GPS定位精度會高很多）發佈PointCloud2類型的

合工大机器人实验室 2020-07-07 21:47:01

Cartographer編譯安裝

官網安裝教程：https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html 不過由於網絡問題，可能在下載的時候會下載中斷。我會上傳我自己下載的到資源處，

合工大机器人实验室 2020-07-07 21:46:51

SLAM前端：ICP（Iterative Closest Point）

ICP算法是點雲配准算法，給出兩組點雲，可以計算出兩組點雲的位姿關係R、t。問題數學表達爲： 已知兩組點雲：                                                            求R、t使

共和国之辉 2020-07-07 14:29:18

SLAM 整體性總結

VSLAM方法框架： 整個SLAM大概可以分爲前端和後端 ，前端相當於VO（視覺里程計），研究幀與幀之間變換關係。首先提取每幀圖像特徵點，利用相鄰幀圖像，進行特徵點匹配，然後利用RANSAC去除大噪聲，然後進行匹配，得到一個pose信息（

追求卓越583 2020-07-07 14:11:01

SLAM前端：PnP（一）DLS、P3P

PnP是一類問題的統稱，是指通過多對點的3D位置及2D投影座標，來估計相機位姿R、t。 場景一：視覺slam中在初始化後可以知道空間中一些點在世界座標系下的座標，在下一幀圖像進行特徵點匹配後，利用這些特徵點的3D座標及2D像素座標，PnP

共和国之辉 2020-07-07 13:52:13

SLAM前端：本質矩陣、基礎矩陣、單應矩陣

本質矩陣描述了相機內參已知的情況下同一個點不同視角下的關係，5自由度。即已知同一個點在兩幀圖像下的座標，兩個座標、相機內參、本質矩陣滿足對極約束條件（1-1）。 基礎矩陣描述了同一個點在不同視角下的關係，7自由度。 單應矩陣描述了同一平面

共和国之辉 2020-07-07 13:52:13

視覺SLAM：視覺SLAM中的李羣與李代數、左乘擾動模型

爲什麼引入李羣與李代數？ SLAM中對矩陣求導數的應用場景？什麼情況下用到李代數左乘擾動模型求導數？ 爲什麼不能用李羣求導？ 李羣與李代數的關係？ 李代數左乘擾動的理解？ 0 爲什麼引入李羣與李代數？ slam中後端優化需要用到求解最小二

共和国之辉 2020-07-07 13:52:12

SLAM前端（里程計二）：ORB特徵點匹配

（1）構建圖像高斯金字塔 將圖像進行降採樣8次，每次尺度縮小1/1.2，如下圖所示： 採用高斯金字塔的目的是解決特徵點尺度不變性，並且可以提取更多的關鍵點。高斯金字塔每個尺度代表了不同的距離，即低分辨率的圖像可以看做從遠處拍攝圖像的一部

共和国之辉 2020-07-07 13:52:11

SLAM前端：對極幾何、三角測量

對極幾何在ORBSLAM初始化過程中求解兩幀的R、t，輸入兩幀圖像的多對匹配點，對極幾何計算得到R、t 三角測量在單目ORBslam初始化過程中計算初始化點雲的3D座標，輸入兩幀的R、t+匹配特徵點的像素座標，輸出點的3D座標 1 對極幾

共和国之辉 2020-07-07 13:52:11

SLAM後端：位姿圖優化（Pose Graph）

BA優化時間跟特徵點數量有關，特徵點數量越多BA消耗時間越長。折衷做法是，在進行幾次優化後，將特徵點位置固定，不再優化特徵點，只優化相機位姿。位姿圖優化是一中方法。場景如下：                                

共和国之辉 2020-07-07 13:52:11

SLAM前端（里程計一）：ORB特徵點提取

ORB特徵 = Fast+描述子，Fast判斷該點是不是特徵點，描述子計算該特徵點的方向信息。 1 Fast角點 （1）Fast規則 如下圖，遍歷每個像素點，檢測在像素點附近的圓形窗口上的16個像素的灰度，如果有n個連續點都比中心像素的灰

共和国之辉 2020-07-07 13:52:10