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);//间隔地发布带角点的图像}
}

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

v-loam源码阅读（一）视觉特征相关推荐

LOAM笔记及A-LOAM源码阅读
转载出处:LOAM笔记及A-LOAM源码阅读 - WellP.C - 博客园导读下面是我对LOAM论文的理解以及对A-LOAM的源码阅读(中文注释版的A-LOAM已经push到github,见A- ...
loam源码解析1 ： scanRegistration（一）
scanRegistration.cpp解析一.概述二.变量说明三.主函数四.IMU回调函数laserCloudHandler 1 接受IMU的角度和加速度信息 2 AccumulateIMU ...
Spring源码阅读源码环境搭建（一）
ring 源码阅读的搭建(一) 一下载spring源码进入官方网页:https://spring.io/projects/spring-framework 进入相关的github位置,下载zip包 ...
转-OpenJDK源码阅读导航跟编译
OpenJDK源码阅读导航 OpenJDK源码阅读导航博客分类: Virtual Machine HotSpot VM Java OpenJDK openjdk 这是链接帖.主体内容都在各链接中. ...
android tcp socket框架_最流行的 Web 框架 Gin 源码阅读
最近公司大部分项目开始往golang换, api的框架选定使用gin, 于是将 gin的源码看了一遍, 会用几篇文章将gin的流程及流程做一个梳理, 下面进入正题. gin框架预览上图大概是 gin ...
Java8 Hashtable 源码阅读
一.Hashtable 概述 Hashtable 底层基于数组与链表实现,通过 synchronized 关键字保证在多线程的环境下仍然可以正常使用.虽然在多线程环境下有了更好的替代者 Concurr ...
Java8 LinkedHashMap 源码阅读
如果你对 HashMap 的源码有了解的话,只需要一图就能知道 LinkedHashMap 的原理了,但是具体的实现细节还是需要去读一下源码. 一.LinkedHashMap 简介 1.1 继承结构 ...
zookeeper 源码阅读（1）
对于源码阅读的几点建议和方式: 1.尽量本地调试可以跑起来代码 2.debug 日志梳理代码执行流程,这样起到事半功倍的作用 3.干巴巴看代码毫无意义,难度极大 zk 是分别有c语言编写的和java ...
mybatis源码阅读(一)：SqlSession和SqlSessionFactory
转载自 mybatis源码阅读(一):SqlSession和SqlSessionFactory 一.接口定义听名字就知道这里使用了工厂方法模式,SqlSessionFactory负责创建SqlSe ...
HashMap jdk1.7源码阅读与解析
转载自 HashMap源码阅读与解析一.导入语 HashMap是我们最常见也是最长使用的数据结构之一,它的功能强大.用处广泛.而且也是面试常见的考查知识点.常见问题可能有HashMap存储结构是什 ...

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

v-loam源码阅读（一）视觉特征相关推荐

最新文章

热门文章