一、详细源码解读
#include <math.h>
#include <vector>
#include <aloam_velodyne/common.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
#include <mutex>
#include <queue>
#include <thread>
#include <iostream>
#include <string>
#include "lidarFactor.hpp"
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
int frameCount = 0;
double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;
const int laserCloudWidth = 21;
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851
int laserCloudValidInd[125];
int laserCloudSurroundInd[125];
// input: from odom
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
// ouput: all visualble cube points
pcl::PointCloud<PointType>::Ptr laserCloudSurround(new pcl::PointCloud<PointType>());
// surround points in map to build tree
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<PointType>());
//input & output: points in one frame. local --> global
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
// points in every cube
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];
//kd-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);
// wmap_T_odom * odom_T_curr = wmap_T_curr;
// transformation between odom's world and map's world frame
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLastBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLastBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullResBuf;
std::queue<nav_msgs::Odometry::ConstPtr> odometryBuf;
std::mutex mBuf;
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
PointType pointOri, pointSel;
ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath;
nav_msgs::Path laserAfterMappedPath;
// set initial guess
void pointAssociateTobeMapped(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_w(pi->x, pi->y, pi->z);
Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);
po->x = point_curr.x();
po->y = point_curr.y();
po->z = point_curr.z();
po->intensity = pi->intensity;
}
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
mBuf.lock();
cornerLastBuf.push(laserCloudCornerLast2);
mBuf.unlock();
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
//receive odomtry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();
// high frequence publish
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMappedHighFrec.publish(odomAftMapped);
}
void process()
{
while(1)
{
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}//确认时间同步
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
//将数据从std::queue<sensor_msgs::PointCloud2ConstPtr>转为pcl::PointCloud<PointType>::Ptr
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();//以上都是读取数据的代码
TicToc t_whole;
transformAssociateToMap();//为了便于学习，将代码放置如下：
void transformAssociateToMap()
{
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}
//Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
//Eigen::Vector3d t_wmap_wodom(0, 0, 0);初值 TicToc t_shift;
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;//10
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;//10
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;//5
这里可以看LOAM笔记及A-LOAM源码阅读_a-loam github代码-优快云博客
其中作者的图非常形象的展示了这个过程！感谢大佬！其中采用了基于空间范围划分的submap概念。
//将三维空间中的点映射到一个三维网格中，其中每个维度都被划分成了50单位大小的格子。
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
while (centerCubeI < 3)//如果中心cube的值偏小，就将整体数组向小的方向移动
{
for (int j = 0; j < laserCloudHeight; j++)//21
{
for (int k = 0; k < laserCloudDepth; k++)//11
{
int i = laserCloudWidth - 1;
//初始化一个变量 i，表示点云数据的宽度维度的索引，从最后一个元素开始
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; i >= 1; i--)//从 i 开始递减，直到1。
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//更新角点云数据数组。
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;//将更新后的角点云数据指针赋值回数组。
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI++;
laserCloudCenWidth++;
}
while (centerCubeI >= laserCloudWidth - 3))//反之如果中心cube的值偏大，就将整体数组向大的方向移动
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; i < laserCloudWidth - 1; i++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
while (centerCubeJ < 3)
{
//对centerCubeJ的处理方法一样，代码省略。
}
while (centerCubeJ >= laserCloudHeight - 3)
{
......
}
while (centerCubeK < 3)
{
......
}
while (centerCubeK >= laserCloudDepth - 3)
{
......//最终结果应该是构建的点云格网位于点云的中部位置
//所以以上代码的作用应该是将三维点云数据的中心映射到三维格网中
}
int laserCloudValidNum = 0;//用于存储有效点云数据的数量。
int laserCloudSurroundNum = 0;//用于存储周围点云数据的数量。
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
{
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)
{
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}//建立中心点的相邻点索引
//laserCloudCornerArray是与中心点相邻几个格网中的角点。
}
}
}
laserCloudCornerFromMap->clear();
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());//智能指针
laserCloudSurfFromMap->clear();
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
//将相邻的角点和面点加入到laserCloudCornerFromMap和laserCloudSurfFromMap
}
int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
//downSizeFilterCorner 滤波器首先使用 setInputCloud 方法加载输入点云，然后调用 filter 方法执行下采样操作。下采样后的结果存储在 laserCloudCornerStack 点云对象中，这个对象随后可以用于进一步的处理或分析。
体素网格滤波器（VoxelGrid）是一种常见的点云下采样方法，它将空间划分为一个三维网格，并在每个体素内部平均或近似点云数据，以减少点的数量。
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
printf("map prepare time %f ms\n", t_shift.toc());//以上是准备工作的代码
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{//我们先看如果相邻的角点和边点不够会怎么样？
//以下着重看怎么构建损失函数。
TicToc t_opt;
TicToc t_tree;
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
//为角点和边点构建kd树。
printf("build tree time %f ms \n", t_tree.toc());
for (int iterCount = 0; iterCount < 2; iterCount++)//优化两次
{
//ceres::LossFunction *loss_function = NULL;
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
//参数 0.1 是尺度参数，用于控制损失函数在多大程度上对离群点（outliers）进行惩罚。
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
pointOri = laserCloudCornerStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
//将点 pointOri 从雷达坐标系转换到世界坐标系。转换后的点存储在 pointSel 中。
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点参考自 LOAM笔记及A-LOAM源码阅读_a-loam github代码-优快云博客
//求线的法向量用的是PCA方法
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
center = center / 5.0;//计算五个最临近点的中心。
//计算协方差矩阵。
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
//计算协方差矩阵的特征值和特征向量
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
// 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量 Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
// 如果最大的特征值 >> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直” Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 从中心点沿着方向向量向两端移动0.1m，构造线上的两个点ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
//残差距离即点到线的距离 problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
int surf_num = 0;
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
-
// 假设平面不通过原点，则平面的一般方程为Ax + By + Cz + 1 = 0。 -
// 用上面的2个矩阵表示平面方程就是 matA0 * norm（A, B, C） = matB0。 -
B0是全是-1的列向量，A0是观测值5个点的xyz坐标，要求的是法向量norm。
if (pointSearchSqDis[4] < 1.0)
{
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
// find the norm of plane
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
//使用 Eigen 库的 colPivHouseholderQr() 方法，对矩阵 matA0 进行列主元Householder QR分解，然后使用 solve 方法求解线性方程组 matA0 * X = matB0，得到平面的法向量 norm。
double negative_OA_dot_norm = 1 / norm.norm();
norm.normalize();
//调用 normalize() 方法将法向量 norm 归一化，使其长度为1。
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well不够平
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
//
- 这个表达式根据平面方程 Ax+By+Cz+D=0Ax+By+Cz+D=0 计算第
j个最近邻点到平面的距离。这里 (A,B,C)(A,B,C) 是法向量norm的各分量，(x,y,z)(x,y,z) 是点的坐标。 negative_OA_dot_norm是常数项 DD，在之前的步骤中已经计算过，它是法向量的模长的倒数。fabs函数用来计算表达式的绝对值，因为距离不能是负数。
{
planeValid = false;
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (planeValid)
{//构造点到面的距离残差项
ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
laserCloudSurfFromMap->points[pointSearchInd[j]].y,
laserCloudSurfFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
TicToc t_solver;//求解损失向量
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
}
else
{
ROS_WARN("time Map corner and surf num are not enough");
}
transformUpdate();
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
TicToc t_add;
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
//po->intensity = 1.0;
} int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
//将三维点云数据映射到三维格网中
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
//为laserCloudCornerArray赋值
}
}
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
for (int i = 0; i < laserCloudValidNum; i++)
{
int ind = laserCloudValidInd[i];
pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());
TicToc t_pub;
//publish surround map for every 5 frame
if (frameCount % 5 == 0)
//这个条件判断意味着每处理5帧数据，就会执行一次里面的代码块。
{
laserCloudSurround->clear();
//清空 laserCloudSurround 点云对象，为新的数据发布做准备。
for (int i = 0; i < laserCloudSurroundNum; i++)
{
int ind = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
//*laserCloudSurround += *laserCloudSurfArray[ind]; 这两行将角点云和表面点云数据添加到 laserCloudSurround 中。
将 laserCloudSurround 转换为ROS消息格式 sensor_msgs::PointCloud2。
设置消息的时间戳和坐标帧ID。
通过 pubLaserCloudSurround.publish(laserCloudSurround3); 发布点云数据。
}
if (frameCount % 20 == 0)
{
pcl::PointCloud<PointType> laserCloudMap;
for (int i = 0; i < 4851; i++)
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "/aft_mapped"));
frameCount++;
}
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;//初始化ros结点
float lineRes = 0;
float planeRes = 0;
nh.param<float>("mapping_line_resolution", lineRes, 0.4);
nh.param<float>("mapping_plane_resolution", planeRes, 0.8);
//0.4和0.8是划分的格网尺寸
printf("line resolution %f plane resolution %f \n", lineRes, planeRes);
downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);0.4*0.4*0.4的三维格网。
downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);
ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
//接收来自里程计的数据，调用回调函数。
用std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLastBuf;存储数据
ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);
pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);
pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);
pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);
pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);
//这几行用于发布数据，大概发布的是配准后的点云数据和里程计数据，具体的数据用途我们在功能函数中看。
for (int i = 0; i < laserCloudNum; i++)
{
laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
}
//reset方法用于重置智能指针，使其指向一个新的pcl::PointCloud<PointType>对象。
std::thread mapping_process{process};
//然后来看process函数
ros::spin();
return 0;
}
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
