Ｖ-LOAM 代码分析 || 1、视觉的特征提取

启动文件中七个节点：

  <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"/>

包括：视觉的特征提取、视觉里程计、图像特征与点云融合激光建图部分

第一部分：
该部分只有一个功能就是对视觉信息进行特征提取。
imageDataSub ：接收原始的图像topic，发布点云形式的特征点以及可显示的特征图像。
imageDataHandler ：fast特征点处理图像。

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>

#include "cameraParameters.h"
#include "pointDefinition.h"

using namespace std;
using namespace cv;

bool systemInited = false;
double timeCur, timeLast;

const int imagePixelNum = imageHeight * imageWidth;
CvSize imgSize = cvSize(imageWidth, imageHeight);

IplImage *imageCur = cvCreateImage(imgSize, IPL_DEPTH_8U, 1);
IplImage *imageLast = cvCreateImage(imgSize, IPL_DEPTH_8U, 1);

int showCount = 0;
const int showSkipNum = 2;
const int showDSRate = 2;
CvSize showSize = cvSize(imageWidth / showDSRate, imageHeight / showDSRate);

IplImage *imageShow = cvCreateImage(showSize, IPL_DEPTH_8U, 1);
IplImage *harrisLast = cvCreateImage(showSize, IPL_DEPTH_32F, 1);

CvMat kMat = cvMat(3, 3, CV_64FC1, kImage);
CvMat dMat = cvMat(4, 1, CV_64FC1, dImage);