wykxwyc / lego-loam_noted

LeGO-LOAM代码注释与学习

LeGO-LOAM_NOTED

LeGO-LOAM代码注释与学习

博客链接

目前进行的一些更新都会在CSDN上进行，全部更新完会再更新个人博客上的内容。 CSDN博客的内容链接：
1.地图优化代码理解
2.图像重投影代码理解
3.特征关联代码理解
4.LeGO-LOAM代码中数学公式的解析
5.LeGO-LOAM论文翻译(内容精简)
6.关于位姿变换的一道题

个人博客上的目前还没有进行更新:
1.地图优化代码理解
2.图像重投影代码理解
3.特征关联代码理解
4.LeGO-LOAM代码中数学公式的解析
5.LeGO-LOAM论文翻译(内容精简)

实验与说明

The cordinate of velodyne LiDAR:

The cordinate of frame-of-integrated_to_init:

Test without IMU:

ROS nodes in LeGO-LOAM:

About update

2018-12-28 Forked from LeGO-LOAM: https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
2019-10-08 Update the comment and the blog

lego-loam_noted's Issues

TransformToStart

楼主，好多人都说TransformToStart这个函数是将当前点转换到上一帧坐标下，可是和强度有什么关系呢？为什么TransformToStart里面要先用intensity信息？
我用自己的包不用imu跑的过程中，由于有一个点的强度信息是nan，导致转换过之后点的xyz坐标都是nan，结果程序崩溃了。所以想问下TransformToStart具体是什么作用？

void TransformToStart(PointType const * const pi, PointType * const po)
{
    float s = 10 * (pi->intensity - int(pi->intensity));

    float rx = s * transformCur[0];
    float ry = s * transformCur[1];
    float rz = s * transformCur[2];
    float tx = s * transformCur[3];
    float ty = s * transformCur[4];
    float tz = s * transformCur[5];

    float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
    float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
    float z1 = (pi->z - tz);

    float x2 = x1;
    float y2 = cos(rx) * y1 + sin(rx) * z1;
    float z2 = -sin(rx) * y1 + cos(rx) * z1;

    po->x = cos(ry) * x2 - sin(ry) * z2;
    po->y = y2;
    po->z = sin(ry) * x2 + cos(ry) * z2;
    po->intensity = pi->intensity;
}

运行直接死掉的原因

你好，谢谢你的注释。我现在运行LeGO-LOAM时，进程会直接死掉，不知道是什么原因。。。希望您指点一下
报错如下：
why@why-desktop:~/SLAM_WS/LeGo-Loam$ roslaunch lego_loam run.launch
... logging to /home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/roslaunch-why-desktop-23115.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://why-desktop:45243/

SUMMARY

PARAMETERS

• /rosdistro: kinetic
• /rosversion: 1.12.14
• /use_sim_time: True

NODES
/
base_link_to_camera (tf/static_transform_publisher)
camera_init_to_map (tf/static_transform_publisher)
featureAssociation (lego_loam/featureAssociation)
imageProjection (lego_loam/imageProjection)
mapOptmization (lego_loam/mapOptmization)
rviz (rviz/rviz)
transformFusion (lego_loam/transformFusion)

ROS_MASTER_URI=http://localhost:11311

process[rviz-1]: started with pid [23137]
process[camera_init_to_map-2]: started with pid [23138]
process[base_link_to_camera-3]: started with pid [23139]
process[imageProjection-4]: started with pid [23140]
process[featureAssociation-5]: started with pid [23147]
process[mapOptmization-6]: started with pid [23155]
process[transformFusion-7]: started with pid [23161]
[ INFO] [1577774128.780622997]: ----> Feature Association Started.
[ INFO] [1577774128.953148373]: ----> Map Optimization Started.
[featureAssociation-5] process has died [pid 23147, exit code -11, cmd /home/why/SLAM_WS/LeGo-Loam/devel/lib/lego_loam/featureAssociation __name:=featureAssociation __log:=/home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/featureAssociation-5.log].
log file: /home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/featureAssociation-5*.log
[ INFO] [1577774129.024141602]: ----> Transform Fusion Started.
[imageProjection-4] process has died [pid 23140, exit code -11, cmd /home/why/SLAM_WS/LeGo-Loam/devel/lib/lego_loam/imageProjection __name:=imageProjection __log:=/home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/imageProjection-4.log].
log file: /home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/imageProjection-4*.log
[mapOptmization-6] process has died [pid 23155, exit code -11, cmd /home/why/SLAM_WS/LeGo-Loam/devel/lib/lego_loam/mapOptmization __name:=mapOptmization __log:=/home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/mapOptmization-6.log].
log file: /home/why/.ros/log/751293c4-2b09-11ea-831c-7085c2882345/mapOptmization-6*.log

这幅图片表达的是什么意思？雷达是顺时针转还是逆时针转？

注释有误

LeGO-LOAM_NOTED/src/LeGO-LOAM/src/featureAssociation.cpp

Line 1088 in fbb9847

void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz,

这个函数只有在使用IMU时才会起到作用，是否使用IMU，估计出来的transformSum[]中的旋转移项含义是不一样的；
若使用IMU，transformSum[]只是估计了一个旋转的小量，此函数将这个小量与IMU测量的一帧点云Lidar发生的旋转结合；
对应的旋转矩阵是 R_{k+1}^{0} * R_{YXZ}^{-imuStart} * R_{ZXY}^{imuEnd}

这辆小车上面有没有安装摄像头？/camera坐标系 和/integrated_to_int坐标系有什么关系？

lego_loam中这个方法,为什么先绕z旋转,再绕x旋转,后绕y旋转?

void pointAssociateToMap(PointType const * const pi, PointType * const po)
{
    // 进行6自由度的变换，先进行旋转，然后再平移
    // 主要进行坐标变换，将局部坐标转换到全局坐标中去	

    // 先绕z轴旋转
    //     |cosrz  -sinrz  0|
    //  Rz=|sinrz  cosrz   0|
    //     |0       0      1|
    // [x1,y1,z1]^T=Rz*[pi->x,pi->y,pi->z]
    float x1 = cYaw * pi->x - sYaw * pi->y;
    float y1 = sYaw * pi->x + cYaw * pi->y;
    float z1 = pi->z;

    // [x2,y2,z2]^T=Rx*[x1,y1,z1]
    //    |1     0        0|
    // Rx=|0   cosrx -sinrx|
    //    |0   sinrx  cosrx|
    float x2 = x1;
    float y2 = cRoll * y1 - sRoll * z1;
    float z2 = sRoll * y1 + cRoll * z1;

    // 最后再绕Y轴旋转，然后加上平移
    //    |cosry   0   sinry|
    // Ry=|0       1       0|
    //    |-sinry  0   cosry|
    po->x = cPitch * x2 + sPitch * z2 + tX;
    po->y = y2 + tY;
    po->z = -sPitch * x2 + cPitch * z2 + tZ;
    po->intensity = pi->intensity;
}

坐标系可视化图给错了吧

The cordinate of frame-of-integrated_to_init，这个坐标系，错了吧？

关于两步LM优化的代码实现

感觉代码里对corner和surf点的求导计算都汇总在了coeffSel里，LM优化直接得到六个自由度，并非像论文所说的分两步，是吗？

transformToStart, transformToStartImu区别

@wykxwyc 您好,请问transformToStart, transformToStartImu有什么区别?transformCur代表了哪两个坐标系间的转换?

IMU该如何融入使用

您好，我最近在看lego_loam的代码，不知道激光坐标系和IMU坐标是怎么规定的？怎么样在自己的激光和IMU运行legoLoam？不知道您解决没有。

论文中说的标签匹配好像没有在代码中体现啊

matV2.at<float>(i, j) = 0;

请问退化处理那，这句代码有何用呢？

the problem about loopClosureEnableFlag

大佬您好，感谢分享受益匪浅，有个问题很困惑向您咨询一下，回环检测的标志位loopClosureEnableFlag，只看到了对它的定义和利用它进行判断，但相应的状态转换时的赋值没看到，是什么原因？
在这里用中文会不会显得不是那么的专业啊，但是我英语好差啊....