hyye / lio-mapping Goto Github PK

When I test in corridor or outdoor with marginalization turn on, the system becomes more unstable. It looks like that errors are accumulating, and finally the system crashes. But if I turn off marginalization ,the system is more stable, but the accuracy and mapping are worse. I wonder if this is related to FEJ or something else.

Hi @hyye,
Is it possible to show all the built map without disappearing in rviz? Now the map will disappear in every certain time interval, is it possible to keep them all? (I know it might have some computational problem)
Secondly, every time I play my bag, the odometry and map always build far way slower than the bag. For example, when a 2 mins bag is finished rosbag play, but the results in rviz will keep updating (about 4 more mins slower than the bag), are there any way to avoid this?

Hello,

I am plotting the GT pose from the topic /Robot_7/pose with the docker image. However, the aft_mapped/camera and gt_pose transforms are not aligned. Is there some transform step that is not given to align the two?

Hello, big thanks for sharing this code.
I made a ros bag from KITTI raw data(2011_09_30_drive_0027_extract). This bag includes cam0, 100Hz imu and velodyne-HDL64e. I have tested this bag with VINS-Mono and lio-mapping(64_scans_test.launch), the result is good.
But the result of lio-mapping(test_outdoor.launch & map_4D_indoor.launch) is bad. lio-mapping is able to initialize but can't reach high level of accuracy in velodyne-HDL64e.

I was playing a dataset recorded by myself. In the first 20seconds, it ran well. However, after that, something got wrong, and I didn't know how to fix it.
The dataset was recorded outside door, and when I used "test_indoor.launch" and "test_outdoor.launch". The mistake was the same.

Hello, I want to get a dense map so I modify values in these files. However, the map is not as dense as those of other schemes(aloam, lego-loam).

In map_builder_node.cc, I reset the value as following,
MapBuilderConfig config; config.map_filter_size = 0.01

In MapBuilder.h, I reset values as following,
'struct MapBuilderConfig {
float corner_filter_size = 0.01;
float surf_filter_size = 0.01;
float map_filter_size = 0.6;

float min_match_sq_dis = 1.0;
float min_plane_dis = 0.2;
};'

In Estimator.h, I reset values as following,
float corner_filter_size = 0.01; float surf_filter_size = 0.01; float map_filter_size = 0.01;

I guess there are 2 reasons.

1. I miss some parameters.
2. The raw point cloud is over-segmentation so that a small number points make up the map.
   Could you give me some help?
   Thank you!

I am using fast1.bag but no rviz has no point cloud display

hello , hyye ! I have some question and need your help.
1、In the outdoor.yaml , the parameter window_size , opt_size . If I set it larger or smaller , what it would be influence ? In other word, how to debug the parameter .
2、Why extrinsic calibration only calibrate the rotation ? when we use the lidar align imu data, why the translation have no influence?
3、I set 0 to make it unchange , if the translation is not accurate enough , what it would be influence?
4、what is the parameter mean below and what is the influence?
keep_features:
gravity_fix:

Hi author,
I run the open source on my own data,
first I run roslaunch lio test_indoor.launch & and then run roslaunch lio map_4D_indoor.launch &. After these command, I play my bag by adding --clock at the end and after running for 20 sec, the process is died like below.

However, if I don't add --clock at the end, the process will not died in the whole bag, but the terminal of roslaunch lio test_indoor.launch & didn't show anything and also nothing shown in rviz as well. Any suggestions for this bug? Thank you!

as you have tested on urban17, can you provide the config file for us?

hello
i have been run
1.roslaunch lio test_indoor.launch
2.roslaunch lio map_4D_indoor.launch
3.rosbag play fast1.bag
but ,No map is displayed in rviz
What's wrong with me?

Hi anthor, nice Job ! Thank you for making this great work open source.

The document "Supplementary Material to-Tightly Coupled 3D Lidar Inertial Odometry and Mapping.pdf" can't be downloaded from the provided website. Can you uploaded it to github?

Thank you very much!

Hi,

thank you so much for answering questions 😄. In VINS-Mono the discrete time noise covariance matrix is approximated to the first order using

So dt * Gt * Qt * Gt' however in your supplementary material your approximation of Qd is

Where does the extra dt come from?

Hello, I am wondering why the first equation in (E.12) is satisfied? Here is define of the question.

Could you give some material about this equation? Thank you very mach!

Hi:

I just test the 'lio_mapping' with our own data, the odometry from topic 'predict_laser_odom' is quite good compared with the ground truth, but I did not see any output from the topic '/lio_map_builder/aft_mapped_to_init', actually this topic does not exist at all. What the reason of this?

May I ask what exact is this topic? Is Lio created the map itself? If it is, what kind of map it created?

I've tested lio_estimator_node with my own datasets and found that the memory grew over time. It grew from 9.2M to 2G in about 40 minutes. I think lio_estimator_node is just a lidar-imu odometry and it doesn't need to consume so much memory.

Thanks for the code from your guys.

After read through the paper i'm wondering if "Refinement With Rotational Constraints" implement in your code?
If yes, where can i find the part of it?

Thanks again.

It seems that running the data you provided will have Extrinsic Calibration successful and imu initialized. However, now I plugin my data and the bag can play normally to the end without any process died, but didn't show anything on Rviz and always show Extrinsic Calibration Failed in the terminal. Does this mean the result didn't converge? How to deal with this problem or what might cause them happened?

Hi, I am testing different lidar frequency on lio-mapping.

So far I found there are some related parameters such as scan_period and time_factor are respectively cast with default value as 0.1 and 10. Also in some lines of they are fed as fixed value parameters such as PointOdometry constructors(line 147 in estimator_node.cc and line 61 in odometry_node.cc) and TransformToEnd function calls(line 668, 670, 672, and 2416 in Estimator.cc ). But I don't know whether that's all and have no idea about what to further modify to parameterize lidar frequency as an option.

Thank you.

I noticed that global mapping can align with gravity because of refinement with rotational constraints. However, I still can't understand this part by your paper and the code in MapBuilder. Can you explain this part a bit more detailly~ Thank you!

Hi author,
I run the below command
roslaunch lio test_indoor.launch & and the process is died like below:

and after that I run the below command
roslaunch lio map_4D_indoor.launch and the process is died like below:

How can I solve this to make the code run?
Thank you!

I confused with the coarse gravity solver，
“ tmp_A = 0.5 * I3x3 * (dt12 * dt12 * dt23 + dt23 * dt23 * dt12);
tmp_b = (pl2 - pl1) * dt23 - (pl3 - pl2) * dt12
+ (rl2 - rl1) * plb * dt23 - (rl3 - rl2) * plb * dt12
+ rl2 * rlb * dp23 * dt12 + rl1 * rlb * dv12 * dt12 * dt23
- rl1 * rlb * dp12 * dt23;”
how to construct the least squares method for gravity ？
thanks for you

Hi @hyye,
Thank you for releasing the source code.
I have a question about time synchronization between lidar and IMU. My understanding is that for tightly couple approach, you need to have a precise timing of measurement messages.
If I understand the code correctly, in this function from src/imu_processor//MeasurementManager.cc, you simply compare the timestamp of IMU and lidar, which I think they are timestamped on arrival at the host PC and not being externally triggered, for buffering.
Is it one of the reasons why you need such a high output rate (at 400 Hz) for IMU?

I have tested your code in a large environment and I can not see the whole map. It seems rviz can only display part of it.

I followed the steps you mentioned to run the data “fast1.bag" normally, and then conversion completed the point cloud used "lio_save_bag_to_pcd" with a record bag, The result is like the picture above. is there something wrong with me? @hyye

Hi, I'm new in LIO/VIO. According to your paper, I think the most different part between LIO and LOAM is the constraint/factor construction. I found this in your paper:

In my opinion, LOAM simply use IMU data to give a predicted pose and focus on the second part of it. LIO add frame-to-frame pose constraint by IMU pre-integration. Am I right?
Threrefore, I'm confused with these constraints. the second part represents point-to-plane residuals and third part represents pose-to-pose residuals. Why can they be "added"? How to determine their covariance? Thank you~

hello ,hyye! Thanks for your brilliant work . Now i test my own dataset and there is a question .
My imu installation has a transform(oritation and translation) to the lidar , and I use the out_door.yaml to test my data (outdoor) , set the initial extrinstic para.
I found that the mapping result about z axis has a obvious drift . What may cause the problem ? or is there any influence about the imu installation location？

Hi @hyye ,

How would you adapt lio-mapping to work with the google cartographer datasets? I see they converted each lidar packets to a pointcloud while you use the accumulated clouds from the velodyne driver. Other than reassembling clouds, are there any parameters that neet to be adapted?

Thanks,

Hello,
From the commits I can see you have added support for KITTI. But From what I can tell:

• The tf tree is getting visualized.

• The LIDAR odometry is working

The IMU based smoothing is not working. How can I get it to work with IMU data? I tried remapping to KITTI topics but no use. Maybe I an looking at the wrong topic. The estimated trajectory should be published on /aft_mapped? correct?

Hi @hyye,
Thank you for releasing this great source code.
I am trying to run lio-mapping with hokuyo3d sensor.
hokuyo3d sensor: https://www.hokuyo-aut.jp/search/single.php?serial=224

However I encountered below error and I don't know How I can resolve the error.

I just rotated the sensor, this system began to drift before crash.
I set 2 to flag of extrinsic parameter calibration(Don't give initial guess parameters).
IMU output rate is about 600hz and LRF output rate is about 10hz.
how would you resolve this error? Also you processed velodyne points(maybe accumulate their points?), How would you adapt lio-mapping to work with hokuyo3d sensor?

Thanks,

Hi, I notice that a significant parameter 'msg_time_delay' is explicitly set in the config file. it controls the way to pair the laser data and the imu data. I also found that all '*config.yaml's set it as 0.05, but I don't know why.
Could you explain more about it, please? How to set it? How to measure it base the customized devices?

Hi,I found that the function ‘PointProcessor::DeSkew()’ does not implement in the code. But in your paper you said LIO can provide better performance with motion compensation.Does it mean the result could be better If I do the undistortion by myself?

hello:
What's the result of testing on kitti? How Lio-mapping ranking on that

Hello Author，
i can run your code use my laser and imu data，

I compare lio-mapping and VINS-mono，
I found that lio-mapping has no loop closure，
now，How do I add loop closure to lio-mapping.

hello ,hyye! Thanks for your brilliant work.
Now I am trying to understand your code and have some questions.

1. the function Estimator::BuildLocalMap maintaining the local map, and solves the pose of the last frame. In an outdoor environment, the flag of keep_Feature is false, the function Estimator::CalculateFeatures will accumulate all features from pivot_idx to window_size, and these features will be used to solve the pose of the lase frame. however, these features from pivot_idx to windows_size -1 are not associated with the pose of the last frame, the points of these features can't transform to the pivot_idx frame though the pose of the last frame.
2. Why not enable deskew in PointOdometry with imu, intuitively it's better.
3. the class of CircularBuffer may be a small bug, its index is out of order, which means that the function Estimator::SlideWindow may give the next frame an abnormal initial value. This is a simple example
   code:
   int loo = 8;
   CircularBuffer buf{loo};
   for (int i=0; i<20; i++)
   {
   buf.push(i);
   std::cout << "||\tindex: "<< i << "\tget: "<< buf[i]<< std::endl;
   }
   Result:
   CircularBuffer: 0 || index: 0 get: 0
   CircularBuffer: 0 1 || index: 1 get: 1
   CircularBuffer: 0 1 2 || index: 2 get: 2
   CircularBuffer: 0 1 2 3 || index: 3 get: 3
   CircularBuffer: 0 1 2 3 4 || index: 4 get: 4
   CircularBuffer: 0 1 2 3 4 5 || index: 5 get: 5
   CircularBuffer: 0 1 2 3 4 5 6 || index: 6 get: 6
   CircularBuffer: 0 1 2 3 4 5 6 7 || index: 7 get: 7
   CircularBuffer: 8 1 2 3 4 5 6 7 || index: 8 get: 1
   CircularBuffer: 8 9 2 3 4 5 6 7 || index: 9 get: 3
   CircularBuffer: 8 9 10 3 4 5 6 7 || index: 10 get: 5
   CircularBuffer: 8 9 10 11 4 5 6 7 || index: 11 get: 7
   CircularBuffer: 8 9 10 11 12 5 6 7 || index: 12 get: 9
   CircularBuffer: 8 9 10 11 12 13 6 7 || index: 13 get: 11
   CircularBuffer: 8 9 10 11 12 13 14 7 || index: 14 get: 13
   CircularBuffer: 8 9 10 11 12 13 14 15 || index: 15 get: 15
   CircularBuffer: 16 9 10 11 12 13 14 15 || index: 16 get: 9
   CircularBuffer: 16 17 10 11 12 13 14 15 || index: 17 get: 11
   CircularBuffer: 16 17 18 11 12 13 14 15 || index: 18 get: 13
   CircularBuffer: 16 17 18 19 12 13 14 15 || index: 19 get: 15

Hi,

would it be possible to give the derivation for the jacobians of the point to plane factor? I am a bit confused on how to find the Jacobians from equations (3) and (4) and how the jacobian matrix is 7 x 3.

Thanks,

I notice that you changed the expression to calculate the 'F matrix' and 'V matrix' in preintegrationBase.h.
The original definition in 'vins mono' is :
'F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * -dt;'
' V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * dt * dt;'
'V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * dt * dt;'
Somehow, they are modified as:
'F.block<3, 3>(0, 12) = -0.1667 * result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * -dt;'
'V.block<3, 3>(0, 0) = 0.5 * delta_q.toRotationMatrix() * dt * dt;'
''V.block<3, 3>(0, 6) = 0.5 * result_delta_q.toRotationMatrix() * dt * dt;''
There must be something that you explored, and then you modified them.
Could you please explain more about it with your professional insight?

Hi there,

thanks a lot for the nice work.

I think there should be a minimum vision (3.4.2, seeing this question) set for OpenCV, otherwise it meets error like

estimator_node.cc:(.text+0x9a1): undefined reference to `cv::Mat::updateContinuityFlag()'

Hi !

I want to use lio-mapping for real-time location, but I find that location in rviz is always slower than real movement. I use Velodyne 16 and a imu with 400Hz frequency, How can I solve this problem?

Thanks for advance!

Hi！I am currently doing Lidar-Imu calibration. Can you tell me how to get the initial external parameters of laser-imu. In addition, i record the still data and used imu-utils to calibrate the noise and bias of the IMU, but the accuracy is especially high, so can you tell me how to calibrate the noise of IMU with dynamic data. Thank you very much!

First of all, your job is really brilliant.
I am new for slam, and I have test your method by our own data collected by hesai lidar and stim 300 imu, but the rqt_gragh result shows that only the lidar data was used, so what should I do to use both lidar and imu data?
Thank you very much. My launch file is below.

<arg name="rviz" default="true" />

<node pkg="lio" type="lio_processor_node" name="lio_processor" output="screen">
    <param name="sensor_type" type="int" value="40" />
    <remap from="/velodyne_points" to="/pandar_points"/>
</node>
<node pkg="lio" type="lio_odometry_node" name="lio_odometry" output="screen" />
<node pkg="lio" type="lio_mapping_node" name="lio_mapping" output="screen" />

<group if="$(arg rviz)">
    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find lio)/rviz_cfg/lio_baseline.rviz" />
</group>

Hello，hyye.First,thanks for your open work,that is so amazing.im green in SLAM. I just confused about why “imu excitation is not enouth”appeared while using my own dataset,and about one minute later,with the imu initialized successfully,the map occurred but the datas in that one minute has lost.Is frequency of the imu not high enough?I used the MTi Xsens 100Hz,RS-16 10Hz
BTW,should it be "enough",i also saw "enouth"in the source code.
thank you very much!

Hello,

I successfully compiled the project on my machine, but when I try to run the rosbag the odom node crashes with the following error in the log file.

  File "/opt/ros/melodic/lib/python2.7/dist-packages/rosmaster/threadpool.py", line 218, in run
    result = cmd(*args)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rosmaster/master_api.py", line 210, in publisher_update_task
    ret = xmlrpcapi(api).publisherUpdate('/master', topic, pub_uris)
  File "/usr/lib/python2.7/xmlrpclib.py", line 1243, in __call__
    return self.__send(self.__name, args)
  File "/usr/lib/python2.7/xmlrpclib.py", line 1602, in __request
    verbose=self.__verbose
  File "/usr/lib/python2.7/xmlrpclib.py", line 1283, in request
    return self.single_request(host, handler, request_body, verbose)
  File "/usr/lib/python2.7/xmlrpclib.py", line 1316, in single_request
    return self.parse_response(response)
  File "/usr/lib/python2.7/xmlrpclib.py", line 1493, in parse_response
    return u.close()
  File "/usr/lib/python2.7/xmlrpclib.py", line 800, in close
    raise Fault(**self._stack[0])
Fault: <Fault -1: 'publisherUpdate: unknown method name'> 

The commands I use to run are the same as in the Readme.md file

Thanks for the great work.

I am testing your package and not able to achieve real-time performace on my own dataset (https://github.com/TixiaoShan/Stevens-VLP16-Dataset). I can only get at most 3Hz pose estimation. I have followed the README and built the package with RELEASE. Can you help take a loop into this problem?

Thank yo so much to update your code. I am confused about this function. Why is 'svd.matrixV().col(3)' equal to the extrinsic rotation between Lidar and IMU? Is there some supplementary material about this? Thank you so much.

bool ImuInitializer::EstimateExtrinsicRotation(CircularBuffer<PairTimeLaserTransform> &all_laser_transforms,
                                               Transform &transform_lb) {

  Transform transform_bl = transform_lb.inverse();
  Eigen::Quaterniond rot_bl = transform_bl.rot.template cast<double>();

  size_t window_size = all_laser_transforms.size() - 1;

  Eigen::MatrixXd A(window_size * 4, 4);

  for (size_t i = 0; i < window_size; ++i) {
    PairTimeLaserTransform &laser_trans_i = all_laser_transforms[i];
    PairTimeLaserTransform &laser_trans_j = all_laser_transforms[i + 1];

    Eigen::Quaterniond delta_qij_imu = laser_trans_j.second.pre_integration->delta_q_;

    Eigen::Quaterniond delta_qij_laser
        = (laser_trans_i.second.transform.rot.conjugate() * laser_trans_j.second.transform.rot).template cast<double>();

    Eigen::Quaterniond delta_qij_laser_from_imu = rot_bl.conjugate() * delta_qij_imu * rot_bl;

    double angular_distance = 180 / M_PI * delta_qij_laser.angularDistance(delta_qij_laser_from_imu);

//    DLOG(INFO) << i << ", " << angular_distance;

    double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;

    Eigen::Matrix4d lq_mat = LeftQuatMatrix(delta_qij_laser);
    Eigen::Matrix4d rq_mat = RightQuatMatrix(delta_qij_imu);

    A.block<4, 4>(i * 4, 0) = huber * (lq_mat - rq_mat);
  }

//  DLOG(INFO) << ">>>>>>> A <<<<<<<" << endl << A;

  Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<double, 4, 1> x = svd.matrixV().col(3);
  Quaterniond estimated_qlb(x);

  transform_lb.rot = estimated_qlb.cast<float>().toRotationMatrix();

  Vector3d cov = svd.singularValues().tail<3>();

  // NOTE: covariance 0.25
  if (cov(1) > 0.25) {
    return true;
  } else {
    return false;
  }
}

Hi, I noticed that parameter bg was first initialized when extrinsic calibration done. However, I still can't understand the theory behind the code in function EstimateGyroBias(). It seems that the residual to be optimized is 2*vec(IMU_ij^T * q_ij) (noted in EstimateGyroBias()). In my opinion, however, IMU_ij do not straightly equal q_ij because of extrinsic rotation R. Why does the extrinsic rotation R disappear in this part of code.

In the paper, the raw point cloud from lidar sensor is de-skewed before feature extraction, but in the code PointProcessor.cc I can not find this part, did I miss something?

LIO becomes very slow in my large environment, to a point where I have to pause my bag file to give LIO time to catch up.

Do you have any advice on how to tune the parameters to keep things running smoothly or constrain processing time?

I would also be interested in some documentation on the following parameters:

• msg_time_delay
• cutoff_deskew
• init_window_factor
• update_laser_imu

In PointOdometry.cc, line #450, function void PointOdometry::Process():

`

         float point_sq_dis, point_sq_dis2 = 25, point_sq_dis3 = 25;
          for (int j = closest_point_idx + 1; j < last_surf_size; j++) {
            if (int(last_surf_cloud_->points[j].intensity) > closestPointScan + 2.5) {
              break;
            }

            point_sq_dis = CalcSquaredDiff(last_surf_cloud_->points[j], point_sel);

            if (int(last_surf_cloud_->points[j].intensity) <= closestPointScan) {
              if (point_sq_dis < point_sq_dis2) {
                point_sq_dis2 = point_sq_dis;
                second_closet_point_idx = j;
              }
            } else {
              if (point_sq_dis < point_sq_dis3) {
                point_sq_dis3 = point_sq_dis;
                third_clost_point_idx = j;
              }
            }
          }
          for (int j = closest_point_idx - 1; j >= 0; j--) {
            if (int(last_surf_cloud_->points[j].intensity) < closestPointScan - 2.5) {
              break;
            }

            point_sq_dis = CalcSquaredDiff(last_surf_cloud_->points[j], point_sel);

            if (int(last_surf_cloud_->points[j].intensity) >= closestPointScan) {
              if (point_sq_dis < point_sq_dis2) {
                point_sq_dis2 = point_sq_dis;
                second_closet_point_idx = j;
              }
            } else {
              if (point_sq_dis < point_sq_dis3) {
                point_sq_dis3 = point_sq_dis;
                third_clost_point_idx = j;
              }
            }
          }

`
It seems that these codes are trying to find the 2nd and 3rd nearest point in those points next to(index increasing) and then previous to(index decreasing) the 1st nearest point.

However when loop in the increasing order, the code seems to find the 2nd nearest only in the lower rings(int(last_surf_cloud_->points[j].intensity) <= closestPointScan), and 3rd nearest only in the upper rings. Similar happen in the decreasing order.

I would like to know if it is a typo? or some consideration behind this? for by intuition, we want to find the 2nd & 3rd in both direction at the same time. Thanks for any help.

Hi anthor，
My robot moves horizontally indoors.
You don't known Extrinsic paramete，so in you indoor_test_config.yaml ,Your estimate_extrinsic is 2.
now i have Extrinsic parameter between IMU and Camera,How to modify my parameters，
Please tell me which parameters and how I need to modify.
Thank you