Is there demand of the precision of Transform? #5

xbcdbc · 2018-10-23T09:57:54Z

No description provided.

ZacharyTaylor · 2018-10-23T12:47:21Z

I have not benchmarked the approach against transformation noise, however there will be a strong correlation between the estimated transformation accuracy and calibration accuracy. In this style of calibration the translation estimate usually suffers much more from noise than the rotation estimate as it is less easily observed.

xbcdbc · 2018-11-01T03:13:20Z

I have get the point cloud through the VLP32-C and a sequence of 6-DOF poses throuht some sensor,when run this project, it seem that it does't get the right result.Maybe I does't use this project in right method. Can I send the data to you?

ZacharyTaylor · 2018-11-01T07:08:42Z

Sure if you send me a link to the data I will have a look and see if I can find the issue.

xbcdbc · 2018-11-01T08:42:19Z

https://note.youdao.com/share/?id=2a8091fb174ee22981966c15280a6091&type=note#/

ZacharyTaylor · 2018-11-01T13:03:41Z

I had a go at downloading from the provided link. But got the following error message, I don't speak Chinese but I believe it is telling me I can't download the file as it is over 150 MB

ZacharyTaylor · 2018-11-08T14:55:48Z

Did you get it working in the end?

xbcdbc · 2018-11-15T01:41:24Z

Did you get it working in the end?

I will try it again.Thank you.if has any questiones,i may ask you angin.

xbcdbc · 2018-11-16T07:47:07Z

Did you get it working in the end?
when i run the newset verison with the example.bag, i get the problem:
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
lidar_align_node: /build/pcl-6_P28C/pcl-1.7.2/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector&, std::vector&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
Aborted (core dumped)

ZacharyTaylor · 2018-11-16T11:45:56Z

From the error message it seems like either no points were loaded or they had non-finite values. I just added a check so that should prevent the system crashing see #13
However, this probably won't solve the issue for you completely. Could you either give an example bag or what rosbag info returns when run on your bag

ZacharyTaylor · 2018-11-16T11:51:35Z

I just realized you said you were running on the bag I gave as an example previously. I am a little unsure how to debug now, as I just ran it on my system with the new version and it exited successfully with the following output

roslaunch lidar_align lidar_align.launch 
... logging to /home/z/.ros/log/355aa218-e978-11e8-a4e5-54ee758049cd/roslaunch-z-work-14701.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/z/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://z-work:35003/

SUMMARY
========

PARAMETERS
 * /lidar_align/input_bag_path: /home/z/Downloads...
 * /lidar_align/input_csv_path: PATH/TO/YOUR.csv
 * /lidar_align/output_calibration_path: /home/z/catkin_ws...
 * /lidar_align/output_pointcloud_path: /home/z/catkin_ws...
 * /lidar_align/transforms_from_csv: False
 * /rosdistro: melodic
 * /rosversion: 1.14.3

NODES
  /
    lidar_align (lidar_align/lidar_align_node)

ROS_MASTER_URI=http://localhost:11311

process[lidar_align-1]: started with pid [14717]
[ INFO] [1542368854.414473070]: Loading Pointcloud Data...
[ INFO] [1542368871.330311883]: Loading Transformation Data...                                
[ INFO] [1542368887.379280807]: Interpolating Transformation Data...                          
[ INFO] [1542368887.397304336]: Performing Global Optimization...                             
[ INFO] [1542368916.819175220]: Performing Local Optimization...                                
[ INFO] [1542368931.642848630]: Saving Aligned Pointcloud...                                     eration: 293
[ INFO] [1542368931.663708414]: Saving Calibration File...                                
[ INFO] [1542368931.663835698]: Final Calibration:                                
Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to  the Lidar Frame:
[-0.00267923, -0.0373245, -0.129704, 3.10134, 0.162723, -0.0620562]

Active Transformation Matrix from the Pose Sensor Frame to  the Lidar Frame:
   0.993715    0.105281  -0.0380283 -0.00267923
   0.103868   -0.993888  -0.0373962  -0.0373245
  -0.041733   0.0332112   -0.998577   -0.129704
          0           0           0           1

Active Translation Vector (x,y,z) from the Pose Sensor Frame to  the Lidar Frame:
[-0.00267923, -0.0373245, -0.129704]

Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to  the Lidar Frame:
[0.0176824, 0.998271, 0.0523779, -0.0199749]

Time offset that must be added to lidar timestamps in seconds:
0.000593067

ROS Static TF Publisher: <node pkg="tf" type="static_transform_publisher" name="pose_lidar_broadcaster" args="-0.00267923 -0.0373245 -0.129704 0.998271 0.0523779 -0.0199749 0.0176824 POSE_FRAME LIDAR_FRAME 100" />
[lidar_align-1] process has finished cleanly
log file: /home/z/.ros/log/355aa218-e978-11e8-a4e5-54ee758049cd/lidar_align-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Could you give a similar full roslaunch output?

xbcdbc · 2018-11-16T13:15:30Z

I get this result. The bag is example.bag.

pw@pw-cowa:~/lidar-align$ roslaunch lidar_align lidar_align.launch
... logging to /home/pw/.ros/log/fc66950c-e990-11e8-a0f9-40167eb5004d/roslaunch-pw-cowa-744.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://pw-cowa:43353/

SUMMARY

PARAMETERS

/lidar_align/input_bag_path: /home/pw/lidar-al...
/lidar_align/input_csv_path: PATH/TO/YOUR.csv
/lidar_align/output_calibration_path: /home/pw/lidar-al...
/lidar_align/output_pointcloud_path: /home/pw/lidar-al...
/lidar_align/transforms_from_csv: False
/rosdistro: kinetic
/rosversion: 1.12.14

NODES
/
lidar_align (lidar_align/lidar_align_node)

ROS_MASTER_URI=http://localhost:11311

process[lidar_align-1]: started with pid [761]
[ INFO] [1542373979.952676919]: Loading Pointcloud Data...
[ INFO] [1542373994.645136596]: Loading Transformation Data...
[ INFO] [1542374008.697634049]: Interpolating Transformation Data...
[ INFO] [1542374008.716612338]: Performing Global Optimization...
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
lidar_align_node: /build/pcl-6_P28C/pcl-1.7.2/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector&, std::vector&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
[lidar_align-1] process has died [pid 761, exit code -6, cmd /home/pw/lidar-align/devel/lib/lidar_align/lidar_align_node __name:=lidar_align __log:=/home/pw/.ros/log/fc66950c-e990-11e8-a0f9-40167eb5004d/lidar_align-1.log].
log file: /home/pw/.ros/log/fc66950c-e990-11e8-a0f9-40167eb5004d/lidar_align-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

xbcdbc · 2018-11-16T13:17:19Z

And when I run the version of 77cc771 ,the problem is disappear.....

ZacharyTaylor · 2018-11-16T13:49:38Z

Hmm, a lot of changes have happened since that PR, but none that impact the point cloud processing. I will try get some time to look at what it could be tomorrow.

xbcdbc · 2018-11-18T05:01:51Z

Hmm, a lot of changes have happened since that PR, but none that impact the point cloud processing. I will try get some time to look at what it could be tomorrow.

I have commit a bag file to the github,can you help me to look at and give me a result,thank you.
https://github.com/xbcdbc/lidar_6DOF.git

ZacharyTaylor · 2018-11-18T10:11:42Z

Tried the bag and it ran without issue on my pc, maybe try catkin clean if it isn't running on yours. The launchfile I used was

<?xml version="1.0" encoding="UTF-8"?>
<launch>

  <arg name="bag_file" default="/home/z/Downloads/carli_2.bag"/>

  <node pkg="lidar_align" name="lidar_align" type="lidar_align_node" output="screen">
    <param name="input_bag_path" value="$(arg bag_file)" />
    <param name="max_time_offset" value="1.0"/>
    <param name="keep_points_ratio" value="0.05"/>
    <param name="estimate_point_times" value="true"/>
    <param name="output_pointcloud_path" value="$(find lidar_align)/results/$(anon lidar_points).ply" />
    <param name="output_calibration_path" value="$(find lidar_align)/results/$(anon calibration).txt" />
  </node>

</launch>

I turned the keep_points_ratio up a bit as there aren't too many lidar messages so it runs pretty quickly.

One big thing to note is this is a motion based calibration technique and so to get a 6-dof transform out you need to give a 6-dof transform in. In your bag the system is largely undergoing planar motion and the roll and pitch values of your transform are always zero, so it is really only a 3-dof input. This makes the transformation between the sensors unobservable for some parameters and so the optimizer finds a random minima just based on noise. Because of this I think only the yaw value and maybe the x and y are usable from the output.

One other point of concern is I had to ramp the max_time_offset parameter way up as the optimizer is coming up with a time offset between your sensors of almost half a second, how are you timestamping your data?

The final calibration parameters I got are:

Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to  the Lidar Frame:
[0.0683994, 0.482944, 0.0350142, -0.00928212, 0.00187862, -3.11278]

Active Transformation Matrix from the Pose Sensor Frame to  the Lidar Frame:
  -0.999567   0.0287948  0.00597997   0.0683994
  -0.028802   -0.999584  -0.0011209    0.482944
 0.00594521 -0.00129266    0.999981   0.0350142
          0           0           0           1

Active Translation Vector (x,y,z) from the Pose Sensor Frame to  the Lidar Frame:
[0.0683994, 0.482944, 0.0350142]

Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to  the Lidar Frame:
[0.0144008, -0.00298162, 0.000603455, -0.999892]

Time offset that must be added to lidar timestamps in seconds:
0.454408

ROS Static TF Publisher: <node pkg="tf" type="static_transform_publisher" name="pose_lidar_broadcaster" args="0.0683994 0.482944 0.0350142 -0.00298162 0.000603455 -0.999892 0.0144008 POSE_FRAME LIDAR_FRAME 100" />

Image of fused cloud:

xbcdbc · 2018-11-19T06:33:38Z

I try two ways "catkin build" and ""catkin_make" to compile the packagem,then roslaunch the .launch file, they all result with "[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
".

xbcdbc · 2018-11-19T06:50:12Z

My ros-version is "kinect",yours is melodic, does it matter?

ZacharyTaylor · 2018-11-19T09:01:09Z

Tried it on a kinetic version and turns out that was the difference. There was a divided by zero error and either pcl or Eigen had changed how they handled it in melodic so I didn't see it. Bug has been fixed in #14 thanks for the help in tracking it down.

xbcdbc · 2018-11-19T09:44:45Z

Tried it on a kinetic version and turns out that was the difference. There was a divided by zero error and either pcl or Eigen had changed how they handled it in melodic so I didn't see it. Bug has been fixed in #14 thanks for the help in tracking it down.

Thank you very much.

xbcdbc · 2018-11-20T06:25:55Z

if i get a realtive accurate translation x,y,z, and give a mininum range，does the result can be better?

ZacharyTaylor · 2018-11-20T08:15:10Z

The main issue is you are giving a 3-dof transform. To get accurate results you need to give a full 6-dof pose taken while the system undergoes highly non-planar motion. High accuracy also requires accurate time-stamping.

trn84 · 2018-11-21T21:15:54Z

Hi Zachary,

I have also tried your optimizer and was wondering how much movement is required in all directions. As some reference I was using a bag file that I have created from the KITTI dataset and used the output of the OxTs robot_localization as the odom source (world -> base_link). However, the results are not consistent with the transform from the dataset calibration (tf: base_link -> velo_link). Do you think moving in a car is suitable for a proper calibration or is it merely 3-DoF?

Thanks anyways for the work.

ZacharyTaylor · 2018-11-21T21:38:57Z

No translational motion is needed, however large changes in roll, pitch and yaw must be provided or the calculated translation will have large errors. Any system that moves exclusively along a planar surface cannot be accurately calibrated by this method.

trn84 · 2018-11-22T08:01:06Z

Hi Zach,

Thanks for the quick response. Okay this seems reasonable. However, if a merely do a local optimization using the "correct" transformation from the tf as the initial guess the results should be stable around this initial guess, right? Maybe I am doing something wrong? Is the rotation of the initial guess provided in radians or degree?

ZacharyTaylor · 2018-11-22T08:32:54Z

The cost function will always change by a small amount for different inputs due to the limited accuracy of floating point numbers. Because of this if an element is unobservable I think it will be pretty random where the local optimization ends up.

The rotational part of the initial guess is generated by taking the rotation in angle-axis form and multiplying the normalized axis by the angle in radians. This is the angle representation used internally by the optimizer.

miracle629 · 2018-11-28T14:56:04Z

Hi Zachary,
I tried lidar_align in melodic, but got the same error with xbcdbc, can't build a kdtree with no pointcloud, my PCL version is 1.8.1,flann version is 1.9.1, could you tell me your PCL and flann version. I googled and found some people think there is a bug in PCL 1.7.1 about kdtree search, but is fixed in 1.8.0, the link is here http://www.pcl-users.org/Kdtree-Radius-Search-Debug-Assertion-Failed-td4025503.html. In kinetic, when I use apt-get install pcl, the version is 1.7.1, while in melodic, the version is 1.8.1, but it failed again. I am troubled with the problem. Please tell me the version and I will try again. Thanks.

ZacharyTaylor · 2018-11-28T15:44:21Z

@miracle629 I think its likely that some nans are still getting into the optimization and causing this. I will have a look if I can work out how. Which dataset are you using it on?

trn84 · 2018-11-29T08:37:43Z

Dear Zach,
do you have a test dataset where I can inspect in which order of magnitude and length/size the recording needs to be for a proper calibration? It would be helpful to check if I am using the tool correctly.

Cheers

ZacharyTaylor · 2018-11-29T09:31:39Z

You can download a dataset I tested things on at https://drive.google.com/open?id=11fUwbVnvej4NZ_0Mntk7XJ2YrZ5Dk3Ub however I think it is probably far from an optimal example as its just me holding the lidar and randomly waving it around. Rovio was used to get the transforms from a VI sensor I had taped to the lidars base.

miracle629 · 2018-12-02T09:34:54Z

Hi Zachary,
I download your example and run it on my PC, it works well in both kinetic and melodic, it seems the problem is our data, we use our own data, actually we use csv to load odom, I compare your odom data and ours and find our position x,y,z are much bigger than yours, in the range of 300-400, and I guess it is about 1000 times of your data, we use 'meter' as the measurement uint, maybe the difference is 'meter' and 'kilometer? I will try it again after a uint conversion. Thanks for your example data.

ZacharyTaylor · 2018-12-02T10:00:07Z

The output will just take the same units as the input. The only constraint is that the input pointclouds and pose are measured in the same units.

It is possible that under extreme conditions the floating point representation will fail to correctly transform the data but I don't believe this would be happening for this magnitude of input.

The error occurs if none of the points in the pointclouds are transformed to finite coordinates. I check for and remove Nans and infs on the input so the transformation class must have an issue with this data.

ZacharyTaylor · 2018-12-02T10:01:59Z

Either that or the transformations are not being read correctly and so are all invalid, I just realized I don't do many checks that the loaded poses are valid

ghost · 2019-05-29T07:43:48Z

Either that or the transformations are not being read correctly and so are all invalid, I just realized I don't do many checks that the loaded poses are valid

hi, I want to use this tool, too. My IMU&VLP16 can both work , but how can I make a bag with odom data.
If you can see this please teach me hhh. Thanks very much!

debuleilei · 2019-09-26T09:14:12Z

@ZacharyTaylor ,hello ,author, I'm not sure whether you can see my question,but I hope so.
I have tried your code with sufer good result ,but when I want to use it on the ubuntu 14.04 platform ,I met a bug during the optimize process, I found that it break at this point :

I think maybe the difference comes from the platform that make the threads unwork ,if you have some advice ,please give a help,thanks!

zhu-bingo · 2020-05-10T12:52:59Z

I get this result. The bag is example.bag.

pw@pw-cowa:~/lidar-align$ roslaunch lidar_align lidar_align.launch
... logging to /home/pw/.ros/log/fc66950c-e990-11e8-a0f9-40167eb5004d/roslaunch-pw-cowa-744.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://pw-cowa:43353/

SUMMARY

PARAMETERS

/lidar_align/input_bag_path: /home/pw/lidar-al...

/lidar_align/input_csv_path: PATH/TO/YOUR.csv

/lidar_align/output_calibration_path: /home/pw/lidar-al...

/lidar_align/output_pointcloud_path: /home/pw/lidar-al...

/lidar_align/transforms_from_csv: False

/rosdistro: kinetic

/rosversion: 1.12.14

NODES
/
lidar_align (lidar_align/lidar_align_node)

ROS_MASTER_URI=http://localhost:11311

process[lidar_align-1]: started with pid [761]
[ INFO] [1542373979.952676919]: Loading Pointcloud Data...
[ INFO] [1542373994.645136596]: Loading Transformation Data...
[ INFO] [1542374008.697634049]: Interpolating Transformation Data...
[ INFO] [1542374008.716612338]: Performing Global Optimization...
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
lidar_align_node: /build/pcl-6_P28C/pcl-1.7.2/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector&, std::vector&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
[lidar_align-1] process has died [pid 761, exit code -6, cmd /home/pw/lidar-align/devel/lib/lidar_align/lidar_align_node __name:=lidar_align __log:=/home/pw/.ros/log/fc66950c-e990-11e8-a0f9-40167eb5004d/lidar_align-1.log].
log file: /home/pw/.ros/log/fc66950c-e990-11e8-a0f9-40167eb5004d/lidar_align-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Did you get it working in the end?
when i run the newset verison with the example.bag, i get the problem:
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
lidar_align_node: /build/pcl-6_P28C/pcl-1.7.2/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector&, std::vector&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
Aborted (core dumped)

请问这个问题解决了吗

Chrislzy1993 · 2021-03-24T02:55:04Z

@ZacharyTaylor ，hello， can you privide me a dataset for test ???

ZacharyTaylor mentioned this issue Nov 19, 2018

solve div by zero error #14

Merged

LarryDong mentioned this issue Sep 14, 2020

performing global optimization failed #26

Open

LarryDong mentioned this issue Apr 15, 2021

Confusion about Odom data #33

Open

Is there demand of the precision of Transform? #5

Is there demand of the precision of Transform? #5

Comments

xbcdbc commented Oct 23, 2018

ZacharyTaylor commented Oct 23, 2018

xbcdbc commented Nov 1, 2018

ZacharyTaylor commented Nov 1, 2018

xbcdbc commented Nov 1, 2018

ZacharyTaylor commented Nov 1, 2018

ZacharyTaylor commented Nov 8, 2018

xbcdbc commented Nov 15, 2018

xbcdbc commented Nov 16, 2018

ZacharyTaylor commented Nov 16, 2018

ZacharyTaylor commented Nov 16, 2018

xbcdbc commented Nov 16, 2018

SUMMARY

xbcdbc commented Nov 16, 2018

ZacharyTaylor commented Nov 16, 2018

xbcdbc commented Nov 18, 2018

ZacharyTaylor commented Nov 18, 2018

xbcdbc commented Nov 19, 2018

xbcdbc commented Nov 19, 2018

ZacharyTaylor commented Nov 19, 2018

xbcdbc commented Nov 19, 2018

xbcdbc commented Nov 20, 2018

ZacharyTaylor commented Nov 20, 2018

trn84 commented Nov 21, 2018

ZacharyTaylor commented Nov 21, 2018

trn84 commented Nov 22, 2018

ZacharyTaylor commented Nov 22, 2018

miracle629 commented Nov 28, 2018

ZacharyTaylor commented Nov 28, 2018

trn84 commented Nov 29, 2018

ZacharyTaylor commented Nov 29, 2018

miracle629 commented Dec 2, 2018

ZacharyTaylor commented Dec 2, 2018

ZacharyTaylor commented Dec 2, 2018

ghost commented May 29, 2019

debuleilei commented Sep 26, 2019

zhu-bingo commented May 10, 2020

SUMMARY

Chrislzy1993 commented Mar 24, 2021