Hi, sorry to bother you.

I am implementing your algorithm on Ubuntu 16.04, and it worked for your demo data.
But when I used my own bag file and 3D map it says globalmap not received.
Could you please tell me why?

Thanks

Hi, could you please explain how you built the IMU data? Did you use the Velodyne embedded sensor or an external one? How did you calculate the orientation? Did you use the common filters from ROS imu_tools or something else? What about the covariance matrices?

Thank you for the help :)

Dear friend,

Thanks for the package.
I had tried using your package for localization where I found some drifts in localization even when Im fusing with IMU.

I used DIRECT7 method for registrationa and localization
NDT_OMP

I saved map at 0.01 resolution

when I try to localize i found some warning like
"Leaf size is too small for the input dataset. Integer indices would overflow."

I altered downsample_resolution and checked as well. Couldnt figure.I attached a localization config file screenshot.Please check.

Kindly help me to gain the accuracy.
Suggest me some steps.

Thanks a lot

Hi Kiode3, thanks for this wonderful ROS package. I am a student looking to learn more about the fundamentals of the UKF you implemented. Is there a particular paper that details your implementation?

When I use "rosparam set use_sim_time true" "roslaunch hdl_localization hdl_localization.launch"
it will stop on the this staement "[ INFO] [1568861604.901954411]: Initializing nodelet with 8 worker threads."
if I use "rosparam set use_sim_time false"
It wiil report the error like this
"[FATAL] [1568860914.615317735]: Failed to load nodelet '/globalmap_server_nodeletof typehdl_localization/GlobalmapServerNodeletto managervelodyne_nodelet_manager'
[FATAL] [1568860914.615321512]: Failed to load nodelet '/hdl_localization_nodeletof typehdl_localization/HdlLocalizationNodeletto managervelodyne_nodelet_manager'"
I use ros-indigo on ubuntu14.04.

Hello, I compiled all the passes according to your installation requirements, but when positioning, the pose did not move, always fluctuating around the starting point. Have you ever encountered such a problem? Thanks a lot!

Could you upload the paper pleasure? I am very interested in this project. I search your paper but I can't find it in the internet .Also,I would like to ask whether there are other open soure about localization.Many thanks!

Hi,
I am trying to run the node on my own bag file, but the position field in the odom topic published by the node is zero. Everything else seems to be correct though. I have attached the Tf tree of the output below.

double InformationMatrixCalculator::calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range) {
pcl::search::KdTree::Ptr tree_(new pcl::search::KdTree());
tree_->setInputCloud(cloud1);

double fitness_score = 0.0;

// Transform the input dataset using the final transformation
pcl::PointCloud input_transformed;
pcl::transformPointCloud (*cloud2, input_transformed, relpose.cast());

I have a question about the relpose. Under my understanding, the relpose is the transformation from clould1 frame to cloud2 frame. so in my thinking, it should exchange cloud1 with cloud2.
for example: tree_->setInputCloud(cloud2);
pcl::transformPointCloud (*cloud1, input_transformed, relpose.cast());
I am not very sure it , so i am looking for your reply.

Hello. Thanks for the great works!

I am now running hdl_localization using my own data. I made my own PCD using hdl_graph_slam, and have a right format rosbag file.

It has no problem while running on a small pcd map. I found a problem on a bigger map, it fails to localization always on the same spot. See the video in the link (fails around 3:00) :
https://youtu.be/pI-KB9ZZ8tA

Changed parameters are here:
invert_imu -> true
cool_time_duration -> 0.1

When 'invert_imu' is false, it fails to localization a way before that spot when it is true. So I am using 'invert_imu' as true.

Additionally, I have a warning message. I don't know is it related to my problem, but I am posting it :
"[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow."

Now, I have a question. Do you remember the 'road-flipped' issue of mine? I thought it is related to the IMU data again, so I checked the source code until 'pose_estimator.hpp' but I couldn't find any error. (I didn't check inside of 'ukf')

Can you tell me how to solve this problem??

Hi,

I have recorded my own 3D pointcloud map and wanna to try it out with hdl_localization package. However, the loaded map orientation if off the plane with RPY=90', 0', 0'
How should i correct the orientation of the loaded 3D pointcloud map?

I use ROS package "pointcloud_to_pcd" to save the map.

Best.
Samuel

hi @koide3 I use my own data to test hdl_localizaiton.After something modified, I can run hdl_localizaiton seccessfully. But the result is not well.Compared with ndt only, hdl_localizaiton even get worse result which sometimes get jumped pose.Here is my launch file:

<!-- globalmap_server_nodelet -->
<node pkg="nodelet" type="nodelet" name="globalmap_server_nodelet" args="load hdl_localization/GlobalmapServerNodelet $(arg nodelet_manager)">
  <param name="map_format" type="string" value="binary_octomap" />  <!--pcd/octomap_binary-->
  <param name="globalmap" value="$(find hdl_localization)/data/lio_sam_map.bt" />
  <param name="convert_utm_to_local" value="false" />
  <param name="downsample_resolution" value="0.1" />
</node>

<!-- hdl_localization_nodelet -->
<node pkg="nodelet" type="nodelet" name="hdl_localization_nodelet" args="load hdl_localization/HdlLocalizationNodelet $(arg nodelet_manager)">
    <remap from="/velodyne_points" to="$(arg points_topic)" />
    <remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
    <!-- odometry frame_id -->
    <param name="odom_child_frame_id" value="$(arg odom_child_frame_id)" />
    <!-- imu settings -->
    <!-- during "cool_time", imu inputs are ignored -->
    <param name="use_imu" value="true" />
    <param name="invert_imu" value="false" />
    <param name="cool_time_duration" value="0" />
    <!-- ndt settings -->
    <!-- if NDT is slow for your PC, try DIRECT1 serach method, which is a bit unstable but extremely fast -->
    <param name="ndt_neighbor_search_method" value="DIRECT7" />
    <param name="ndt_resolution" value="2.0" />
    <param name="downsample_resolution" value="0.1" />
    <!-- if "specify_init_pose" is true, pose estimator will be initialized with the following params -->
    <!-- otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" -->
    <param name="specify_init_pose" value="true" />
    <param name="init_pos_x" value="0.0" />
    <param name="init_pos_y" value="0.0" />
    <param name="init_pos_z" value="0.0" />
    <param name="init_ori_w" value="1.0" />
    <param name="init_ori_x" value="0.0" />
    <param name="init_ori_y" value="0.0" />
    <param name="init_ori_z" value="0.0" />
</node>

@koide3 Hello, I found that the covariance matrix is not positive definite in the test process, and then decomposed into nan, the whole software hangs up.I tried to use the ensurepostive function, but the result was the same

I find that the pose estimator doesn't work and the car doesn't move. I don't know if the problem is the leaf size is too small for the input datase.
So I changed the leaf size of the global map from 1 to 10 and also the input point cloud from 0.1 to 1.0, but the problem still remain.

Thanks for sharing your awesome project.

However, I wonder the prediction part of PoseSystem that almost copies the past state expect of some portion of the gyro.
Velocity stays 0 since it gets continuously updated by itself.
Position is from the past frame.

Or do I misunderstand?

Hi koide3,
which frame does imu use in hdl localization? front left up?

Hi, I'm using VLP16 and trying to use my bag file into your system but nothing appear in Rviz.

Hi @koide3

I have been trying to use hdl_localization along with my Gazebo simulation. I have pointcloud map recorded and able to be loaded onto the RVIZ. However, when i moved my vehicle around, RVIZ showed that my vehicle was moving around. But the aligned pointcloud, doesn't move with the vehicle. Here's the picture:

I go through the codes. And i found the below might be the reason why the aligned pointcloud doesn't move with vehicle. I guess it's due to aligned pointcloud is published on "map" frame. Here i attached the picture of my guess:

By the way, I modified the original code, in order to match my usage, i change the frame_id from "velodyne" to "3d_laser". Here's the picture of modified area.

Besides, i also attached the Nodegraph and TF tree that is used here.

As you can see, for the "odom" -> "base_link", i use robot_localization pkg to publish this transform. While I use the static_transform_publisher from tf2_ros pkg to publish "map"->"odom" transform. It's located in the launch file as below:

Please guide me on how to get the aligned pointcloud to work normally, as it should be moved together with the vehicle frame (base_link) or lidar frame (3d_laser).

Best.
Samuel

Hi, My name is Welly. Currently I want to implement your code on my system with Velodyne Lidar VLP-16. Would you like to help me to set up the environment on Linux? I have installed and download the requirements on your project, but I can't run it. I attach the screenshot bellow.

I used CLion and Ubuntu 18.04 to run your code.

hello,friends. today, i compile this on Xavier which is NVIDIA's product, it is the ARM architecture different from the intel‘s x86_64. when i build it on Xavier, some errors occur!! like these:
c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’?
c++: error: unrecognized command line option ‘-msse2’
c++: error: unrecognized command line option ‘-msse3’
c++: error: unrecognized command line option ‘-msse4’
c++: error: unrecognized command line option ‘-msse4.1’
c++: error: unrecognized command line option ‘-msse4.2’
c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’?
c++: error: unrecognized command line option ‘-msse2’
c++: error: unrecognized command line option ‘-msse3’
c++: error: unrecognized command line option ‘-msse4’
c++: error: unrecognized command line option ‘-msse4.1’
c++: error: unrecognized command line option ‘-msse4.2’

In the CMakeLists.txt is "add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")".
which is OK on the intel‘s x86_64 architecture. Can you give me some help ? THANK YOU FIRST!!

sorry, already find it!

Hey! Thanks for the great work! However I am facing a problem, the globalmapserver nodlet fails with a Double free or corrupt (out) segmentation fault. I have tried running the given pcd file and my own pcd file but with no luck. I tried debugging the nodelet with Valgrind, and it seems to me that the node is not able to fetch data from the pcd file. What could be the cause of this error?

Thanks in advance!

Hi, @koide3
when I input that " rosparam set use_sim_time true", there is the error "ERROR: Unable to communicate with master!"?
what happened?

Hi Koide!
Are you managing the possibility that the point clouds frame_id could be different than the sensor frame_id? For example, in my case, I'm using the OS1 Ouster sensor and I have the os1_sensor frame with its two childs, namely, os1_lidar and os1_imu. As far as I see from the code, it seems that you are assuming the frame_id of the point clouds the same of the sensor. Indeed, your code computes the odometry from map to lidar_frame, instead, It should give the possibility to publish the transform from map to sensor_frame. The same holds for the IMU, it is assumed to be in the same frame of the point clouds. Is this right?

The idea would be to give the user the possibility to choose the frame_id against which to publish the odometry. Indeed, it is hardcoded in the hdl_localization_nodelet.cpp file that child_frame_id="velodyne"

/**
   * @brief publish odometry
   * @param stamp  timestamp
   * @param pose   odometry pose to be published
   */
  void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
    // broadcast the transform over tf
    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", "velodyne");
    pose_broadcaster.sendTransform(odom_trans);

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "map";

    odom.pose.pose.position.x = pose(0, 3);
    odom.pose.pose.position.y = pose(1, 3);
    odom.pose.pose.position.z = pose(2, 3);
    odom.pose.pose.orientation = odom_trans.transform.rotation;

    odom.child_frame_id = "velodyne";
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

Thank you for sharing your work, koide.

I wonder that is there any way to get error value between align points and map??

I mean, I want to get some message if localization is fail or not.

Hi,Thanks hdl_localization. Recently I am doing some work on navigation, I have a question about how to connect movebase with hdl_localization? please

Hi,
When I run it with the default launch file (hdl_localization.launch) and the hdl_400.bag, it seems to work for the first few frames, but it stopped moving after a while, seeming to have lost the track.
So in the video provided in the readme, did you use the original launch file, or have you modified some settings?
Thanks.

void predict(const VectorXt& control) {
// calculate sigma points
ensurePositiveFinite(cov);
computeSigmaPoints(mean, cov, sigma_points);
for (int i = 0; i < S; i++) {
sigma_points.row(i) = system.f(sigma_points.row(i), control);
}

const auto& R = process_noise;

// unscented transform
VectorXt mean_pred(mean.size());
MatrixXt cov_pred(cov.rows(), cov.cols());

mean_pred.setZero();
cov_pred.setZero();
for (int i = 0; i < S; i++) {
  mean_pred += weights[i] * sigma_points.row(i);
}
for (int i = 0; i < S; i++) {
  VectorXt diff = sigma_points.row(i).transpose() - mean;  
  cov_pred += weights[i] * diff * diff.transpose();
}
cov_pred += R;

mean = mean_pred;
cov = cov_pred;

}

**I do not know why not using mean_pred to calculate the cov_pred. **

It happens occasionally when doing localization while playing rosbag files, but sometimes it doesn't happen.
I wonder why...

Hi, thank you for your work!
The demo bag worked well on my computer. So I decided to test the algorithm on my own data. I built a map of a parking lot(150m*100m) and tested hdl_localization on it(the start is at the bottom right corner).

I changed the launch file to accommodate my Robosense-32 LiDAR:

<?xml version="1.0"?>
<launch>
  <!-- arguments -->
  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
  <arg name="points_topic" default="/middle/rslidar_points" />
  <arg name="imu_topic" default="/ox_imu/data" />
  <arg name="odom_child_frame_id" default="middle_lidar" />

  <!-- in case you use velodyne_driver, comment out the following line -->
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>

    <!-- globalmap_server_nodelet -->
    <node pkg="nodelet" type="nodelet" name="globalmap_server_nodelet" args="load hdl_localization/GlobalmapServerNodelet $(arg nodelet_manager)">
      <param name="globalmap_pcd" value="$(find hdl_localization)/data/map.pcd" />
      <param name="downsample_resolution" value="0.05" />
    </node>

    <!-- hdl_localization_nodelet -->
    <node pkg="nodelet" type="nodelet" name="hdl_localization_nodelet" args="load hdl_localization/HdlLocalizationNodelet $(arg nodelet_manager)">
        <remap from="/velodyne_points" to="$(arg points_topic)" />
        <remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
        <!-- odometry frame_id -->
        <param name="odom_child_frame_id" value="$(arg odom_child_frame_id)" />
        <!-- imu settings -->
        <!-- during "cool_time", imu inputs are ignored -->
        <param name="use_imu" value="true" />
        <param name="invert_imu" value="true" />
        <param name="cool_time_duration" value="2.0" />
        <!-- ndt settings -->
        <!-- if NDT is slow for your PC, try DIRECT1 serach method, which is a bit unstable but extremely fast -->
        <param name="ndt_neighbor_search_method" value="DIRECT7" />
        <param name="ndt_resolution" value="1.0" />
        <param name="downsample_resolution" value="0.1" />
        <!-- if "specify_init_pose" is true, pose estimator will be initialized with the following params -->
        <!-- otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" -->
        <param name="specify_init_pose" value="true" />
        <param name="init_pos_x" value="0.0" />
        <param name="init_pos_y" value="0.0" />
        <param name="init_pos_z" value="0.0" />
        <param name="init_ori_w" value="1.0" />
        <param name="init_ori_x" value="0.0" />
        <param name="init_ori_y" value="0.0" />
        <param name="init_ori_z" value="0.0" />
    </node>
</launch>

I used the same bag to build the map and to test the localization algorithm. For the first few frames the localization went on pretty well:

However, the localization fails after several steps, remaining at the same spot:

I noticed similar issues like #25 #30 and #31 and tried the solutions you gave. I tried different downsample rates of both the map and the pointcloud, different neighbor search methods, with and without IMU, but the problem still exists. Could you please give me some advice on this problem? Is it a problem of parameter tuning(too sparse map vs. too dense pointcloud) or is it specific to the scenario I'm using(lack of features)?
Also, when changing the downsample rates, I received warnings like:

[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

I'm planning to use the algorithm on a larger map. Does this constrain the map size I'm using?

Hi @koide3,

Is there a way for me to get the pose/trajectory from the hdl_localization_nodelet, similar the rosservice call from the hdl_graph_slam package?
I would like to plot them against the ground truth. Thank you in advance!

Same issue following url
koide3/ndt_omp#5

Hi Koide,
Can I ask you why you didn't fill the covariance matrix of the nav_msgs::Odometry odometry message? I see that your UKF class exposes a method called getCov(). Could I use this method to fill the covariance of the /odom topic?

Thank you.

Hi, and thank you for making this code available.
I am trying to write a version that doesn't use ROS, but I am having trouble finding a dataset to test it on.
Is the data you used available as pcap, xyz or anything other than a rosbag?
Thanks again.

Hi koide3,
my launch params,

<param name="ndt_neighbor_search_method" value="DIRECT7" />
<param name="ndt_resolution" value="6.0" />
  <param name="downsample_resolution" value="2.0" />

 the ndt align drift a little,how can I tune the ndt parameters？

rviz -d hdl_localizatrion.rviz
must be
rviz -d hdl_localization.rviz

roslaunch hdl_localization hdl_localization.launch
this is also wrong it must be hdl_localization/launch/hdl_localization.launch

Hi @koide3 or anyone

I faced an error when running the launch file, hdl_localization.launch with the given bag file, hdl_400.bag. The alligned pointcloud doesn't match the globalmap, and the odom is also wrong. Here i attached the picture:

I tried with both DIRECT7 and DIRECT1, unfortunately the situations are the same. Is there a way to make it to run like the pictures you show in the README.md?

Best,
Samuel

Hello,
When using this package on my own data, I get this error related to the velodyne noedelet manager :

ROS_MASTER_URI=http://localhost:11311

process[pcd_filter-1]: started with pid [6045]
process[velodyne_nodelet_manager-2]: started with pid [6046]
process[globalmap_server_nodelet-3]: started with pid [6047]
process[hdl_localization_nodelet-4]: started with pid [6048]
[ INFO] [1551999333.124715664]: Initializing nodelet with 8 worker threads.
[ INFO] [1551999349.304315088, 1544280386.764437095]: search_method DIRECT7 is selected
[ INFO] [1551999349.305892338, 1544280386.764437095]: enable imu-based prediction
[ INFO] [1551999349.318159343, 1544280386.764437095]: globalmap received!
[ INFO] [1551999354.942834302, 1544280386.764437095]: initial pose received!!
[velodyne_nodelet_manager-2] process has died [pid 6046, exit code -11, cmd /opt/ros/kinetic/lib/nodelet/nodelet manager __name:=velodyne_nodelet_manager __log:=/home/mahdi/.ros/log/19b150fa-412c-11e9-8462-74d435ebc48a/velodyne_nodelet_manager-2.log].
log file: /home/mahdi/.ros/log/19b150fa-412c-11e9-8462-74d435ebc48a/velodyne_nodelet_manager-2*.log
^C[hdl_localization_nodelet-4] killing on exit
[globalmap_server_nodelet-3] killing on exit
[pcd_filter-1] killing on exit
terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injectorboost::lock_error >'
what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument
^C^Cshutting down processing monitor...
... shutting down processing monitor complete
done

I tried commenting out the 7th line in the launch file but then the package isnt working anymore. Any suggestions? Thank you

hi, so can I make my robot navigate to another point autonomously?

I tried the data that auhor give. But the result is bad, the point cloud can't align well with the prior map. I don't understang why this could happen.

hi @koide3

thanks for your kind effort on this package, could you tell me what is the difference between this package and conventional ndt ?

Best
Welson

hello, koide,thanks for your hard work。I wander why you use ukf in this project，not ekf?

I build a 3D map using hdl_graph_slam and i am able to relocalize using the 3D map using hdl_localization module .

But the 3D map is tilted for some reason , what could be the reason for this ?

// predict
if(!use_imu) {
  pose_estimator->predict(stamp, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero());
} else {
  std::lock_guard<std::mutex> lock(imu_data_mutex);
  auto imu_iter = imu_data.begin();
  for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
    if(stamp < (*imu_iter)->header.stamp) {
      break;
    }
    const auto& acc = (*imu_iter)->linear_acceleration;
    const auto& gyro = (*imu_iter)->angular_velocity;
    double gyro_sign = invert_imu ? -1.0 : 1.0;
    pose_estimator->predict((*imu_iter)->header.stamp, Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z));
  }
  imu_data.erase(imu_data.begin(), imu_iter);
}

**why using the imu_iter on the stamp with observation not the pre_stamp **

In the hdl_graph_slam, I can change the topic and frame IDs (to run on data from other lidars) in the launch file. But in this project, I could not see interfaces about such IDs in the launch file.

So how to change the IDs?

On default, the invert_imu is enabled but the localization just failed when the car turns. Then I turned it off, and everything works perfectly fine. Could you give me a hint of what this invert_imu does?

Hi guys

i had found the yaw on certain position were drift quite alot...
while on other some other position seems to be okay.

im using the yaw from

double yaw_angle = tf::getYaw(odom_trans.transform.rotation);
in here hdl_localization_nodelet.cpp#L255

with some extra calculation
concerning yaw angle value were range from -3.1415... to 3.1415.. i assume it shoud be pi(22/7)
therefore:
yaw=yaw_angle/((22/7)/180)

so i could get the yaw from 0-180 degree for both positive and negative.

this were working fine for yaw 0,90,and 180..
and some other angle..

however on some circumatances... the yaw drift like up to 5 degree.

here some screen shoot

from current location, 9.15,4.009
to target location 7.13, 2.03 should be around -135 degree from x axis
however the yaw showing -137 degree when it standing on current locating facing to target location.
(see bottom right corner)

2nd sample:
this 2nd sample drift till 5 degree
(see bottom right corner)

could i calculate the yaw wrongly?
or im missing something?

Hi @koide3, anyone
i have little problem here with the alignment
aligned_points was unable to align with the map.

what im using was only velodyne vlp 16
and there is no change on the code part.

i did saw one silimar case, on closed case, however it was on hdl_400 rosbag
since im not using rosbag and its on my own,
i cant apply the solution about slowing down the playback speed.

here is the complete video
https://www.youtube.com/watch?v=DcGqOhGzQDU&t=3s