megviirobot / odomlasercalibratool

Extrinsic Calibration of a Odom and 2d Laser

laser-odom_calibration

1. Prerequisites

1. ubuntu 16.04 + ROS Kinetic

2. Eigen3

2. Usage

    cd catkin_ws/src
    git clone https://github.com/MegviiRobot/OdomLaserCalibraTool
    cd ..
    catkin_make
    source devel/setup.bash
    roslaunch example.launch

INPUT:

A rosbag includes laser scan and odometer data(left & right wheels' velocity). Please check the topic name with launch file.

*Note that we use rostopic type geometry_msgs::Vector3Stamped to record odometer's raw data, for other types please modify the code.

OUTPUT:

Extrinsic parameters between LiDAR and odomter(x, y, yaw), intrinsic paramters of odomter(left & right wheels' radius, track between two wheels) and related uncertainty analysis.

e.g.

3. Authors

1. Zhijie WANG, Research Intern at Megvii, will graduate from Beijing University of Posts & Telecommunications with a bachelor degree in July 2019, contact with (paul dot wangzhijie at outlook dot com).

2. Yijia HE, you can find the chinese version in his homepage, if you have any question, please contact (heyijia_2013 at 163 dot com)

3. Xiao Liu, his homepage

4. References

Censi, Andrea, Franchi, Antonio, Marchionni, Luca & Oriolo, Giuseppe (2013). Simultaneous calibration of odometry and sensor parameters for mobile robots. IEEE Transactions on Robotics, 29, 475-492.

odomlasercalibratool's Issues

Suggestions

Thanks for your excellent work.

I have two suggestions for improving this package:

1. To facilitate the followers, the developers are encouraged to provide an example rosbag.

2. Please explain more about the statement in your blog, "注意：代码可以标定车轮半径等内参数，但是需要你提供左右轮的速度，实际上左右轮速度都知道了，就不需要标定半径了。因此，我们对需求进行了改动，需要你提供 ros odom 数据格式（机器人前进速度和角速度），代码中会假定一个车轮半径和轮间距，从而给你算出一个码盘内参数。"

If possible, could you please indicate what every element in the required 'geometry_msgs::Vector3Stamped' (odometer) represents?
Or, the meaning of the variables 'tmp.v_l' and 'tmp.v_r' (io.cpp, line76)?

Matrix A is almost singular.

I modify code to subscribe odometry message and calcibra with my robsbag (static at the first 30s
) : nav_msgs::OdometryConstPtr odom = m.instantiate<nav_msgs::Odometry>();

but meet this Errors as follow, How to improve it. Could you send me a test rosbag huasheng_zyh@163 dot com

[1584078565.403185]: Opening bag...
[1584078565.414886]: DONE!
[1584078565.414898]: Quering topics bag...
[1584078565.592179]: Data reading finished!
[1584078565.599994]: Hi!
[1584078565.600031]: laser size: 1594
[1584078565.600025]: odom size: 2106
[1584078565.600063]: Begin CSM!
[1584078565.600076]: Converting scan to csm type...
[1584078565.716041]: Converting finished!
[1584078565.716054]: LDP numbers: 1594
[1584078565.716058]: Canonical scan matching...

Matching progress: 99%

[1584078582.795411]: Invaild matches: 0
[1584078582.795415]: Vaild rate: 1
[1584078582.795429]: Matching finished!
[1584078582.795431]: Writing results...
[1584078582.834387]: Saving results...
[1584078582.834422]: Results saved!
[1584078582.834425]: Start synchronzing...

Synchronzing progress: 99%

[1584078582.886559]: Sync finished!
[1584078582.889306]: Sync results saved!
[1584078582.891421]: Matrix A is almost singular.
[1584078582.891428]: Failed calibration.
[1584078582.893521]: Matrix A is almost singular.
[1584078582.893527]: Failed calibration.
[1584078582.895626]: Matrix A is almost singular.
[1584078582.895632]: Failed calibration.
[1584078582.897725]: Matrix A is almost singular.
[1584078582.897731]: Failed calibration.
[1584078582.899834]: Matrix A is almost singular.
[1584078582.899843]: Failed calibration.

Suitable for wheel odom and 3D laser(velodyne)?

Hi doctor he,
Thanks for your work !
We can use this project to calibrate the extrinsic between a wheel odom and a velodyne-16, or there is some reference paper about this? Thanks very much !
Best

Is there a way to determine the value of z between odom and lidar?

The output gives x,y, and yaw. Is there a way to determine z between the lidar and odom

Confusion about x computation

Why we use ZZ instead of Z to compute x given lamda?
https://github.com/MegviiRobot/OdomLaserCalibraTool/blob/5de1ec7a329c27c7e0162eecf922d6c07dfdda86/src/solver.cpp#L345C3-L345C3

请问这里的速度单位是什么？ 轮子的线速度？

sensor_msgs::JointStateConstPtr odom = m.instantiate<sensor_msgs::JointState>();
if (odom != NULL) {
odometerData tmp;
tmp.timestamp = odom->header.stamp;
tmp.v_l = odom->velocity[0];
tmp.v_r = odom->velocity[1];
odom_data.push_back(tmp);
}

Failed Calibration

Hi, I used my rosbag to calibrate odometry(radii and distance between the wheels), I use nav_msgs::Odometry, but it failed(Matrix A is almost singular, failed calibration). Should I use geometry_msgs::Vector3Stamped if I want to calibrate odometry? or could you please send me example bag, my email: [email protected]. Thanks.

Best.

What information of topic 'odom' is actullay utilized?

I note that the code in io.cpp#L72 reads the position information of the robot. But in Readme, the veloctiy of left and right wheels is required. So I confuse that what information is input indeed?

The wheel radius in the calibration parameters is negative

Hi, I encountered the following problems while using the program
The absolute value of the calibration result is basically consistent with the actual parameters of the robot, but the wheel radius and laser-odom x are negative.
The attachment has data and a more detailed problem description.

datas_and_questions.zip

How to infer Matrix M ? why m12=0 m23 =0 m45 = 0?

As paper Simultaneous Calibration of Odometry and Sensor Parameters for Mobile Robots

why m12=0 m23 =0 m45 = 0? How to infer Matrix M ?

内参标定不准确

你好，我在使用该程序进行测试时，外参（雷达到机器人中心的tf变化）标定挺好，但是输出结果中的内参（轮子半径和轮间距）就误差很大，能解释一下为啥吗？谢谢

计算雷达里程计 部分的代码是不是存在错误？

感谢开源这么棒的工作！

想问下 计算雷达里程计 部分的代码是不是存在错误？

OdomLaserCalibraTool/src/scan_match.cpp

Lines 139 to 140 in 5de1ec7

	last_odom.x = results.x[0] + scan_odom.back().x * cos(results.x[2]) - scan_odom.back().y * sin(results.x[2]);
	last_odom.y = results.x[1] + scan_odom.back().x * sin(results.x[2]) + scan_odom.back().y * cos(results.x[2]);

result 中应该保存的是相对 位置变化，应该对应与 注释中的 {b_x, b_y_, b_theta} 部分吧？

exist an error in function “void cSolver::compute_disagreement()”

void cSolver::compute_disagreement(cSynchronizer::sync_data &calib_data, const calib_result &res)
{
.........
if (fabs(o_theta) > 1e-12)
{
t1 = sin(o_theta) / o_theta;
t2 = (1 - cos(o_theta)) / o_theta;
}
else
{
t1 = 0;
t2 = 0;
}
.......
}

when "fabs(o_theta) < 1e-12", the t1 should equal to one

Matrix A is almost singular.

Dear Author,
I'm sorry to bother you. I noted that someone had proposed this question,I checked and there is no problem as you said.
I increase the max_cond_number to 300 and the problem still occurs.
I have attached the bag file I used on google drive:
https://drive.google.com/drive/folders/1TTCReR23v-2bXWYk_9_Ra9HQkNhUyOPX?usp=sharing
Can you take some time to help me check where the problem is. Thanks a lot!
I also want to know what does the variable “first_odom" in scan_match.cpp mean. Do I need change the initial value?

Best Wishes!

get none results

Hi I record rosbag and run the code, but always get unvalied results, like these:
[1633353814.615495]: [0;33mOpening bag...[0m
[1633353814.621704]: [0;32mDONE![0m
[1633353814.621714]: [0;33mQuering topics bag...[0m
[1633353814.630232]: [0;32mData reading finished![0m
[1633353814.631262]: [0;37mHi![0m
[1633353814.631271]: [0;37mlaser size: [0m1208
[1633353814.631275]: [0;37modom size: [0m0
[1633353814.631278]: [0;32mBegin CSM![0m
[1633353814.631281]: [0;33mConverting scan to csm type...[0m
[1633353814.740251]: [0;32mConverting finished![0m
[1633353814.740272]: [0;37mLDP numbers: [0m1208
[1633353814.740276]: [0;33mCanonical scan matching...[0m

Matching progress: 0% 0% 0% 0% 0% 0% 0% 0% 0% 0% 0% 0% 1% 1% 1% 1% 1% 1% 1% 1% 1% 1% 1% 1% 2% 2% 2% 2% 2% 2% 2% 2% 2% 2% 2% 2% 3% 3% 3% 3% 3% 3% 3% 3% 3% 3% 3% 3% 4% 4% 4% 4% 4% 4% 4% 4% 4% 4% 4% 4% 5% 5% 5% 5% 5% 5% 5% 5% 5% 5% 5% 5% 6% 6% 6% 6% 6% 6% 6% 6% 6% 6% 6% 6% 7% 7% 7% 7% 7% 7% 7% 7% 7% 7% 7% 7% 8% 8% 8% 8% 8% 8% 8% 8% 8% 8% 8% 8% 9% 9% 9% 9% 9% 9% 9% 9% 9% 9% 9% 9% 10% 10% 10% 10% 10% 10% 10% 10% 10% 10% 10% 10% 11% 11% 11% 11% 11% 11% 11% 11% 11% 11% 11% 11% 12% 12% 12% 12% 12% 12% 12% 12% 12% 12% 12% 12% 12% 13% 13% 13% 13% 13% 13% 13% 13% 13% 13% 13% 13% 14% 14% 14% 14% 14% 14% 14% 14% 14% 14% 14% 14% 15% 15% 15% 15% 15% 15% 15% 15% 15% 15% 15% 15% 16% 16% 16% 16% 16% 16% 16% 16% 16% 16% 16% 16% 17% 17% 17% 17% 17% 17% 17% 17% 17% 17% 17% 17% 18% 18% 18% 18% 18% 18% 18% 18% 18% 18% 18% 18% 19% 19% 19% 19% 19% 19% 19% 19% 19% 19% 19% 19% 20% 20% 20% 20% 20% 20% 20% 20% 20% 20% 20% 20% 21% 21% 21% 21% 21% 21% 21% 21% 21% 21% 21% 21% 22% 22% 22% 22% 22% 22% 22% 22% 22% 22% 22% 22% 23% 23% 23% 23% 23% 23% 23% 23% 23% 23% 23% 23% 24% 24% 24% 24% 24% 24% 24% 24% 24% 24% 24% 24% 25% 25% 25% 25% 25% 25% 25% 25% 25% 25% 25% 25% 25% 26% 26% 26% 26% 26% 26% 26% 26% 26% 26% 26% 26% 27% 27% 27% 27% 27% 27% 27% 27% 27% 27% 27% 27% 28% 28% 28% 28% 28% 28% 28% 28% 28% 28% 28% 28% 29% 29% 29% 29% 29% 29% 29% 29% 29% 29% 29% 29% 30% 30% 30% 30% 30% 30% 30% 30% 30% 30% 30% 30% 31% 31% 31% 31% 31% 31% 31% 31% 31% 31% 31% 31% 32% 32% 32% 32% 32% 32% 32% 32% 32% 32% 32% 32% 33% 33% 33% 33% 33% 33% 33% 33% 33% 33% 33% 33% 34% 34% 34% 34% 34% 34% 34% 34% 34% 34% 34% 34% 35% 35% 35% 35% 35% 35% 35% 35% 35% 35% 35% 35% 36% 36% 36% 36% 36% 36% 36% 36% 36% 36% 36% 36% 37% 37% 37% 37% 37% 37% 37% 37% 37% 37% 37% 37% 37% 38% 38% 38% 38% 38% 38% 38% 38% 38% 38% 38% 38% 39% 39% 39% 39% 39% 39% 39% 39% 39% 39% 39% 39% 40% 40% 40% 40% 40% 40% 40% 40% 40% 40% 40% 40% 41% 41% 41% 41% 41% 41% 41% 41% 41% 41% 41% 41% 42% 42% 42% 42% 42% 42% 42% 42% 42% 42% 42% 42% 43% 43% 43% 43% 43% 43% 43% 43% 43% 43% 43% 43% 44% 44% 44% 44% 44% 44% 44% 44% 44% 44% 44% 44% 45% 45% 45% 45% 45% 45% 45% 45% 45% 45% 45% 45% 46% 46% 46% 46% 46% 46% 46% 46% 46% 46% 46% 46% 47% 47% 47% 47% 47% 47% 47% 47% 47% 47% 47% 47% 48% 48% 48% 48% 48% 48% 48% 48% 48% 48% 48% 48% 49% 49% 49% 49% 49% 49% 49% 49% 49% 49% 49% 49% 50% 50% 50% 50% 50% 50% 50% 50% 50% 50% 50% 50% 50% 51% 51% 51% 51% 51% 51% 51% 51% 51% 51% 51% 51% 52% 52% 52% 52% 52% 52% 52% 52% 52% 52% 52% 52% 53% 53% 53% 53% 53% 53% 53% 53% 53% 53% 53% 53% 54% 54% 54% 54% 54% 54% 54% 54% 54% 54% 54% 54% 55% 55% 55% 55% 55% 55% 55% 55% 55% 55% 55% 55% 56% 56% 56% 56% 56% 56% 56% 56% 56% 56% 56% 56% 57% 57% 57% 57% 57% 57% 57% 57% 57% 57% 57% 57% 58% 58% 58% 58% 58% 58% 58% 58% 58% 58% 58% 58% 59% 59% 59% 59% 59% 59% 59% 59% 59% 59% 59% 59% 60% 60% 60% 60% 60% 60% 60% 60% 60% 60% 60% 60% 61% 61% 61% 61% 61% 61% 61% 61% 61% 61% 61% 61% 62% 62% 62% 62% 62% 62% 62% 62% 62% 62% 62% 62% 62% 63% 63% 63% 63% 63% 63% 63% 63% 63% 63% 63% 63% 64% 64% 64% 64% 64% 64% 64% 64% 64% 64% 64% 64% 65% 65% 65% 65% 65% 65% 65% 65% 65% 65% 65% 65% 66% 66% 66% 66% 66% 66% 66% 66% 66% 66% 66% 66% 67% 67% 67% 67% 67% 67% 67% 67% 67% 67% 67% 67% 68% 68% 68% 68% 68% 68% 68% 68% 68% 68% 68% 68% 69% 69% 69% 69% 69% 69% 69% 69% 69% 69% 69% 69% 70% 70% 70% 70% 70% 70% 70% 70% 70% 70% 70% 70% 71% 71% 71% 71% 71% 71% 71% 71% 71% 71% 71% 71% 72% 72% 72% 72% 72% 72% 72% 72% 72% 72% 72% 72% 73% 73% 73% 73% 73% 73% 73% 73% 73% 73% 73% 73% 74% 74% 74% 74% 74% 74% 74% 74% 74% 74% 74% 74% 75% 75% 75% 75% 75% 75% 75% 75% 75% 75% 75% 75% 75% 76% 76% 76% 76% 76% 76% 76% 76% 76% 76% 76% 76% 77% 77% 77% 77% 77% 77% 77% 77% 77% 77% 77% 77% 78% 78% 78% 78% 78% 78% 78% 78% 78% 78% 78% 78% 79% 79% 79% 79% 79% 79% 79% 79% 79% 79% 79% 79% 80% 80% 80% 80% 80% 80% 80% 80% 80% 80% 80% 80% 81% 81% 81% 81% 81% 81% 81% 81% 81% 81% 81% 81% 82% 82% 82% 82% 82% 82% 82% 82% 82% 82% 82% 82% 83% 83% 83% 83% 83% 83% 83% 83% 83% 83% 83% 83% 84% 84% 84% 84% 84% 84% 84% 84% 84% 84% 84% 84% 85% 85% 85% 85% 85% 85% 85% 85% 85% 85% 85% 85% 86% 86% 86% 86% 86% 86% 86% 86% 86% 86% 86% 86% 87% 87% 87% 87% 87% 87% 87% 87% 87% 87% 87% 87% 87% 88% 88% 88% 88% 88% 88% 88% 88% 88% 88% 88% 88% 89% 89% 89% 89% 89% 89% 89% 89% 89% 89% 89% 89% 90% 90% 90% 90% 90% 90% 90% 90% 90% 90% 90% 90% 91% 91% 91% 91% 91% 91% 91% 91% 91% 91% 91% 91% 92% 92% 92% 92% 92% 92% 92% 92% 92% 92% 92% 92% 93% 93% 93% 93% 93% 93% 93% 93% 93% 93% 93% 93% 94% 94% 94% 94% 94% 94% 94% 94% 94% 94% 94% 94% 95% 95% 95% 95% 95% 95% 95% 95% 95% 95% 95% 95% 96% 96% 96% 96% 96% 96% 96% 96% 96% 96% 96% 96% 97% 97% 97% 97% 97% 97% 97% 97% 97% 97% 97% 97% 98% 98% 98% 98% 98% 98% 98% 98% 98% 98% 98% 98% 99% 99% 99% 99% 99% 99% 99% 99% 99% 99% 99% 99%

[1633353819.347997]: [0;37mInvaild matches: [0m0
[1633353819.348003]: [0;37mVaild rate: [0m1
[1633353819.348017]: [0;32mMatching finished![0m
[1633353819.348021]: [0;33mWriting results...[0m
[1633353819.364523]: [0;33mSaving results...[0m
[1633353819.364603]: [0;32mResults saved![0m
[1633353819.364609]: [0;33mStart synchronzing...[0m

Synchronzing progress:

[1633353819.364631]: [0;32mSync finished![0m
[1633353819.364647]: [0;32mSync results saved![0m
J: -nan
-nan
[1633353819.364661]: [1;31mCan't find J21, J22[0m
[1633353819.364664]: [1;31mFailed calibration.[0m
J: -nan
-nan
[1633353819.364671]: [1;31mCan't find J21, J22[0m
[1633353819.364674]: [1;31mFailed calibration.[0m
J: -nan
-nan
[1633353819.364681]: [1;31mCan't find J21, J22[0m
[1633353819.364684]: [1;31mFailed calibration.[0m
J: -nan
-nan
[1633353819.364691]: [1;31mCan't find J21, J22[0m
[1633353819.364693]: [1;31mFailed calibration.[0m
J: -nan
-nan
[1633353819.364700]: [1;31mCan't find J21, J22[0m
[1633353819.364703]: [1;31mFailed calibration.[0m

-------Errors (std dev)-------
LiDAR-odom x: -nan mm
LiDAR-odom y: -nan mm
LiDAR-odom yaw: -nan deg

-------Uncertainty-------
Uncertainty Left wheel radius : -nan mm
Uncertainty Right wheel radius : -nan mm
Uncertainty Axle between wheels : -nan mm
Uncertainty LiDAR-odom-x : -nan mm
Uncertainty LiDAR-odom-y : -nan mm
Uncertainty LiDAR-odom-yaw : inf deg

I have try to go straightly or rotate at place, all ways can not get a good result. I see odom and laser on Rviz, they look normal. So should I do something to get a good result?

請問能不能也發給我example rosbag 試看看(我用自己錄的rosbag誤差很大)

請問能不能也發給我example rosbag 試看看(我用自己錄的rosbag誤差很大)
請放在這個google drive 上:
https://drive.google.com/drive/folders/1rbIeLl8nRSY3FBXGcIrFKFFIner1PYc9?usp=sharing

Originally posted by @jiayi0723 in #2 (comment)
大哥能不能也给我发一个，我的邮箱[email protected]

Failed calibration

Hi, I used my rosbag to calibrated, but failed.
The message was:
[1631759779.130374]: Matrix A is almost singular.
[1631759779.130399]: Failed calibration.
Coud you please tell me why it failed, or send me a example bag. my email: [email protected].

Best.