v-loam源码阅读（一）视觉特征

在一开始写lego-LOAM的时候，我就对v-loam有所耳闻，毕竟同出于大神之手，但我一直错误地以为它是将视觉与激光雷达点云融合成一个大型的rgbd，就像我之前的某种错误的打开方式，用单目相机和雷达融合进行slam操作。。。当然，这个v-loam的包并非来自作者开源而是他人复现，但github上的星星也比较高，也具备很高的阅读价值。

值得一提的是，在一年多以前写的Lego-LOAM分析，引起了比较高的热度，其中一些复杂的计算被我略过，一些解说上的错误也引起了读者的攻讦，时隔一年后再次回顾loam系列，我将会重点阅读原本那些难以理解的内容，想必也会重新感觉受益匪浅。

<node pkg="demo_lidar" type="featureTracking" name="featureTracking" output="screen"/>
  <node pkg="demo_lidar" type="visualOdometry" name="visualOdometry" output="screen"/>
  <node pkg="demo_lidar" type="bundleAdjust" name="bundleAdjust" output="screen"/>
  <node pkg="demo_lidar" type="processDepthmap" name="processDepthmap" output="screen"/>
  <node pkg="demo_lidar" type="stackDepthPoint" name="stackDepthPoint" output="screen"/>
  <node pkg="demo_lidar" type="transformMaintenance" name="transformMaintenance" output="screen"/>
  <node pkg="demo_lidar" type="registerPointCloud" name="registerPointCloud" output="screen"/>

从launch文件上看，它具备了多达7个进程之多，囊括了图像的特征提取、视觉里程计、图像特征与点云融合（深度图）以及激光建图部分。在定位的过程中，它以视觉里程计的先验开始，中间经历了光束平差法，再将不同精度的里程计进行融合从而得到最终的位姿，而建图只是顺便将点云“去畸变“之后进行粘贴。

struct ImagePoint {
     float u, v;
     int ind;
};

POINT_CLOUD_REGISTER_POINT_STRUCT (ImagePoint,
                                   (float, u, u)
                                   (float, v, v)
                                   (int, ind, ind))

struct DepthPoint {
     float u, v;
     float depth;
     int label;
     int ind;
};

POINT_CLOUD_REGISTER_POINT_STRUCT (DepthPoint,
                                   (float, u, u)
                                   (float, v, v)
                                   (float, depth, depth)
                                   (int, label, label)
                                   (int, ind, ind))

这个过程最开始的部分是处理图像的节点，首先关注一下它涉及的话题，它接收原始的图像话题，发布点云形式的特征点以及给用户观看的特征图像。

  ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/raw", 1, imageDataHandler);

  ros::Publisher imagePointsLastPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_last", 5);
  imagePointsLastPubPointer = &imagePointsLastPub;

  ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show", 1);
  imageShowPubPointer = &imageShowPub;

 这个节点只有一个主线，那就是处理图像特征点，甚至没有计算深度，查找的方法依然是fast特征点。

void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
{
  timeLast = timeCur;
  timeCur = imageData->header.stamp.toSec() - 0.1163;

  IplImage *imageTemp = imageLast;
  imageLast = imageCur;
  imageCur = imageTemp;

  for (int i = 0; i < imagePixelNum; i++) {
    imageCur->imageData[i] = (char)imageData->data[i];
  }

  IplImage *t = cvCloneImage(imageCur);
  cvRemap(t, imageCur, mapx, mapy);//获得去畸变的图像
  //cvEqualizeHist(imageCur, imageCur);
  cvReleaseImage(&t);

  cvResize(imageLast, imageShow);//缩小为之前的四分之一？
  cvCornerHarris(imageShow, harrisLast, 3);//查找harris角点作为跟踪的依据

  CvPoint2D32f *featuresTemp = featuresLast;
  featuresLast = featuresCur;
  featuresCur = featuresTemp;

  pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
  imagePointsLast = imagePointsCur;
  imagePointsCur = imagePointsTemp;
  imagePointsCur->clear();

  if (!systemInited) {
    systemInited = true;
    return;
  }

  int recordFeatureNum = totalFeatureNum;
  for (int i = 0; i < ySubregionNum; i++) {
    for (int j = 0; j < xSubregionNum; j++) {
      int ind = xSubregionNum * i + j;
      int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
      //subregionWidth和subregionHeight是将图像边缘去除后，分割为12*8个小块，分块后再对每一块进行角点的跟踪
      if (numToFind > 0) {
        int subregionLeft = xBoundary + (int)(subregionWidth * j);
        int subregionTop = yBoundary + (int)(subregionHeight * i);
        CvRect subregion = cvRect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
        cvSetImageROI(imageLast, subregion);
        //在featuresLast数组后直接继续添加上一帧被跟踪的角点
        cvGoodFeaturesToTrack(imageLast, imageEig, imageTmp, featuresLast + totalFeatureNum,
                              &numToFind, 0.1, 5.0, NULL, 3, 1, 0.04);

        int numFound = 0;
        for(int k = 0; k < numToFind; k++) {
          featuresLast[totalFeatureNum + k].x += subregionLeft;
          featuresLast[totalFeatureNum + k].y += subregionTop;//以上一帧图像为基准记录角点运动

          int xInd = (featuresLast[totalFeatureNum + k].x + 0.5) / showDSRate;
          int yInd = (featuresLast[totalFeatureNum + k].y + 0.5) / showDSRate;

          if (((float*)(harrisLast->imageData + harrisLast->widthStep * yInd))[xInd] > 1e-7) {
            featuresLast[totalFeatureNum + numFound].x = featuresLast[totalFeatureNum + k].x;
            featuresLast[totalFeatureNum + numFound].y = featuresLast[totalFeatureNum + k].y;
            featuresInd[totalFeatureNum + numFound] = featuresIndFromStart;

            numFound++;
            featuresIndFromStart++;
          }
        }
        totalFeatureNum += numFound;
        subregionFeatureNum[ind] += numFound;

        cvResetImageROI(imageLast);
      }
    }
  }
  //对稀疏特征的光流进行跟踪，使用金字塔中的Lucas-Kanade方法，pyrCur是当前帧的金字塔缓存，cvTermCriteria是在金字塔中终止迭代的条件
  cvCalcOpticalFlowPyrLK(imageLast, imageCur, pyrLast, pyrCur,
                         featuresLast, featuresCur, totalFeatureNum, cvSize(winSize, winSize), 
                         3, featuresFound, featuresError, 
                         cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.01), 0);

  for (int i = 0; i < totalSubregionNum; i++) {
    subregionFeatureNum[i] = 0;
  }

  ImagePoint point;
  int featureCount = 0;
  double meanShiftX = 0, meanShiftY = 0;
  for (int i = 0; i < totalFeatureNum; i++) {
    double trackDis = sqrt((featuresLast[i].x - featuresCur[i].x) 
                    * (featuresLast[i].x - featuresCur[i].x)
                    + (featuresLast[i].y - featuresCur[i].y) 
                    * (featuresLast[i].y - featuresCur[i].y));

    if (!(trackDis > maxTrackDis || featuresCur[i].x < xBoundary || 
      featuresCur[i].x > imageWidth - xBoundary || featuresCur[i].y < yBoundary || 
      featuresCur[i].y > imageHeight - yBoundary)) {

      int xInd = (int)((featuresLast[i].x - xBoundary) / subregionWidth);
      int yInd = (int)((featuresLast[i].y - yBoundary) / subregionHeight);
      int ind = xSubregionNum * yInd + xInd;//subregion的编号

      if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
        featuresCur[featureCount].x = featuresCur[i].x;
        featuresCur[featureCount].y = featuresCur[i].y;
        featuresLast[featureCount].x = featuresLast[i].x;
        featuresLast[featureCount].y = featuresLast[i].y;
        featuresInd[featureCount] = featuresInd[i];
        //u=x/z，v=y/z，得到的是像素的归一化后的相机坐标系下的三维坐标
        point.u = -(featuresCur[featureCount].x - kImage[2]) / kImage[0];
        point.v = -(featuresCur[featureCount].y - kImage[5]) / kImage[4];
        point.ind = featuresInd[featureCount];
        imagePointsCur->push_back(point);

        if (i >= recordFeatureNum) {
          point.u = -(featuresLast[featureCount].x - kImage[2]) / kImage[0];
          point.v = -(featuresLast[featureCount].y - kImage[5]) / kImage[4];
          imagePointsLast->push_back(point);
        }

        meanShiftX += fabs((featuresCur[featureCount].x - featuresLast[featureCount].x) / kImage[0]);
        meanShiftY += fabs((featuresCur[featureCount].y - featuresLast[featureCount].y) / kImage[4]);

        featureCount++;
        subregionFeatureNum[ind]++;
      }
    }
  }
  totalFeatureNum = featureCount;
  meanShiftX /= totalFeatureNum;
  meanShiftY /= totalFeatureNum;

  sensor_msgs::PointCloud2 imagePointsLast2;
  pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
  imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
  imagePointsLastPubPointer->publish(imagePointsLast2);

  showCount = (showCount + 1) % (showSkipNum + 1);
  if (showCount == showSkipNum) {
    Mat imageShowMat(imageShow);
    bridge.image = imageShowMat;
    bridge.encoding = "mono8";
    sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
    imageShowPubPointer->publish(imageShowPointer);//间隔地发布带角点的图像
  }
}

这样将角点发布给了里程计节点。避免一次写太长，我们下一个节点再详细学习视觉里程计的实现。