作者您好,我在使用您提供的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
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
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
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
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
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