在这一套代码中，激光部分其实是比较简略的，它主要起的是融合视觉并提供深度，避免了逆深度的求解（这不还是大号rgbd嘛。。。），而不是激光里程计的作用。
首先是processDepthMap，它接收了第二部分的视觉里程计与原有的点云，给第三部分的ba发布了深度图点云。
ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> ("/cam_to_init", 5, voDataHandler);
ros::Subscriber syncCloudSub = nh.subscribe<sensor_msgs::PointCloud2>
("/sync_scan_cloud_filtered", 5, syncCloudHandler);
ros::Publisher depthCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_cloud", 5);
depthCloudPubPointer = &depthCloudPub;接收视觉里程计是为了进行相邻两帧的坐标变换，与vo部分不断交互进行位姿估算，这样的做法似乎值得商榷。。。
void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
{
double time = voData->header.stamp.toSec();
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
//求出本帧和上一帧每一项的差值
double rx = voData->twist.twist.angular.x - rxRec;
double ry = voData->twist.twist.angular.y - ryRec;
double rz = voData->twist.twist.angular.z - rzRec;
if (ry < -PI) {
ry += 2 * PI;
} else if (ry > PI) {
ry -= 2 * PI;
}
double tx = voData->pose.pose.position.x - txRec;
double ty = voData->pose.pose.position.y - tyRec;
double tz = voData->pose.pose.position.z - tzRec;
rxRec = voData->twist.twist.angular.x;
ryRec = voData->twist.twist.angular.y;
rzRec = voData->twist.twist.angular.z;
txRec = voData->pose.pose.position.x;
tyRec = voData->pose.pose.position.y;
tzRec = voData->pose.pose.position.z;
//原有的平移是在世界坐标系下，现在将其变换到上一帧的坐标系下，这样将旋转与平移变换统一
double x1 = cos(yaw) * tx + sin(yaw) * tz;
double y1 = ty;
double z1 = -sin(yaw) * tx + cos(yaw) * tz;
double x2 = x1;
double y2 = cos(pitch) * y1 - sin(pitch) * z1;
double z2 = sin(pitch) * y1 + cos(pitch) * z1;
tx = cos(roll) * x2 + sin(roll) * y2;
ty = -sin(roll) * x2 + cos(roll) * y2;
tz = z2;
voDataInd = (voDataInd + 1) % keepVoDataNum;
voDataTime[voDataInd] = time;
voRx[voDataInd] = rx;
voRy[voDataInd] = ry;
voRz[voDataInd] = rz;
voTx[voDataInd] = tx;
voTy[voDataInd] = ty;
voTz[voDataInd] = tz;
double cosrx = cos(rx);
double sinrx = sin(rx);
double cosry = cos(ry);
double sinry = sin(ry);
double cosrz = cos(rz);
double sinrz = sin(rz);
if (time - timeRec < 0.5) {
pcl::PointXYZI point;
tempCloud->clear();
double x1, y1, z1, x2, y2, z2;
int depthCloudNum = depthCloud->points.size();
for (int i = 0; i < depthCloudNum; i++) {
point = depthCloud->points[i];
x1 = cosry * point.x - sinry * point.z;
y1 = point.y;
z1 = sinry * point.x + cosry * point.z;
x2 = x1;
y2 = cosrx * y1 + sinrx * z1;
z2 = -sinrx * y1 + cosrx * z1;
point.x = cosrz * x2 + sinrz * y2 - tx;
point.y = -sinrz * x2 + cosrz * y2 - ty;
point.z = z2 - tz;
double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
double timeDis = time - initTime - point.intensity;
if (fabs(point.x / point.z) < 2 && fabs(point.y / point.z) < 1 && point.z > 0.5 && pointDis < 15 &&
timeDis < 5.0) {
tempCloud->push_back(point);
}
}
depthCloud->clear();
pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
downSizeFilter.setInputCloud(tempCloud);
downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
downSizeFilter.filter(*depthCloud);
depthCloudNum = depthCloud->points.size();
tempCloud->clear();
for (int i = 0; i < depthCloudNum; i++) {
point = depthCloud->points[i];
//投影到z=10的相机坐标系下
if (fabs(point.x / point.z) < 1 && fabs(point.y / point.z) < 0.6) {
point.intensity = depthCloud->points[i].z;
point.x *= 10 / depthCloud->points[i].z;
point.y *= 10 / depthCloud->points[i].z;
point.z = 10;
tempCloud->push_back(point);
}
}
tempCloud2->clear();
downSizeFilter.setInputCloud(tempCloud);
downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
downSizeFilter.filter(*tempCloud2);
int tempCloud2Num = tempCloud2->points.size();
for (int i = 0; i < tempCloud2Num; i++) {
tempCloud2->points[i].z = tempCloud2->points[i].intensity;
tempCloud2->points[i].x *= tempCloud2->points[i].z / 10;
tempCloud2->points[i].y *= tempCloud2->points[i].z / 10;
tempCloud2->points[i].intensity = 10;
}
sensor_msgs::PointCloud2 depthCloud2;
pcl::toROSMsg(*tempCloud2, depthCloud2);
depthCloud2.header.frame_id = "camera2";
depthCloud2.header.stamp = voData->header.stamp;
depthCloudPubPointer->publish(depthCloud2);
}
timeRec = time;
}void syncCloudHandler(const sensor_msgs::PointCloud2ConstPtr& syncCloud2)
{
if (startCount < startSkipNum) {//每0.5秒提供一次原始点云
startCount++;
return;
}
if (!systemInited) {
initTime = syncCloud2->header.stamp.toSec();
systemInited = true;
}
double time = syncCloud2->header.stamp.toSec();
double timeLasted = time - initTime;
syncCloud->clear();
pcl::fromROSMsg(*syncCloud2, *syncCloud);
double scale = 0;
int voPreInd = keepVoDataNum - 1;
if (voDataInd >= 0) {
while (voDataTime[voRegInd] <= time && voRegInd != voDataInd) {
voRegInd = (voRegInd + 1) % keepVoDataNum;
}//轮转查询与当前点云时间戳最相近的里程计时间戳
voPreInd = (voRegInd + keepVoDataNum - 1) % keepVoDataNum;
double voTimePre = voDataTime[voPreInd];
double voTimeReg = voDataTime[voRegInd];//分别获取前后最相近的里程计时间
if (voTimeReg - voTimePre < 0.5) {
double scale = (voTimeReg - time) / (voTimeReg - voTimePre);
if (scale > 1) {//与哪个时间近就选择哪个里程计信息作为变换的依据
scale = 1;
} else if (scale < 0) {
scale = 0;
}
}
}
double rx2 = voRx[voRegInd] * scale;
double ry2 = voRy[voRegInd] * scale;
double rz2 = voRz[voRegInd] * scale;
double tx2 = voTx[voRegInd] * scale;
double ty2 = voTy[voRegInd] * scale;
double tz2 = voTz[voRegInd] * scale;
double cosrx2 = cos(rx2);
double sinrx2 = sin(rx2);
double cosry2 = cos(ry2);
double sinry2 = sin(ry2);
double cosrz2 = cos(rz2);
double sinrz2 = sin(rz2);
pcl::PointXYZI point;
double x1, y1, z1, x2, y2, z2;
int syncCloudNum = syncCloud->points.size();
for (int i = 0; i < syncCloudNum; i++) {
point.x = syncCloud->points[i].x;
point.y = syncCloud->points[i].y;
point.z = syncCloud->points[i].z;
point.intensity = timeLasted;
x1 = cosry2 * point.x - sinry2 * point.z;
y1 = point.y;
z1 = sinry2 * point.x + cosry2 * point.z;
x2 = x1;
y2 = cosrx2 * y1 + sinrx2 * z1;
z2 = -sinrx2 * y1 + cosrx2 * z1;
point.x = cosrz2 * x2 + sinrz2 * y2 - tx2;
point.y = -sinrz2 * x2 + cosrz2 * y2 - ty2;
point.z = z2 - tz2;
if (voDataInd >= 0) {
int voAftInd = (voRegInd + 1) % keepVoDataNum;
while (voAftInd != (voDataInd + 1) % keepVoDataNum) {
double rx = voRx[voAftInd];
double ry = voRy[voAftInd];
double rz = voRz[voAftInd];
double tx = voTx[voAftInd];
double ty = voTy[voAftInd];
double tz = voTz[voAftInd];
double cosrx = cos(rx);
double sinrx = sin(rx);
double cosry = cos(ry);
double sinry = sin(ry);
double cosrz = cos(rz);
double sinrz = sin(rz);
x1 = cosry * point.x - sinry * point.z;
y1 = point.y;
z1 = sinry * point.x + cosry * point.z;
x2 = x1;
y2 = cosrx * y1 + sinrx * z1;
z2 = -sinrx * y1 + cosrx * z1;
point.x = cosrz * x2 + sinrz * y2 - tx;
point.y = -sinrz * x2 + cosrz * y2 - ty;
point.z = z2 - tz;
voAftInd = (voAftInd + 1) % keepVoDataNum;
}
}
double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
if (fabs(point.x / point.z) < 2 && fabs(point.y / point.z) < 1.5 && point.z > 0.5 && pointDis < 15) {
depthCloud->push_back(point);
}
}
}stackDepthPoint其实就是接收上一个processDepthMap节点发布的点云，并且将它每5帧作为一个集合，交给bundle adjustment进行优化。
ros::Subscriber depthPointsSub = nh.subscribe<sensor_msgs::PointCloud2>
("/depth_points_last", 5, depthPointsHandler);
ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_stacked", 1);
depthPointsPubPointer = &depthPointsPub;void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
{
frameCount = (frameCount + 1) % 5;
if (frameCount != 0) {
return;
}
pcl::PointCloud<DepthPoint>::Ptr depthPointsCur = depthPoints[0];
depthPointsCur->clear();
pcl::fromROSMsg(*depthPoints2, *depthPointsCur);
for (int i = 0; i < keyframeNum - 1; i++) {
depthPoints[i] = depthPoints[i + 1];
depthPointsTime[i] = depthPointsTime[i + 1];
}
depthPoints[keyframeNum - 1] = depthPointsCur;
depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec();
keyframeCount++;
if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) {
depthPointsStacked->clear();
for (int i = 0; i < keyframeNum; i++) {
*depthPointsStacked += *depthPoints[i];
}
sensor_msgs::PointCloud2 depthPoints3;
pcl::toROSMsg(*depthPointsStacked, depthPoints3);
depthPoints3.header.frame_id = "camera";
depthPoints3.header.stamp = depthPoints2->header.stamp;
depthPointsPubPointer->publish(depthPoints3);
lastPubTime = depthPointsTime[keyframeNum - 1];
}
}最后是建图的部分，registerPointCloud节点，我们看到它接收的是来自transformMaintenance的融合后的里程计以及原始点云，发布的是最后的点云地图。
ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> ("/cam2_to_init", 5, voDataHandler);
ros::Subscriber syncCloudSub = nh.subscribe<sensor_msgs::PointCloud2>
("/sync_scan_cloud_filtered", 5, syncCloudHandler);
ros::Publisher surroundCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("/surround_cloud", 1);
surroundCloudPubPointer = &surroundCloudPub;void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
{
double time = voData->header.stamp.toSec();
double rx, ry, rz;
geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(rz, rx, ry);
double tx = voData->pose.pose.position.x;
double ty = voData->pose.pose.position.y;
double tz = voData->pose.pose.position.z;
voDataInd = (voDataInd + 1) % keepVoDataNum;
voDataTime[voDataInd] = time;
voRx[voDataInd] = rx;
voRy[voDataInd] = ry;
voRz[voDataInd] = rz;
voTx[voDataInd] = tx;
voTy[voDataInd] = ty;
voTz[voDataInd] = tz;
}
void syncCloudHandler(const sensor_msgs::PointCloud2ConstPtr& syncCloud2)
{
if (startCount < startSkipNum) {
startCount++;
return;
}
double time = syncCloud2->header.stamp.toSec();
syncCloud->clear();
pcl::fromROSMsg(*syncCloud2, *syncCloud);
double scaleCur = 1;
double scaleLast = 0;
int voPreInd = keepVoDataNum - 1;
if (voDataInd >= 0) {
while (voDataTime[voRegInd] <= time && voRegInd != voDataInd) {
voRegInd = (voRegInd + 1) % keepVoDataNum;
}//根据时间戳进行位姿的插值
voPreInd = (voRegInd + keepVoDataNum - 1) % keepVoDataNum;
double voTimePre = voDataTime[voPreInd];
double voTimeReg = voDataTime[voRegInd];
if (voTimeReg - voTimePre < 0.5) {
double scaleLast = (voTimeReg - time) / (voTimeReg - voTimePre);
double scaleCur = (time - voTimePre) / (voTimeReg - voTimePre);
if (scaleLast > 1) {
scaleLast = 1;
} else if (scaleLast < 0) {
scaleLast = 0;
}
if (scaleCur > 1) {
scaleCur = 1;
} else if (scaleCur < 0) {
scaleCur = 0;
}
}
}
double rx2 = voRx[voRegInd] * scaleCur + voRx[voPreInd] * scaleLast;
double ry2;
if (voRy[voRegInd] - voRy[voPreInd] > PI) {
ry2 = voRy[voRegInd] * scaleCur + (voRy[voPreInd] + 2 * PI) * scaleLast;
} else if (voRy[voRegInd] - voRy[voPreInd] < -PI) {
ry2 = voRy[voRegInd] * scaleCur + (voRy[voPreInd] - 2 * PI) * scaleLast;
} else {
ry2 = voRy[voRegInd] * scaleCur + voRy[voPreInd] * scaleLast;
}
double rz2 = voRz[voRegInd] * scaleCur + voRz[voPreInd] * scaleLast;
double tx2 = voTx[voRegInd] * scaleCur + voTx[voPreInd] * scaleLast;
double ty2 = voTy[voRegInd] * scaleCur + voTy[voPreInd] * scaleLast;
double tz2 = voTz[voRegInd] * scaleCur + voTz[voPreInd] * scaleLast;
double cosrx2 = cos(rx2);
double sinrx2 = sin(rx2);
double cosry2 = cos(ry2);
double sinry2 = sin(ry2);
double cosrz2 = cos(rz2);
double sinrz2 = sin(rz2);
//以上的插值完成后，再计算出插值得到的新位姿
//依次把相机坐标系下的路标点转换到世界坐标系下，求解世界坐标系下的点云
pcl::PointXYZ point;
int syncCloudNum = syncCloud->points.size();
for (int i = 0; i < syncCloudNum; i++) {
point = syncCloud->points[i];
double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
if (pointDis > 0.3 && pointDis < 5) {
double x1 = cosrz2 * point.x - sinrz2 * point.y;
double y1 = sinrz2 * point.x + cosrz2 * point.y;
double z1 = point.z;
double x2 = x1;
double y2 = cosrx2 * y1 + sinrx2 * z1;
double z2 = -sinrx2 * y1 + cosrx2 * z1;
point.x = cosry2 * x2 - sinry2 * z2 + tx2;
point.y = y2 + ty2;
point.z = sinry2 * x2 + cosry2 * z2 + tz2;
surroundCloud->push_back(point);
}
}
showCount = (showCount + 1) % (showSkipNum + 1);
if (showCount != showSkipNum) {
return;
}
tempCloud->clear();
int surroundCloudNum = surroundCloud->points.size();
for (int i = 0; i < surroundCloudNum; i++) {
point = surroundCloud->points[i];
double xDiff = point.x - voTx[voRegInd];
double yDiff = point.y - voTy[voRegInd];
double zDiff = point.z - voTz[voRegInd];
double pointDis = sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);
if (pointDis < 50) {
tempCloud->push_back(point);
}
}
surroundCloud->clear();
pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter;
downSizeFilter.setInputCloud(tempCloud);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(*surroundCloud);
sensor_msgs::PointCloud2 surroundCloud2;
pcl::toROSMsg(*surroundCloud, surroundCloud2);
surroundCloud2.header.frame_id = "/camera_init";
surroundCloud2.header.stamp = syncCloud2->header.stamp;
surroundCloudPubPointer->publish(surroundCloud2);
}总体看来，这一版本的v-loam其实是更注重视觉里程计（姑且称为vio）而并非像论文所描述的使用低频率的雷达里程计对vio的矫正，而视觉里程计与imu的融合方法似乎也没有实现共同更新它们的状态，与一般的vio来说有一定差别，当然，简单复现论文中的激光里程计也并非难事，只需在激光部分添加一次低频率的scan to map的匹配，再将位姿与vio进行一次融合。以实践角度上看，lego-loam的效果在数据集上略胜，在工业界的热度明显更高（毕竟开源），用起来效果也很好，而这一版本的v-loam更注重里程计的思想，没有加入后端和回环，作为定位算法似乎很不错。但是通过阅读，感觉可以改进的地方不仅这些，待到将v-slam源码全部刷一遍后再认真思考。
当然，v-loam并非完全不如lego-loam，从工程的角度上看，v-loam可以使用的传感器类型更丰富，既然用一个单目能够达到视觉里程计的作用，那就不必再用360°的激光雷达了！用一个90°的雷达（种草大疆livox系列）完全可以达到更好的效果。假定使用一个小fov的雷达，使用lego-loam建图定位又成了难题，定不住位啊，尤其是在平坦的墙体附近！
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
