r3live数据集hku_main_building.bag测试算法中途挂掉

ZoranDai commented 3 months ago

作者您好，我在使用您提供的ff_lins_r3live.yaml并使用 R3LIVE Campus Dataset中的hku_main_building.bag测试本算法时发现程序出现如下报错，此外，我测试您自测的数据集时（park.bag）程序可以正常运行。

W0617 21:47:51.779225 27323 residual_block.cc:127]

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all
residual and jacobians that were requested or there was a non-finite value (nan/infinite) generated during the or jacobian computation.

Residual Block size: 4 parameter blocks x 1 residuals

For each parameter block, the value of the parameters are printed in the first column
and the value of the jacobian under the corresponding residual. If a ParameterBlock was held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry of the Jacobian/residual array was requested but was not written to by user code, it is indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating to Inf or NaN is also an error.

Residuals: -nan

Parameter Block 0, size: 7

-0.75506 |          nan 
  5.6792 |          nan 
 149.511 |          nan

-0.0530644 | nan -0.0743469 | nan -0.126301 | nan 0.987778 | 0

Parameter Block 1, size: 7

-0.783129 | nan 6.26575 | nan 165.144 | nan -0.0644445 | nan -0.0790704 | nan -0.111678 | nan 0.988495 | 0

Parameter Block 2, size: 7

0.0404252 | nan -0.0159453 | nan 0.00438504 | nan 0.998268 | nan 0.0117375 | -nan -0.0505127 | nan 0.0277912 | 0

Parameter Block 3, size: 1

       0 |         -nan

E0617 21:47:51.779275 27323 trust_region_minimizer.cc:72] Terminating: Initial residual and Jacobian evaluation failed. I0617 21:47:51.779397 27323 optimizer.cc:156] Remove 58 lidar outliers factors W0617 21:47:51.787488 27323 residual_block.cc:127]

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all
residual and jacobians that were requested or there was a non-finite value (nan/infinite) generated during the or jacobian computation.

Residual Block size: 4 parameter blocks x 1 residuals

For each parameter block, the value of the parameters are printed in the first column
and the value of the jacobian under the corresponding residual. If a ParameterBlock was held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry of the Jacobian/residual array was requested but was not written to by user code, it is indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating to Inf or NaN is also an error.

Residuals: -nan

Parameter Block 0, size: 7

-0.75506 |          nan 
  5.6792 |          nan 
 149.511 |          nan

-0.0530644 | nan -0.0743469 | nan -0.126301 | nan 0.987778 | 0

Parameter Block 1, size: 7

-0.783129 | nan 6.26575 | nan 165.144 | nan -0.0644445 | nan -0.0790704 | nan -0.111678 | nan 0.988495 | 0

Parameter Block 2, size: 7

0.0404252 | nan -0.0159453 | nan 0.00438504 | nan 0.998268 | nan 0.0117375 | -nan -0.0505127 | nan 0.0277912 | 0

Parameter Block 3, size: 1

       0 |         -nan

W0617 21:47:51.787492 27612 residual_block.cc:127]

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all
residual and jacobians that were requested or there was a non-finite value (nan/infinite) generated during the or jacobian computation.

Residual Block size: 4 parameter blocks x 1 residuals

For each parameter block, the value of the parameters are printed in the first column
and the value of the jacobian under the corresponding residual. If a ParameterBlock was held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry of the Jacobian/residual array was requested but was not written to by user code, it is indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating to Inf or NaN is also an error.

Residuals: -nan

Parameter Block 0, size: 7

-0.75506 |          nan 
  5.6792 |          nan 
 149.511 |          nan

-0.0530644 | nan -0.0743469 | nan -0.126301 | nan 0.987778 | 0

Parameter Block 1, size: 7

-0.783129 | nan 6.26575 | nan 165.144 | nan -0.0644445 | nan -0.0790704 | nan -0.111678 | nan 0.988495 | 0

Parameter Block 2, size: 7

0.0404252 | nan -0.0159453 | nan 0.00438504 | nan 0.998268 | nan 0.0117375 | -nan -0.0505127 | nan 0.0277912 | 0

Parameter Block 3, size: 1

       0 |         -nan

W0617 21:47:51.787515 27608 residual_block.cc:127]

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all
residual and jacobians that were requested or there was a non-finite value (nan/infinite) generated during the or jacobian computation.

Residual Block size: 4 parameter blocks x 1 residuals

For each parameter block, the value of the parameters are printed in the first column
and the value of the jacobian under the corresponding residual. If a ParameterBlock was held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry of the Jacobian/residual array was requested but was not written to by user code, it is indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating to Inf or NaN is also an error.

Residuals: -nan

Parameter Block 0, size: 7

-0.75506 |          nan 
  5.6792 |          nan 
 149.511 |          nan

-0.0530644 | nan -0.0743469 | nan -0.126301 | nan 0.987778 | 0

Parameter Block 1, size: 7

-0.783129 | nan 6.26575 | nan 165.144 | nan -0.0644445 | nan -0.0790704 | nan -0.111678 | nan 0.988495 | 0

Parameter Block 2, size: 7

0.0404252 | nan -0.0159453 | nan 0.00438504 | nan 0.998268 | nan 0.0117375 | -nan -0.0505127 | nan 0.0277912 | 0

Parameter Block 3, size: 1

       0 |         -nan

W0617 21:47:51.787492 27611 residual_block.cc:127]

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all
residual and jacobians that were requested or there was a non-finite value (nan/infinite) generated during the or jacobian computation.

Residual Block size: 4 parameter blocks x 1 residuals

For each parameter block, the value of the parameters are printed in the first column
and the value of the jacobian under the corresponding residual. If a ParameterBlock was held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry of the Jacobian/residual array was requested but was not written to by user code, it is indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating to Inf or NaN is also an error.

Residuals: -nan

Parameter Block 0, size: 7

-0.75506 |          nan 
  5.6792 |          nan 
 149.511 |          nan

-0.0530644 | nan -0.0743469 | nan -0.126301 | nan 0.987778 | 0

Parameter Block 1, size: 7

-0.783129 | nan 6.26575 | nan 165.144 | nan -0.0644445 | nan -0.0790704 | nan -0.111678 | nan 0.988495 | 0

Parameter Block 2, size: 7

0.0404252 | nan -0.0159453 | nan 0.00438504 | nan 0.998268 | nan 0.0117375 | -nan -0.0505127 | nan 0.0277912 | 0

Parameter Block 3, size: 1

       0 |         -nan

E0617 21:47:51.788050 27323 trust_region_minimizer.cc:72] Terminating: Initial residual and Jacobian evaluation failed. W0617 21:47:51.788703 27323 misc.cc:130] Failed to get state data index at 26136.895397 W0617 21:47:51.788834 27323 optimizer.cc:571] Wrong matching time node 26135.986051 to 26136.895397 E0617 21:47:51.788951 27323 optimizer.cc:207] Wrong matched lidar frame at 26136.895397 I0617 21:47:51.789013 27323 optimizer.cc:182] Optimization costs 0.666075 ms and 9.14415, with iteration 0 and 0 I0617 21:47:51.790398 27323 lidar_map.cc:130] Remove lidar keyframe at 26135.986051 I0617 21:47:51.790422 27323 optimizer.cc:400] Marginalize 1 states, last time 26136.085889, costs 0.249335 I0617 21:47:51.790519 27323 misc.cc:198] Redo INS mechanization at 26137.054532 to 26137.076988 is 0.0224569 s I0617 21:47:51.795207 27323 lidar_map.cc:82] New lidar keyframe at 26137.154382 with interval 0.0998507 s, translation 8.08335, rotation 0.022616 I0617 21:47:51.796661 27323 ff_lins.cc:242] Accumulate 1 frames at 26137.154382 with 7545 points I0617 21:47:51.796907 27323 ff_lins.cc:517] Add new lidar time node at 26137.154382 with dt 0.100 I0617 21:47:51.800987 27323 lidar_map.cc:412] Find the newest 130 featureas and average 23 features at 26137.154382, update map cost 1.52139 ms I0617 21:47:51.801030 27323 optimizer.cc:407] Total 11 pose states from 26136.085889 to 26137.154382 W0617 21:47:51.801221 27323 misc.cc:130] Failed to get state data index at 26136.854064 W0617 21:47:51.801249 27323 optimizer.cc:571] Wrong matching time node 26136.085889 to 26136.854064 E0617 21:47:51.801257 27323 optimizer.cc:459] Wrong matched lidar frame at 26137.154382 I0617 21:47:51.801474 27323 optimizer.cc:127] Add 10 preintegration, 132 lidar factors I0617 21:47:51.807972 27323 optimizer.cc:156] Remove 41 lidar outliers factors W0617 21:47:51.819098 27323 misc.cc:130] Failed to get state data index at 26136.854064 W0617 21:47:51.819242 27323 optimizer.cc:571] Wrong matching time node 26136.085889 to 26136.854064 E0617 21:47:51.819252 27323 optimizer.cc:207] Wrong matched lidar frame at 26136.854064 I0617 21:47:51.819380 27323 optimizer.cc:182] Optimization costs 5.61189 ms and 4.33467, with iteration 6 and 11 I0617 21:47:51.819996 27323 lidar_map.cc:130] Remove lidar keyframe at 26136.085889 I0617 21:47:51.820012 27323 optimizer.cc:400] Marginalize 1 states, last time 26136.185716, costs 0.208661 I0617 21:47:51.820065 27323 misc.cc:198] Redo INS mechanization at 26137.154382 to 26137.176993 is 0.0226109 s I0617 21:47:51.823982 27323 lidar_map.cc:82] New lidar keyframe at 26137.349442 with interval 0.19506 s, translation 16.3481, rotation 0.0114846 I0617 21:47:51.825424 27323 ff_lins.cc:242] Accumulate 1 frames at 26137.349442 with 7612 points I0617 21:47:51.825774 27323 ff_lins.cc:517] Add new lidar time node at 26137.349442 with dt 0.195 I0617 21:47:51.828315 27323 lidar_map.cc:412] Find the newest 7 featureas and average 1 features at 26137.349442, update map cost 0.763283 ms I0617 21:47:51.828336 27323 optimizer.cc:407] Total 11 pose states from 26136.185716 to 26137.349442 W0617 21:47:51.828460 27323 misc.cc:130] Failed to get state data index at 26136.854064 W0617 21:47:51.828493 27323 optimizer.cc:571] Wrong matching time node 26136.185716 to 26136.854064 E0617 21:47:51.828502 27323 optimizer.cc:459] Wrong matched lidar frame at 26137.349442 I0617 21:47:51.828572 27323 optimizer.cc:127] Add 10 preintegration, 7 lidar factors I0617 21:47:51.830756 27323 optimizer.cc:156] Remove 1 lidar outliers factors W0617 21:47:51.837357 27323 misc.cc:130] Failed to get state data index at 26136.854064 W0617 21:47:51.837385 27323 optimizer.cc:571] Wrong matching time node 26136.185716 to 26136.854064 E0617 21:47:51.837391 27323 optimizer.cc:207] Wrong matched lidar frame at 26136.854064 I0617 21:47:51.837455 27323 optimizer.cc:182] Optimization costs 2.16741 ms and 6.50056, with iteration 6 and 11 I0617 21:47:51.838855 27323 lidar_map.cc:130] Remove lidar keyframe at 26136.185716 I0617 21:47:51.838872 27323 optimizer.cc:400] Marginalize 1 states, last time 26136.289211, costs 0.177864 I0617 21:47:51.838920 27323 misc.cc:198] Redo INS mechanization at 26137.349442 to 26137.372995 is 0.0235529 s I0617 21:47:51.840654 27323 lidar_map.cc:82] New lidar keyframe at 26137.445552 with interval 0.0961102 s, translation 8.32611, rotation 0.00942272 I0617 21:47:51.842090 27323 ff_lins.cc:242] Accumulate 1 frames at 26137.445552 with 8022 points I0617 21:47:51.842305 27323 ff_lins.cc:517] Add new lidar time node at 26137.445552 with dt 0.096 I0617 21:47:51.847918 27323 lidar_map.cc:412] Find the newest 88 featureas and average 16 features at 26137.445552, update map cost 1.51543 ms I0617 21:47:51.847950 27323 optimizer.cc:407] Total 11 pose states from 26136.289211 to 26137.445552 W0617 21:47:51.848065 27323 misc.cc:130] Failed to get state data index at 26136.895397 W0617 21:47:51.848090 27323 optimizer.cc:571] Wrong matching time node 26136.289211 to 26136.895397 E0617 21:47:51.848098 27323 optimizer.cc:459] Wrong matched lidar frame at 26137.445552 I0617 21:47:51.848284 27323 optimizer.cc:127] Add 10 preintegration, 88 lidar factors I0617 21:47:51.861635 27323 optimizer.cc:156] Remove 39 lidar outliers factors W0617 21:47:51.866465 27323 misc.cc:130] Failed to get state data index at 26136.895397 W0617 21:47:51.866633 27323 optimizer.cc:571] Wrong matching time node 26136.289211 to 26136.895397 E0617 21:47:51.866647 27323 optimizer.cc:207] Wrong matched lidar frame at 26136.895397 I0617 21:47:51.866735 27323 optimizer.cc:182] Optimization costs 13.1406 ms and 4.57755, with iteration 6 and 11 I0617 21:47:51.868084 27323 lidar_map.cc:130] Remove lidar keyframe at 26136.289211 I0617 21:47:51.868110 27323 optimizer.cc:400] Marginalize 1 states, last time 26136.353686, costs 0.206422 I0617 21:47:51.868170 27323 misc.cc:198] Redo INS mechanization at 26137.445552 to 26137.467985 is 0.0224325 s I0617 21:47:51.871656 27322 lidar_viewer_rviz.cc:76] Lidar viewer thread is exited I0617 21:47:51.872114 27323 lidar_map.cc:82] New lidar keyframe at 26137.587645 with interval 0.142093 s, translation 12.6332, rotation 0.00967779 I0617 21:47:51.873627 27323 ff_lins.cc:242] Accumulate 1 frames at 26137.587645 with 7934 points I0617 21:47:51.873909 27323 ff_lins.cc:517] Add new lidar time node at 26137.587645 with dt 0.142 I0617 21:47:51.876935 27323 lidar_map.cc:412] Find the newest 25 featureas and average 4 features at 26137.587645, update map cost 1.05871 ms I0617 21:47:51.876965 27323 optimizer.cc:407] Total 11 pose states from 26136.353686 to 26137.587645 W0617 21:47:51.877079 27323 misc.cc:130] Failed to get state data index at 26136.854064 W0617 21:47:51.877108 27323 optimizer.cc:571] Wrong matching time node 26136.353686 to 26136.854064 E0617 21:47:51.877118 27323 optimizer.cc:459] Wrong matched lidar frame at 26137.587645 I0617 21:47:51.877233 27323 optimizer.cc:127] Add 10 preintegration, 25 lidar factors I0617 21:47:51.886273 27323 optimizer.cc:156] Remove 21 lidar outliers factors W0617 21:47:51.890851 27323 optimizer.cc:199] Estimate large td_b_l 0.931364, and td_b_l is reset W0617 21:47:51.890969 27323 misc.cc:130] Failed to get state data index at 26136.854064 W0617 21:47:51.890977 27323 optimizer.cc:571] Wrong matching time node 26136.353686 to 26136.854064 E0617 21:47:51.890985 27323 optimizer.cc:207] Wrong matched lidar frame at 26136.854064 I0617 21:47:51.891065 27323 optimizer.cc:182] Optimization costs 8.97178 ms and 4.54072, with iteration 6 and 11 I0617 21:47:51.892428 27323 lidar_map.cc:130] Remove lidar keyframe at 26136.353686 I0617 21:47:51.892447 27323 optimizer.cc:400] Marginalize 1 states, last time 26136.590054, costs 0.2043 I0617 21:47:51.892488 27323 misc.cc:198] Redo INS mechanization at 26137.587645 to 26137.609012 is 0.0213668 s I0617 21:47:51.892500 27323 ff_lins.cc:295] Fusion thread is exited W0617 21:47:51.892607 27316 ff_lins.cc:159] Estimated lidar extrinsics: (0.999911, 0.003695, -0.000529, 0.012788), (0.035, -0.024, 0.029), 0.0000 W0617 21:47:51.892743 27316 ff_lins.cc:163] LINS has finished processing I0617 21:47:52.121268 27305 main.cc:57] FF-LINS process costs 1.24986 seconds

thlsealight commented 3 months ago

不同数据集的配置文件不相同，请你使用对应的配置文件。另外，FF-LINS需要前右下的IMU，因此请使用我们提供的ROS包。

ZoranDai commented 3 months ago

好的谢谢我尝试修改一下IMU的信息

ZoranDai commented 3 months ago

问题已经解决，只需将r3live中使用的avia lidar 中imu坐标系 front-left-up 转换为 front-right-down形式即可，具体改动为修改ROS文件夹中的fusion.cc文件中的imuCallback函数，即可适配r3live数据集 20240618-201435

i2Nav-WHU / FF-LINS

r3live数据集hku_main_building.bag测试算法中途挂掉 #8

i2Nav-WHU / FF-LINS

r3live数据集hku_main_building.bag测试 算法中途挂掉 #8

r3live数据集hku_main_building.bag测试算法中途挂掉 #8