《视觉slam十四讲》第八讲课后习题

1.除了LK光流之外，还有哪些光流方法？它们各有什么特点？

光流法就是一种你根据视觉成像信息来感知自己的运动状态的一种方法。目前，按照理论基础与数学方法的区别把它们分成：基于梯度的方法、基于匹配的方法、基于能量的方法、基于相位的方法以及神经动力学方法。具体各种算法有一篇博客说的比较细节，大家可以参考：https://blog.csdn.net/qq_38906523/article/details/80781242，有兴趣可以去读一些关于光流算法的综述，比如《A Database and Evaluation Methodology for Optical Flow》。

 

2.在本节程序的求图像求梯度的过程中，我们简单求了u+1和u-1的灰度差除以2,作为u方向上的梯度值。这种做法有什么缺点？提示：对于距离较近的特征，变化应该较快；而距离比较远的特征在图像中变化慢，求梯度时能否利用此信息？

根据题目里的提示，当被观测的特征点离相机距离比较小时，其像素的运动会明显加快，像素跨度会增大，像素的梯度不可能能被左右两个像素灰度值之和除以2的，而且这种方法也很容易被噪音影响以至于光流运算过程精度下降。当距离特别远时，容易速度变化过小，引起漂移。在求梯度时，对于不同深度的像素块可以分层优化求解。

 

3.在稀疏直接法，假设单个像素周围小块的光度也不变，是否可以提高算法健壮性？请编程实现。

相较于单像素比较，以像素块作为误差测量量后的鲁棒性明显没有稀疏直接法那么飘了。（毕竟如果要比较的话相似的像素点实在是太多了）其CMakeLists.txt代码如下所示：

cmake_minimum_required(VERSION 2.6)
project(homeworks8_3)

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

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})
set(G2O_LIBS
  g2o_core g2o_types_sba g2o_solver_csparse g2o_stuff g2o_csparse_extension
)

include_directories("/usr/include/eigen3")


add_executable(homeworks8_3 homeworks8_3.cpp)

target_link_libraries(homeworks8_3 ${OpenCV_LIBS} ${G2O_LIBS})

install(TARGETS homeworks8_3 RUNTIME DESTINATION bin)

homeworks8_3.cpp代码如下所示：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace g2o;

//一次测量值，只是一个点的三维坐标和这个点对应灰度图上的灰度值
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);
}

bool poseEstimationDirect(const vector& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw);

//将一个3d点投影到一个图像平面，误差是光度误差
class EdgeSE3ProjectDirect: public BaseUnaryEdge<1, double, VertexSE3Expmap>
{
public:
  
  //Eigen数据结构对其
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
  //默认构造函数
  EdgeSE3ProjectDirect() {}
  
  //自定义构造函数，参数：一个3d点下标，内参矩阵4个参数，参考图以及变换后的图
  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()
  {
    //将顶点取出为位姿指针v
    const VertexSE3Expmap* v = static_cast (_vertices[0]);
    
    //从世界坐标系下坐标到像素坐标，位姿估计值.map()函数即为乘上位姿T，这里其实为3d点世界坐标乘上相机位姿，计算当前相机坐标系下的坐标
    Eigen::Vector3d x_local = v -> estimate().map (x_world_);
    //3d投影到像素坐标
    float x = x_local[0] * fx_/x_local[2] + cx_;
    float y = x_local[1] * fy_/x_local[2] + cy_;
    
    //检测像素是否还在图像中，这里靠近边缘4像素就认为已经出了图像，将误差设置为0,此条边level设置为1,用于区分
    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;
      //在此处进行更改
      float sumValue = 0.0;
      for (int i=x-2; i<=x+2; ++i)
	for(int j=y-2; j<=y+2; ++j)
	  sumValue += getPixelValue(i,j);
	
      sumValue /= 25;
      _error(0,0) = sumValue - _measurement;
    }
  }
  
  //计算线性增量，也就是雅克比矩阵J
  virtual void linearizeOplus()
  {
    //判断一下这条边对应的像素点是不是超过了像素平面，出了就直接设置为0
    if (level() == 1)
    {
      _jacobianOplusXi = Eigen::Matrix::Zero();
      return;
    }
    
    //如果没出，正常点，取出位姿估计，求出在此位姿相机坐标系下的空间点坐标
    VertexSE3Expmap* vtx = static_cast (_vertices[0]);
    //195页的q
    Eigen::Vector3d xyz_trans = vtx -> estimate().map (x_world_);
    
    //取出q的x，y
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    //z的倒数
    double invz = 1.0/xyz_trans[2];
    //z倒数的平方
    double invz_2 = invz*invz;
    
    //求取像素梯度
    float u = x * fx_ * invz + cx_;
    float v = y * fy_ * invz + cy_;
    
    //p195页的8.15
    Eigen::Matrix 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_;

    //像素部分偏导8.16
    Eigen::Matrix 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;

    //8.16
    _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:
  inline float getPixelValue (float x, float y)
  {
    uchar* data = & image_ -> data[int(y) * image_ -> step + int (x)];
    //floor函数，向下取整数，xx和yy为小数部分
    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_;
  float cx_=0, cy_=0, fx_=0, fy_=0;
  cv::Mat* image_ = nullptr;
};

int main(int argc, char** argv)
{
  //定位数据文件
  string path_to_dataset = "/home/fuhang/projects/useLK/data";
  string associate_file = path_to_dataset + "/associate.txt";
  
  //读入到文件输入流
  ifstream fin (associate_file);
  
  string rgb_file, time_rgb, depth_file, time_depth;
  
  cv::Mat color, depth, gray;
  //测量值数组，就是一堆灰度值
  vector 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();
  
  //prev_color也就是图像流的第一帧，在整个程序中，也只有第一帧对这变量进行了赋值，所有帧的参考帧都为第一帧
  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);
    //printf("a");
    //程序开始对第一帧进行处理，也就是参考帧
    if (index == 0)
    {
      //对color图的第一帧提取FAST特征点
      vector keypoints;
      cv::Ptr detector = cv::FastFeatureDetector::create("FAST");
      detector -> detect(color, keypoints);
    
      //遍历color图第一帧中的特征点数组，进行筛选和在深度图和灰度图上定位深度值和灰度值
      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;
	
	//int cvRound(double value);对一个double类型1数据进行四舍五入
	//提取特征点坐标的深度值
        ushort d = depth.ptr (cvRound(kp.pt.y)) [cvRound(kp.pt.x)];
        //没测量到的点跳过
	if (d == 0)
	  continue;
	
	//测量到深度值后，进行反投影得到空间3d坐标
        Eigen::Vector3d p3d = project2Dto3D (kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale);
        
	//寻找关键点位置上的灰度值
	float grayscale = float (gray.ptr (cvRound(kp.pt.y)) [cvRound(kp.pt.x)]);
        
	//最后这里得到了三维坐标和对应的灰度值，一个测量值就搞定了，被推进measurements数组
	measurements.push_back(Measurement (p3d, grayscale));
      }
      //第一帧赋值给变量prev_color
      prev_color = color.clone();
      continue;
    }
  
    //使用直接法计算相机运动
    //开始计算时间
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //在这个函数执行时measurements是不变的，只有不断读入的&gray灰度图是变化的
    poseEstimationDirect (measurements, &gray, K, Tcw);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration time_used = chrono::duration_cast>(t2-t1);
    cout << "Tcw = " << endl << Tcw.matrix() << endl;
  
    //画出特征点
    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)));
  
    //遍历数组measurements并进行操作
    for (Measurement m:measurements)
    {
      //随机选20%的关键点
      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;
}

//用直接法求位姿的函数poseEstimationDirect
bool poseEstimationDirect(const std::vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw)
{
  //初始化g2o
  //求解向量为6或1的
  typedef g2o::BlockSolver> DirectBlock;
  DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense ();
  DirectBlock* solver_ptr = new DirectBlock (linearSolver);
  
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
  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
    );
    //这些边对应的顶点就是ID为0的那一個pose点
    edge -> setVertex(0,pose);
    edge -> setMeasurement(m.grayscale);
    //信息矩阵设置为单位阵，表征每个边的权重都一样
    edge -> setInformation(Eigen::Matrix::Identity());
    //依次添加标签
    edge -> setId(id++);
    //添加进优化器
    optimizer.addEdge(edge);
  }
  
  cout << "edges in graph: " << optimizer.edges().size() << endl;
  optimizer.initializeOptimization();
  optimizer.optimize(30);
  Tcw = pose -> estimate();
  
}

结果可以自行在KDevelop中运行一下，可以明显发现特征点鲁棒性加强了。

 

4.使用Ceres实现RGB-D上的稀疏直接法和半稠密直接法。

前端不变，只需要改一下优化过程就行了。

稀疏直接法：

CMakeLists.txt代码如下：

cmake_minimum_required(VERSION 2.6)
project(homeworks8_4_1)

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

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(homeworks8_4_1 homeworks8_4_1.cpp)

target_link_libraries(homeworks8_4_1 ${OpenCV_LIBS} ${CERES_LIBRARIES})

install(TARGETS homeworks8_4_1 RUNTIME DESTINATION bin)

homeworks8_4_1.cpp代码如下：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 

#include 
#include 

using namespace std;


//一次测量值，只是一个点的三维坐标和这个点对应灰度图上的灰度值
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);
}

bool poseEstimationDirect(const vector& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw);


class SparseBA : public ceres::SizedCostFunction<1,6>
{
public:
  cv::Mat * gray_;
  double cx_, cy_;
  double fx_, fy_;
  
  double pixelValue_;
  double X_, Y_, Z_;
  
SparseBA(cv::Mat *gray, double cx, double cy, double fx, double fy, double X, double Y, double Z, double pixelValue)
{
  gray_ = gray;
  cx_ = cx;
  cy_ = cy;
  fx_ = fx;
  fy_ = fy;
  X_ = X;
  Y_ = Y;
  Z_ = Z;
  pixelValue_ = pixelValue;
}

virtual bool Evaluate (double const *const *pose, double *residual, double **jacobians) const{
  //存储p的坐标
  double p[3];
  p[0] = X_; 
  p[1] = Y_;
  p[2] = Z_;
  
  //存储新的p'的坐标
  double newP[3];
  double R[3];
  R[0] = pose[0][0];
  R[1] = pose[0][1];
  R[2] = pose[0][2];
  ceres::AngleAxisRotatePoint(R, p, newP);
  
  newP[0] += pose[0][3];
  newP[1] += pose[0][4];
  newP[2] += pose[0][5];
  
  //新的p‘点投影到像素坐标系
  double ux = fx_ * newP[0] / newP[2] + cx_;
  double uy = fy_ * newP[1] / newP[2] + cy_;
  
  residual[0] = getPixelValue(ux, uy) - pixelValue_;
  
  if (jacobians)
  {
    double invz = 1.0 / newP[2];
    double invz_2 = invz * invz;
    
    //公式8.15
    Eigen::Matrix jacobian_uv_ksai;
    jacobian_uv_ksai(0,0) = -newP[0] * newP[1] * invz_2 * fx_;
    jacobian_uv_ksai(0,1) = (1 + (newP[0] * newP[0] * invz_2)) * fx_;
    jacobian_uv_ksai(0,2) = -newP[1] * invz * fx_;
    jacobian_uv_ksai(0,3) = invz * fx_;
    jacobian_uv_ksai(0,4) = 0;
    jacobian_uv_ksai(0,5) = -newP[0] * invz_2 * fx_;
    
    jacobian_uv_ksai(1,0) = -(1 + newP[1] * newP[1] * invz_2) * fy_;
    jacobian_uv_ksai(1,1) = newP[0] * newP[1] * invz_2 * fy_;
    jacobian_uv_ksai(1,2) = newP[0] * invz * fy_;
    jacobian_uv_ksai(1,3) = 0;
    jacobian_uv_ksai(1,4) = invz * fy_;
    jacobian_uv_ksai(1,5) = -newP[1] * invz_2 * fy_;
    
    //像素梯度
    Eigen::Matrix jacobian_pixel_uv;
    jacobian_pixel_uv(0,0) = (getPixelValue(ux+1, uy) - getPixelValue(ux-1, uy))/2;
    jacobian_pixel_uv(0,1) = (getPixelValue(ux, uy+1) - getPixelValue(ux, uy-1))/2;
    
    //公式8.16
    Eigen::Matrix jacobian = jacobian_pixel_uv * jacobian_uv_ksai;
    
    jacobians[0][0] = jacobian(0);
    jacobians[0][1] = jacobian(1);
    jacobians[0][2] = jacobian(2);
    jacobians[0][3] = jacobian(3);
    jacobians[0][4] = jacobian(4);
    jacobians[0][5] = jacobian(5);
  }
  
  return true;
  
}

double getPixelValue (double x, double y) const
{
  uchar* data = & gray_->data[int(y) * gray_->step + int(x)];
  double xx = x - floor(x);
  double yy = y - floor(y);
  return double (
    (1 - xx) * (1 - yy) * data[0] + xx * (1 - yy) * data[1] + (1 - xx) * yy * data[gray_->step] + xx * yy * data[gray_->step + 1]
  );
}
};



int main(int argc, char** argv)
{
  //定位数据文件
  string path_to_dataset = "/home/fuhang/projects/useLK/data";
  string associate_file = path_to_dataset + "/associate.txt";
  
  //读入到文件输入流
  ifstream fin (associate_file);
  
  string rgb_file, time_rgb, depth_file, time_depth;
  
  cv::Mat color, depth, gray;
  //测量值数组，就是一堆灰度值
  vector 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();
  
  //prev_color也就是图像流的第一帧，在整个程序中，也只有第一帧对这变量进行了赋值，所有帧的参考帧都为第一帧
  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);
    //printf("a");
    //程序开始对第一帧进行处理，也就是参考帧
    if (index == 0)
    {
      //对color图的第一帧提取FAST特征点
      vector keypoints;
      cv::Ptr detector = cv::FastFeatureDetector::create("FAST");
      detector -> detect(color, keypoints);
    
      //遍历color图第一帧中的特征点数组，进行筛选和在深度图和灰度图上定位深度值和灰度值
      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;
	
	//int cvRound(double value);对一个double类型1数据进行四舍五入
	//提取特征点坐标的深度值
        ushort d = depth.ptr (cvRound(kp.pt.y)) [cvRound(kp.pt.x)];
        //没测量到的点跳过
	if (d == 0)
	  continue;
	
	//测量到深度值后，进行反投影得到空间3d坐标
        Eigen::Vector3d p3d = project2Dto3D (kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale);
        
	//寻找关键点位置上的灰度值
	float grayscale = float (gray.ptr (cvRound(kp.pt.y)) [cvRound(kp.pt.x)]);
        
	//最后这里得到了三维坐标和对应的灰度值，一个测量值就搞定了，被推进measurements数组
	measurements.push_back(Measurement (p3d, grayscale));
      }
      //第一帧赋值给变量prev_color
      prev_color = color.clone();
      continue;
    }
  
    //使用直接法计算相机运动
    //开始计算时间
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //在这个函数执行时measurements是不变的，只有不断读入的&gray灰度图是变化的
    poseEstimationDirect (measurements, &gray, K, Tcw);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration time_used = chrono::duration_cast>(t2-t1);
    cout << "Tcw = " << endl << Tcw.matrix() << endl;
  
    //画出特征点
    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)));
  
    //遍历数组measurements并进行操作
    for (Measurement m:measurements)
    {
      //随机选20%的关键点
      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;
}

//用直接法求位姿的函数poseEstimationDirect
bool poseEstimationDirect(const std::vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw)
{
  ceres::Problem problem;
  //定义位姿数组
  double pose[6];
  //用轴角进行优化
  Eigen::AngleAxisd rotationVector(Tcw.rotation());
  pose[0] = rotationVector.angle() * rotationVector.axis()(0);
  pose[1] = rotationVector.angle() * rotationVector.axis()(1);
  pose[2] = rotationVector.angle() * rotationVector.axis()(2);
  pose[3] = Tcw.translation()(0);
  pose[4] = Tcw.translation()(1);
  pose[5] = Tcw.translation()(2);
  
  //构建Ceres问题
  for (Measurement m:measurements)
  {
    ceres::CostFunction * costFunction = new SparseBA(gray, K(0,2), K(1,2), K(0,0), K(1,1), m.pos_world(0), m.pos_world(1), m.pos_world(2), double(m.grayscale));
    problem.AddResidualBlock(costFunction, nullptr, pose);
  }
  
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  
  cv::Mat rotateVectorCV = cv::Mat::zeros(3, 1, CV_64FC1);
  rotateVectorCV.at(0) = pose[0];
  rotateVectorCV.at(1) = pose[1];
  rotateVectorCV.at(2) = pose[2];
  
  cv::Mat RCV;
  cv::Rodrigues(rotateVectorCV, RCV);
  Tcw(0,0) = RCV.at(0,0); Tcw(0,1) = RCV.at(0,1); Tcw(0,2) = RCV.at(0,2);
  Tcw(1,0) = RCV.at(1,0); Tcw(1,1) = RCV.at(1,1); Tcw(1,2) = RCV.at(1,2);
  Tcw(2,0) = RCV.at(2,0); Tcw(2,1) = RCV.at(2,1); Tcw(2,2) = RCV.at(2,2);

  Tcw(0,3) = pose[3];
  Tcw(1,3) = pose[4];
  Tcw(2,3) = pose[5];
}

观察输出结果，不难发现ceres库优化在前几帧的结果和g2o表现的差异不大，但是后几帧差距就很明显了。这应该是和输入数据measurements的特征点不稳定是有关系的。

半稠密直接法：

CMakeLists.txt代码如下所示：

cmake_minimum_required(VERSION 2.6)
project(homeworks8_4_2)

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

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(homeworks8_4_2 homeworks8_4_2.cpp)

target_link_libraries(homeworks8_4_2 ${OpenCV_LIBS} ${CERES_LIBRARIES})

install(TARGETS homeworks8_4_2 RUNTIME DESTINATION bin)

homeworks8_4_2.cpp代码如下所示：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 

#include 
#include 

using namespace std;


//一次测量值，只是一个点的三维坐标和这个点对应灰度图上的灰度值
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);
}

bool poseEstimationDirect(const vector& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw);


class SparseBA : public ceres::SizedCostFunction<1,6>
{
public:
  cv::Mat * gray_;
  double cx_, cy_;
  double fx_, fy_;
  
  double pixelValue_;
  double X_, Y_, Z_;
  
SparseBA(cv::Mat *gray, double cx, double cy, double fx, double fy, double X, double Y, double Z, double pixelValue)
{
  gray_ = gray;
  cx_ = cx;
  cy_ = cy;
  fx_ = fx;
  fy_ = fy;
  X_ = X;
  Y_ = Y;
  Z_ = Z;
  pixelValue_ = pixelValue;
}

virtual bool Evaluate (double const *const *pose, double *residual, double **jacobians) const{
  //存储p的坐标
  double p[3];
  p[0] = X_; 
  p[1] = Y_;
  p[2] = Z_;
  
  //存储新的p'的坐标
  double newP[3];
  double R[3];
  R[0] = pose[0][0];
  R[1] = pose[0][1];
  R[2] = pose[0][2];
  ceres::AngleAxisRotatePoint(R, p, newP);
  
  newP[0] += pose[0][3];
  newP[1] += pose[0][4];
  newP[2] += pose[0][5];
  
  //新的p‘点投影到像素坐标系
  double ux = fx_ * newP[0] / newP[2] + cx_;
  double uy = fy_ * newP[1] / newP[2] + cy_;
  
  residual[0] = getPixelValue(ux, uy) - pixelValue_;
  
  if (jacobians)
  {
    double invz = 1.0 / newP[2];
    double invz_2 = invz * invz;
    
    //公式8.15
    Eigen::Matrix jacobian_uv_ksai;
    jacobian_uv_ksai(0,0) = -newP[0] * newP[1] * invz_2 * fx_;
    jacobian_uv_ksai(0,1) = (1 + (newP[0] * newP[0] * invz_2)) * fx_;
    jacobian_uv_ksai(0,2) = -newP[1] * invz * fx_;
    jacobian_uv_ksai(0,3) = invz * fx_;
    jacobian_uv_ksai(0,4) = 0;
    jacobian_uv_ksai(0,5) = -newP[0] * invz_2 * fx_;
    
    jacobian_uv_ksai(1,0) = -(1 + newP[1] * newP[1] * invz_2) * fy_;
    jacobian_uv_ksai(1,1) = newP[0] * newP[1] * invz_2 * fy_;
    jacobian_uv_ksai(1,2) = newP[0] * invz * fy_;
    jacobian_uv_ksai(1,3) = 0;
    jacobian_uv_ksai(1,4) = invz * fy_;
    jacobian_uv_ksai(1,5) = -newP[1] * invz_2 * fy_;
    
    //像素梯度
    Eigen::Matrix jacobian_pixel_uv;
    jacobian_pixel_uv(0,0) = (getPixelValue(ux+1, uy) - getPixelValue(ux-1, uy))/2;
    jacobian_pixel_uv(0,1) = (getPixelValue(ux, uy+1) - getPixelValue(ux, uy-1))/2;
    
    //公式8.16
    Eigen::Matrix jacobian = jacobian_pixel_uv * jacobian_uv_ksai;
    
    jacobians[0][0] = jacobian(0);
    jacobians[0][1] = jacobian(1);
    jacobians[0][2] = jacobian(2);
    jacobians[0][3] = jacobian(3);
    jacobians[0][4] = jacobian(4);
    jacobians[0][5] = jacobian(5);
  }
  
  return true;
  
}

double getPixelValue (double x, double y) const
{
  uchar* data = & gray_->data[int(y) * gray_->step + int(x)];
  double xx = x - floor(x);
  double yy = y - floor(y);
  return double (
    (1 - xx) * (1 - yy) * data[0] + xx * (1 - yy) * data[1] + (1 - xx) * yy * data[gray_->step] + xx * yy * data[gray_->step + 1]
  );
}
};



int main(int argc, char** argv)
{
  //定位数据文件
  string path_to_dataset = "/home/fuhang/projects/useLK/data";
  string associate_file = path_to_dataset + "/associate.txt";
  
  //读入到文件输入流
  ifstream fin (associate_file);
  
  string rgb_file, time_rgb, depth_file, time_depth;
  
  cv::Mat color, depth, gray;
  //测量值数组，就是一堆灰度值
  vector 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();
  
  //prev_color也就是图像流的第一帧，在整个程序中，也只有第一帧对这变量进行了赋值，所有帧的参考帧都为第一帧
  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);
    //printf("a");
    //程序开始对第一帧进行处理，也就是参考帧
    if (index == 0)
    {
     //双层循环遍历像素点，不要图片边缘
      for (int x=10; x(y)[x+1] - gray.ptr(y)[x-1],
	    gray.ptr(y+1)[x] - gray.ptr(y-1)[x]
	  );
	  //模长小于50则梯度不明显就跳过
	  if (delta.norm() < 50)
	    continue;
	  ushort d = depth.ptr(y)[x];
	  if (d==0)
	    continue;
	  Eigen::Vector3d p3d = project2Dto3D(x, y, d, fx, fy, cx, cy, depth_scale);
	  float grayscale = float(gray.ptr(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();
    //在这个函数执行时measurements是不变的，只有不断读入的&gray灰度图是变化的
    poseEstimationDirect (measurements, &gray, K, Tcw);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration time_used = chrono::duration_cast>(t2-t1);
    cout << "Tcw = " << endl << Tcw.matrix() << endl;
  
    //画出特征点
    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)));
  
    //遍历数组measurements并进行操作
    for (Measurement m:measurements)
    {
      //随机选20%的关键点
      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;
}

//用直接法求位姿的函数poseEstimationDirect
bool poseEstimationDirect(const std::vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw)
{
  ceres::Problem problem;
  //定义位姿数组
  double pose[6];
  //用轴角进行优化
  Eigen::AngleAxisd rotationVector(Tcw.rotation());
  pose[0] = rotationVector.angle() * rotationVector.axis()(0);
  pose[1] = rotationVector.angle() * rotationVector.axis()(1);
  pose[2] = rotationVector.angle() * rotationVector.axis()(2);
  pose[3] = Tcw.translation()(0);
  pose[4] = Tcw.translation()(1);
  pose[5] = Tcw.translation()(2);
  
  //构建Ceres问题
  for (Measurement m:measurements)
  {
    ceres::CostFunction * costFunction = new SparseBA(gray, K(0,2), K(1,2), K(0,0), K(1,1), m.pos_world(0), m.pos_world(1), m.pos_world(2), double(m.grayscale));
    problem.AddResidualBlock(costFunction, NULL, pose);
  }
  
  ceres::Solver::Options options;
  options.num_threads = 4;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  
  cv::Mat rotateVectorCV = cv::Mat::zeros(3, 1, CV_64FC1);
  rotateVectorCV.at(0) = pose[0];
  rotateVectorCV.at(1) = pose[1];
  rotateVectorCV.at(2) = pose[2];
  
  cv::Mat RCV;
  cv::Rodrigues(rotateVectorCV, RCV);
  Tcw(0,0) = RCV.at(0,0); Tcw(0,1) = RCV.at(0,1); Tcw(0,2) = RCV.at(0,2);
  Tcw(1,0) = RCV.at(1,0); Tcw(1,1) = RCV.at(1,1); Tcw(1,2) = RCV.at(1,2);
  Tcw(2,0) = RCV.at(2,0); Tcw(2,1) = RCV.at(2,1); Tcw(2,2) = RCV.at(2,2);

  Tcw(0,3) = pose[3];
  Tcw(1,3) = pose[4];
  Tcw(2,3) = pose[5];
}

最后运行时前几帧和g2o相差不多，但是到第4帧时crash了，这个坑我以后再来填。

 

5.相比与RGB-D的直接法，单目直接法往往更加复杂。除了匹配未知之外，像素的距离也是待估计的，我们需要在优化时把像素深度也当作优化变量。阅读相关文献【57】，【59】，理解它的原理。

码够了一定的代码也该好好看看论文。文献【57】LSD-SLAM: Large-Scale Direct Monocular SLAM，文献【59】Semi-dense Visual Odometry for a Monocular Camera都是关于Jakob Engel大牛的LSD-SLAM算法的。单目SLAM，相较于其它SLAM算法，单目SLAM算法最大的问题就是尺度不确定。这样就容易出现尺度漂移等问题。但这又是一个双目以及RGB-D相机所不及的一个优点，由于尺度不确定，它对环境有着很强的适应性，可以实现室内室外环境的灵活切换，后两者往往对环境有着较大的限制性要求。接下来的是LSD-SLAM算法的流程结构。

（如有错误请指正）

LSD-SLAM算法主要创新点在于：

1.将深度噪声融入光度误差检测之中；

2.通过Sim(3)双向检测直接配准的方式实现了图优化中关键帧节点间边的构造。

但是这个算法存在着一些假设，甚至有些是强假设：

1.灰度不变性假设；（这个假设直接法都存在，属于强假设）

2.每一个像素的逆深度而不是深度服从高斯概率分布；（这个假设相较于假设深度服从高斯分布优越性较大，详细可以看我后续博客）

3.图像灰度噪声服从高斯分布；

4.像素的残差互相独立。（这个假设实际不成立，但是其在算法中保证了噪声在不同自由度之间的相关性）

单目SLAM中LSD-SLAM算是一种比较具有代表性的系统，通过对其代码的详细学习可以让我们更加深刻地了解单目SLAM地基本设计思路以及一些瓶颈问题。

 

6.由于图像的非凸性，直接法目前还只能用于短距离、非自动曝光的相机。你能否提出增强直接法健壮性的方案？阅读文献【58，60】。

由文中图像三维化显示不难发现，一般图像并不具备明显的凹凸性，其灰度值所形成的三维表面也是极其粗糙的。文献【60】Direct-Visual-Inertial-Odometry-with-Stereo-Cameras和文献【58】Direct Sparse Odometry都是LSD-SLAM的作者Jakob Engel参与的。这些文章都对图像的非凸性缺点进行了改良。

其中文献【60】主要是通过外加IMU传感器与光度误差紧耦合来克服实际图像中的光度误差的非凸性，主要是从构建最小能量函数和将IMU数据参与因子图优化来实现。因为IMU对于基体的短期位姿运动测量能够达到很精确的程度，但是长时间测量容易发生偏移，刚好视觉图像能起到纠正的作用，所以在此机理下，二者紧耦合能够取到不错的效果。详细算法及代码可以自行查看论文。

文献【58】是介绍DSO算法的文章。DSO算法算是直接法单目SLAM思想上比较成熟的一种算法。其中有很多比较先进的思想相较于以往的SLAM，包括对相机光度的重新修正，而且对于图像的非凸性，其也有解决的办法。在算法梯度下降时需要用到雅可比矩阵，DSO算法的雅可比矩阵主要包含三块，分别是图像雅可比、几何雅可比以及光度雅可比。后两者的值作者认为其相较于像素坐标变量而言是光滑的，所以只计算一次，但是图像雅可比显然不够光滑，作者设计了一种名为First Estimate Jacobian(FEJ)的优化方式。由于在优化的过程中会出现零空间（null space），边缘化操作容易导致零空间降维，所以FEJ强制让每一个地图点在同一时刻线性化，从而避免了零空间降维的发生，同时也保证了计算算量。

对于直接法健壮性增强的方法还有很多，但是能不真正增加计算量的方法却很少（前面提到的块操作也是一种）。直接法属于一种直接数据关联的方法，这一点比特征点法强不少，但是其优化过程比BA问题复杂太多，而且如果初始值估计不好对后续算法收敛性有着很大的影响。对于直接法单目SLAM的学习还有待加强。