[ros-users] slam_gmapping: extracting pose history
Ivan Dryanovski
ivan.dryanovski at gmail.com
Fri May 14 00:49:34 UTC 2010
Brian and Giorgio,
Thank you for the replies so far. I resolved the pose history issue.
However I am running into some different problems at the moment. I
have already contacted Giorgio about this issue. I am including a more
detailed explanation this time.
This loop in the ROS driver I am using is responsible for building the
map before publishing it. It builds it by iterating up the node tree:
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, delta_);
for(GMapping::GridSlamProcessor::TNode* n = best.node; n; n = n->parent)
{
if(!n->reading) {
ROS_DEBUG("Reading is NULL");
continue;
}
matcher.invalidateActiveArea();
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
}
However, the execution time of this loop grows linearly with the
runtime of the program. I am assuming this is because the depth of the
node tree grows as well. I am attaching output of the slam_gmapping
node to show the execution time of the loop. The two logs are taken
using this .bag file:
http://pr.willowgarage.com/data/gmapping/basic_localization_stage.bag
The two test runs are with slightly different parameters (included in
the top of the log file.) You will see that the execution time starts
at less than 100 ms, and steadily increases up to 3 seconds, over the
course of 90 seconds. The only time the value drops significantly is
right at the end of the bag file, when no more data is being sent.You
will notice that even after resampling, the time remains 1-2 seconds.
While 1-2 seconds does not sound bad, I assume that this will only get
worse, since it grows linearly with the number of scans processed.
We are using the gmapping package to perform odometry-free slam on a
quadrotor micro-UAV. We've had good success so far. However, when we
hit the map update step, and it takes 3+ seconds to perform that
operation, it really throws off the scan matcher.
Giorgio has informed me that it is not necessary to iterate over the
tree in order to get the map. In fact, I'm told I should be able to
get the map simply by doing
GMapping::GridSlamProcessor::Particle best =
gsp_->getParticles()[gsp_->getBestParticleIndex()];
GMapping::ScanMatcherMap smap = best.map;
I had some limited success in implementing this inside the ROS driver,
but it doesn't really work correctly. My guess is that there are extra
functions that need to be called for invalidating/re-calculating the
size of the map, but I don't know exactly what and where.
Thanks,
Ivan Dryanovski
-------------- next part --------------
[parameters:]
<param name="temporalUpdate" value="0.5"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="20"/>
<param name="throttle_scans" value="1"/>
------------------------------------------------------------------------
[ INFO] [34.600893000]: Initialization complete
update frame 0
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 0
Registering First Scan
[ INFO] [34.702660999]: === TRAJECTORY TREE ITERATION TIME: 61 [ms]
update frame 3
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 1
Average Scan Matching Score=1064.2
neff= 20
Registering Scans:Done
update frame 7
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 2
Average Scan Matching Score=1064.2
neff= 20
Registering Scans:Done
update frame 12
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 3
Average Scan Matching Score=1064.2
neff= 20
Registering Scans:Done
update frame 18
update ld=0.1 ad=0
Laser Pose= 0.1 2.07285e-15 0
m_count 4
Average Scan Matching Score=1024.59
neff= 19.5599
Registering Scans:Done
update frame 19
update ld=0.25 ad=0
Laser Pose= 0.35 7.25498e-15 0
m_count 5
Average Scan Matching Score=997.944
neff= 19.5317
Registering Scans:Done
update frame 20
update ld=0.45 ad=0.20944
Laser Pose= 0.8 1.65828e-14 0.20944
m_count 6
Average Scan Matching Score=938.081
neff= 18.0612
Registering Scans:Done
update frame 21
update ld=0.1 ad=0.733038
Laser Pose= 0.858779 0.0809017 0.942478
m_count 7
Average Scan Matching Score=861.04
neff= 17.3131
Registering Scans:Done
[ INFO] [42.668418925]: === TRAJECTORY TREE ITERATION TIME: 581 [ms]
update frame 22
update ld=1.4 ad=0
Laser Pose= 1.68168 1.21353 0.942478
m_count 8
Average Scan Matching Score=849.717
neff= 13.3346
Registering Scans:Done
update frame 23
update ld=0.69562 ad=0.314159
Laser Pose= 2.02954 1.81592 1.25664
m_count 9
Average Scan Matching Score=832.409
neff= 13.1745
Registering Scans:Done
update frame 24
update ld=0.496827 ad=0.314159
Laser Pose= 2.1227 2.30394 1.5708
m_count 10
Average Scan Matching Score=836.478
neff= 13.1487
Registering Scans:Done
[ INFO] [49.167889869]: === TRAJECTORY TREE ITERATION TIME: 592 [ms]
update frame 25
update ld=0.929071 ad=0.733038
Laser Pose= 1.85643 3.19403 2.30383
m_count 11
Average Scan Matching Score=763.116
neff= 9.20134
*************RESAMPLE***************
Deleting Nodes: 1 2 3 7 11 12 13 14 15 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 26
update ld=0.249781 ad=0.314159
Laser Pose= 1.64277 3.32342 2.61799
m_count 12
Average Scan Matching Score=1008.52
neff= 18.3627
Registering Scans:Done
update frame 27
update ld=0.6 ad=0
Laser Pose= 1.12316 3.62342 2.61799
m_count 13
Average Scan Matching Score=990.687
neff= 16.8581
Registering Scans:Done
[ INFO] [54.195042844]: === TRAJECTORY TREE ITERATION TIME: 732 [ms]
update frame 28
update ld=1.2 ad=0
Laser Pose= 0.0839251 4.22342 2.61799
m_count 14
Average Scan Matching Score=1046.88
neff= 16.3939
Registering Scans:Done
update frame 29
update ld=0.65 ad=0
Laser Pose= -0.478991 4.54842 2.61799
m_count 15
Average Scan Matching Score=1070.82
neff= 16.3822
Registering Scans:Done
update frame 30
update ld=0.55 ad=0
Laser Pose= -0.955305 4.82342 2.61799
m_count 16
Average Scan Matching Score=1071.32
neff= 16.9671
Registering Scans:Done
update frame 31
update ld=0.55 ad=0
Laser Pose= -1.43162 5.09842 2.61799
m_count 17
Average Scan Matching Score=1069.87
neff= 16.589
Registering Scans:Done
[ INFO] [60.393489826]: === TRAJECTORY TREE ITERATION TIME: 902 [ms]
update frame 32
update ld=1.4 ad=0
Laser Pose= -2.64405 5.79842 2.61799
m_count 18
Average Scan Matching Score=982.749
neff= 8.19079
*************RESAMPLE***************
Deleting Nodes: 3 5 7 11 12 14 16 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 33
update ld=0.6 ad=0
Laser Pose= -3.16367 6.09842 2.61799
m_count 19
Average Scan Matching Score=1044.7
neff= 19.9058
Registering Scans:Done
update frame 34
update ld=0.8 ad=0
Laser Pose= -3.85649 6.49842 2.61799
m_count 20
Average Scan Matching Score=1003.62
neff= 18.961
Registering Scans:Done
[ INFO] [66.835207799]: === TRAJECTORY TREE ITERATION TIME: 1412 [ms]
update frame 35
update ld=1.95 ad=0
Laser Pose= -5.54524 7.47342 2.61799
m_count 21
Average Scan Matching Score=755.124
neff= 6.95536
*************RESAMPLE***************
Deleting Nodes: 2 3 4 6 8 12 13 15 17 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 36
update ld=0.8 ad=0
Laser Pose= -6.23806 7.87342 2.61799
m_count 22
Average Scan Matching Score=971.941
neff= 17.8668
Registering Scans:Done
[ INFO] [71.759819730]: === TRAJECTORY TREE ITERATION TIME: 1191 [ms]
update frame 37
update ld=0.757674 ad=0.837758
Laser Pose= -6.98653 7.99116 -2.82743
m_count 23
Average Scan Matching Score=866.52
neff= 7.94249
*************RESAMPLE***************
Deleting Nodes: 2 3 4 5 7 8 13 15 16 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 38
update ld=0.55 ad=0
Laser Pose= -7.50961 7.8212 -2.82743
m_count 24
Average Scan Matching Score=1044.48
neff= 15.0282
Registering Scans:Done
update frame 39
update ld=0.6 ad=0
Laser Pose= -8.08025 7.63579 -2.82743
m_count 25
Average Scan Matching Score=1047.78
neff= 15.3552
Registering Scans:Done
[ INFO] [77.488704688]: === TRAJECTORY TREE ITERATION TIME: 1335 [ms]
update frame 40
update ld=1.55 ad=0
Laser Pose= -9.55438 7.15681 -2.82743
m_count 26
Average Scan Matching Score=985.637
neff= 12.8784
Registering Scans:Done
update frame 41
update ld=0.7 ad=0
Laser Pose= -10.2201 6.9405 -2.82743
m_count 27
Average Scan Matching Score=978.231
neff= 12.4864
Registering Scans:Done
update frame 42
update ld=0.6 ad=0
Laser Pose= -10.7908 6.75509 -2.82743
m_count 28
Average Scan Matching Score=893.235
neff= 12.3072
Registering Scans:Done
[ INFO] [84.194350662]: === TRAJECTORY TREE ITERATION TIME: 2105 [ms]
update frame 43
update ld=2.2 ad=0
Laser Pose= -12.8831 6.07525 -2.82743
m_count 29
Average Scan Matching Score=555.099
neff= 6.88032
*************RESAMPLE***************
Deleting Nodes: 1 5 6 8 10 12 13 14 15 16 17 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 44
update ld=0.7 ad=0
Laser Pose= -13.5488 5.85894 -2.82743
m_count 30
Average Scan Matching Score=788.588
neff= 14.1037
Registering Scans:Done
[ INFO] [89.255374633]: === TRAJECTORY TREE ITERATION TIME: 1660 [ms]
update frame 45
update ld=1.05751 ad=0.628319
Laser Pose= -14.3044 5.11902 -2.19911
m_count 31
Average Scan Matching Score=859.541
neff= 6.24972
*************RESAMPLE***************
Deleting Nodes: 1 4 8 10 11 13 14 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 46
update ld=0.7 ad=0
Laser Pose= -14.7158 4.55271 -2.19911
m_count 32
Average Scan Matching Score=1022.23
neff= 18.156
Registering Scans:Done
update frame 47
update ld=0.598176 ad=0.209439
Laser Pose= -15.0504 4.05685 -1.98968
m_count 33
Average Scan Matching Score=998.586
neff= 17.5587
Registering Scans:Done
[ INFO] [96.567962551]: === TRAJECTORY TREE ITERATION TIME: 2366 [ms]
update frame 48
update ld=1.79772 ad=0
Laser Pose= -15.8431 2.44335 -1.98968
m_count 34
Average Scan Matching Score=850.066
neff= 9.36294
*************RESAMPLE***************
Deleting Nodes: 0 2 4 5 6 8 10 14 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 49
update ld=0.85 ad=0
Laser Pose= -16.1889 1.66684 -1.98968
m_count 35
Average Scan Matching Score=975.225
neff= 13.7335
Registering Scans:Done
[ INFO] [102.094871502]: === TRAJECTORY TREE ITERATION TIME: 1806 [ms]
update frame 50
update ld=1.59123 ad=0.209439
Laser Pose= -16.9845 0.288791 -2.19911
m_count 36
Average Scan Matching Score=991.533
neff= 13.6754
Registering Scans:Done
update frame 51
update ld=0.8 ad=0
Laser Pose= -17.4547 -0.358422 -2.19911
m_count 37
Average Scan Matching Score=1050.26
neff= 13.8189
Registering Scans:Done
[ INFO] [107.785389479]: === TRAJECTORY TREE ITERATION TIME: 1995 [ms]
update frame 52
update ld=1.59412 ad=0.209439
Laser Pose= -18.2832 -1.72031 -1.98968
m_count 38
Average Scan Matching Score=820.565
neff= 7.99354
*************RESAMPLE***************
Deleting Nodes: 2 3 4 7 8 9 12 13 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 53
update ld=0.4 ad=0.20944
Laser Pose= -18.3664 -2.11157 -1.78024
m_count 39
Average Scan Matching Score=1004.27
neff= 17.7484
Registering Scans:Done
[ INFO] [113.166469434]: === TRAJECTORY TREE ITERATION TIME: 2043 [ms]
update frame 54
update ld=1.54159 ad=0
Laser Pose= -18.8577 -3.57279 -1.78024
m_count 40
Average Scan Matching Score=384.137
neff= 9.13579
*************RESAMPLE***************
Deleting Nodes: 0 3 8 9 13 14 16 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 55
update ld=0.5 ad=0
Laser Pose= -18.9616 -4.06186 -1.78024
m_count 41
Average Scan Matching Score=841.23
neff= 18.0774
Registering Scans:Done
[ INFO] [118.149668367]: === TRAJECTORY TREE ITERATION TIME: 2162 [ms]
update frame 56
update ld=1.84948 ad=0.10472
Laser Pose= -19.3359 -5.87308 -1.67552
m_count 42
Average Scan Matching Score=553.584
neff= 7.42555
*************RESAMPLE***************
Deleting Nodes: 0 1 2 4 7 8 11 12 14 16 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 57
update ld=0.45 ad=0
Laser Pose= -19.383 -6.32061 -1.67552
m_count 43
Average Scan Matching Score=1060.43
neff= 19.279
Registering Scans:Done
update frame 58
update ld=0.449028 ad=0.20944
Laser Pose= -19.3464 -6.76814 -1.46608
m_count 44
Average Scan Matching Score=1029.41
neff= 18.9278
Registering Scans:Done
[ INFO] [126.152724295]: === TRAJECTORY TREE ITERATION TIME: 3189 [ms]
update frame 59
update ld=1.29305 ad=0.628319
Laser Pose= -19.3673 -8.06102 -2.0944
m_count 45
Average Scan Matching Score=697.644
neff= 11.7036
Registering Scans:Done
[ INFO] [126.795137292]: === TRAJECTORY TREE ITERATION TIME: 0 [ms]
-------------- next part --------------
[parameters:] all default
------------------------------------------------------------------------
-maxUrange 29.99 -maxUrange 29.99 -sigma 0.05 -kernelSize 1 -lstep 0.05 -lobsGain 3 -astep 0.05
-srr 0.1 -srt 0.2 -str 0.1 -stt 0.2
-linearUpdate 1 -angularUpdate 0.5 -resampleThreshold 0.5
-xmin -100 -xmax 100 -ymin -100 -ymax 100 -delta 0.05 -particles 20
[ INFO] [34.621196999]: Initialization complete
update frame 0
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 0
Registering First Scan
[ INFO] [34.732914998]: === TRAJECTORY TREE ITERATION TIME: 70 [ms]
update frame 1
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 1
Average Scan Matching Score=1064.2
neff= 20
Registering Scans:Done
update frame 2
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 2
Average Scan Matching Score=1064.2
neff= 20
Registering Scans:Done
update frame 3
update ld=0 ad=0
Laser Pose= 0 0 0
m_count 3
Average Scan Matching Score=1064.2
neff= 20
Registering Scans:Done
update frame 4
update ld=0.1 ad=0
Laser Pose= 0.1 2.07285e-15 0
m_count 4
Average Scan Matching Score=1024.75
neff= 19.698
Registering Scans:Done
update frame 5
update ld=0.65 ad=0
Laser Pose= 0.75 1.55464e-14 0
m_count 5
Average Scan Matching Score=929.491
neff= 19.3557
Registering Scans:Done
update frame 6
update ld=0.0891007 ad=0.942478
Laser Pose= 0.829389 0.0404508 0.942478
m_count 6
Average Scan Matching Score=803.108
neff= 17.2044
Registering Scans:Done
[ INFO] [42.374536908]: === TRAJECTORY TREE ITERATION TIME: 492 [ms]
update frame 7
update ld=1.35 ad=0
Laser Pose= 1.6229 1.13262 0.942478
m_count 7
Average Scan Matching Score=778.857
neff= 13.1364
Registering Scans:Done
update frame 8
update ld=0.598107 ad=0.20944
Laser Pose= 1.95308 1.63134 1.15192
m_count 8
Average Scan Matching Score=910.129
neff= 12.5911
Registering Scans:Done
update frame 9
update ld=0.448661 ad=0.20944
Laser Pose= 2.09668 2.0564 1.36136
m_count 9
Average Scan Matching Score=880.05
neff= 12.2459
Registering Scans:Done
[ INFO] [48.290523854]: === TRAJECTORY TREE ITERATION TIME: 701 [ms]
update frame 10
update ld=0.939086 ad=0.628319
Laser Pose= 1.99807 2.99029 1.98968
m_count 10
Average Scan Matching Score=730.372
neff= 9.8985
*************RESAMPLE***************
Deleting Nodes: 4 5 6 9 10 11 13 17 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 11
update ld=0.390333 ad=0.628318
Laser Pose= 1.72937 3.27342 2.61799
m_count 11
Average Scan Matching Score=980.115
neff= 16.5408
Registering Scans:Done
update frame 12
update ld=0.6 ad=0
Laser Pose= 1.20976 3.57342 2.61799
m_count 12
Average Scan Matching Score=987.482
neff= 17.4553
Registering Scans:Done
[ INFO] [53.893228823]: === TRAJECTORY TREE ITERATION TIME: 705 [ms]
update frame 13
update ld=1.15 ad=0
Laser Pose= 0.213829 4.14842 2.61799
m_count 13
Average Scan Matching Score=1026.94
neff= 17.3865
Registering Scans:Done
update frame 14
update ld=0.8 ad=0
Laser Pose= -0.478991 4.54842 2.61799
m_count 14
Average Scan Matching Score=1066.78
neff= 17.2957
Registering Scans:Done
update frame 15
update ld=0.85 ad=0
Laser Pose= -1.21511 4.97342 2.61799
m_count 15
Average Scan Matching Score=1050.55
neff= 13.9789
Registering Scans:Done
[ INFO] [60.472536794]: === TRAJECTORY TREE ITERATION TIME: 1112 [ms]
update frame 16
update ld=1.8 ad=0
Laser Pose= -2.77396 5.87342 2.61799
m_count 16
Average Scan Matching Score=889.004
neff= 8.4028
*************RESAMPLE***************
Deleting Nodes: 5 6 9 12 14 15 17 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 17
update ld=0.75 ad=0
Laser Pose= -3.42348 6.24842 2.61799
m_count 17
Average Scan Matching Score=1007.43
neff= 19.6612
Registering Scans:Done
[ INFO] [65.627045721]: === TRAJECTORY TREE ITERATION TIME: 1204 [ms]
update frame 18
update ld=1.8 ad=0
Laser Pose= -4.98232 7.14842 2.61799
m_count 18
Average Scan Matching Score=822.304
neff= 9.31719
*************RESAMPLE***************
Deleting Nodes: 0 1 8 9 13 14 15 17 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 19
update ld=0.95 ad=0
Laser Pose= -5.80505 7.62342 2.61799
m_count 19
Average Scan Matching Score=999.818
neff= 19.8919
Registering Scans:Done
[ INFO] [71.152614659]: === TRAJECTORY TREE ITERATION TIME: 1207 [ms]
update frame 20
update ld=1.06247 ad=0.628319
Laser Pose= -6.7898 8.02229 -3.03687
m_count 20
Average Scan Matching Score=798.854
neff= 5.11531
*************RESAMPLE***************
Deleting Nodes: 0 2 5 6 8 9 10 12 13 15 18 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 21
update ld=0.397946 ad=0.20944
Laser Pose= -7.17674 7.92936 -2.82743
m_count 21
Average Scan Matching Score=1032.64
neff= 15.4769
Registering Scans:Done
update frame 22
update ld=0.8 ad=0
Laser Pose= -7.93759 7.68214 -2.82743
m_count 22
Average Scan Matching Score=1035.14
neff= 16.7938
Registering Scans:Done
[ INFO] [78.090914617]: === TRAJECTORY TREE ITERATION TIME: 1698 [ms]
update frame 23
update ld=2.15 ad=0
Laser Pose= -9.98236 7.01776 -2.82743
m_count 23
Average Scan Matching Score=751.358
neff= 5.45783
*************RESAMPLE***************
Deleting Nodes: 0 1 2 9 10 11 12 17 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 24
update ld=0.85 ad=0
Laser Pose= -10.7908 6.75509 -2.82743
m_count 24
Average Scan Matching Score=838.72
neff= 15.2216
Registering Scans:Done
[ INFO] [82.996300612]: === TRAJECTORY TREE ITERATION TIME: 1202 [ms]
update frame 25
update ld=1.55 ad=0
Laser Pose= -12.2649 6.27612 -2.82743
m_count 25
Average Scan Matching Score=670.165
neff= 11.0797
Registering Scans:Done
update frame 26
update ld=0.55 ad=0
Laser Pose= -12.788 6.10616 -2.82743
m_count 26
Average Scan Matching Score=948.001
neff= 10.6571
Registering Scans:Done
update frame 27
update ld=0.6 ad=0
Laser Pose= -13.3586 5.92075 -2.82743
m_count 27
Average Scan Matching Score=863.695
neff= 11.8677
Registering Scans:Done
[ INFO] [88.535154540]: === TRAJECTORY TREE ITERATION TIME: 1443 [ms]
update frame 28
update ld=0.903636 ad=0.628319
Laser Pose= -14.0986 5.40218 -2.19911
m_count 28
Average Scan Matching Score=923.601
neff= 11.8019
Registering Scans:Done
update frame 29
update ld=0.75 ad=0
Laser Pose= -14.5395 4.79542 -2.19911
m_count 29
Average Scan Matching Score=1018.4
neff= 9.98898
*************RESAMPLE***************
Deleting Nodes: 0 2 4 5 9 10 11 13 15 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 30
update ld=0.65 ad=0
Laser Pose= -14.9215 4.26956 -2.19911
m_count 30
Average Scan Matching Score=1026.07
neff= 16.6808
Registering Scans:Done
[ INFO] [94.754941483]: === TRAJECTORY TREE ITERATION TIME: 1595 [ms]
update frame 31
update ld=1.29684 ad=0.10472
Laser Pose= -15.4868 3.10238 -2.0944
m_count 31
Average Scan Matching Score=744.974
neff= 10.1653
Registering Scans:Done
update frame 32
update ld=0.55 ad=0
Laser Pose= -15.7618 2.62606 -2.0944
m_count 32
Average Scan Matching Score=1063.3
neff= 10.1881
Registering Scans:Done
update frame 33
update ld=0.6 ad=0.10472
Laser Pose= -16.0058 2.07794 -1.98968
m_count 33
Average Scan Matching Score=1034.54
neff= 7.03379
*************RESAMPLE***************
Deleting Nodes: 2 3 5 10 12 13 15 16 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
[ INFO] [101.288997438]: === TRAJECTORY TREE ITERATION TIME: 1743 [ms]
update frame 34
update ld=1.59401 ad=0.209439
Laser Pose= -16.72 0.652849 -2.19911
m_count 34
Average Scan Matching Score=930.877
neff= 11.3966
Registering Scans:Done
update frame 35
update ld=0.8 ad=0
Laser Pose= -17.1902 0.00563538 -2.19911
m_count 35
Average Scan Matching Score=1061.05
neff= 11.2843
Registering Scans:Done
[ INFO] [106.594184406]: === TRAJECTORY TREE ITERATION TIME: 1814 [ms]
update frame 36
update ld=1.64821 ad=0.10472
Laser Pose= -18.1205 -1.3549 -2.0944
m_count 36
Average Scan Matching Score=883.959
neff= 7.21177
*************RESAMPLE***************
Deleting Nodes: 0 1 8 9 10 12 14 16 18 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 37
update ld=0.399999 ad=0.314159
Laser Pose= -18.2832 -1.72031 -1.78024
m_count 37
Average Scan Matching Score=1045.17
neff= 18.616
Registering Scans:Done
[ INFO] [112.590899390]: === TRAJECTORY TREE ITERATION TIME: 2304 [ms]
update frame 38
update ld=1.69069 ad=0
Laser Pose= -18.8057 -3.32825 -1.78024
m_count 38
Average Scan Matching Score=267.676
neff= 8.45856
*************RESAMPLE***************
Deleting Nodes: 2 3 5 7 11 12 14 15 16 17 18 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
update frame 39
update ld=0.65 ad=0
Laser Pose= -18.9408 -3.96405 -1.78024
m_count 39
Average Scan Matching Score=707.614
neff= 6.88858
*************RESAMPLE***************
Deleting Nodes: 0 2 4 5 8 9 11 13 15 16 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
[ INFO] [119.028992325]: === TRAJECTORY TREE ITERATION TIME: 2676 [ms]
update frame 40
update ld=2.39768 ad=0.10472
Laser Pose= -19.383 -6.32061 -1.67552
m_count 40
Average Scan Matching Score=615.21
neff= 7.15963
*************RESAMPLE***************
Deleting Nodes: 0 1 2 3 4 6 8 11 12 13 18 19 Done
Deleting old particles...Done
Copying Particles and Registering scans... Done
[ INFO] [123.652256273]: === TRAJECTORY TREE ITERATION TIME: 2106 [ms]
update frame 41
update ld=1.39331 ad=0
Laser Pose= -19.3307 -7.71294 -1.67552
m_count 41
Average Scan Matching Score=805.11
neff= 17.4657
Registering Scans:Done
update frame 42
update ld=0.35 ad=0.314159
Laser Pose= -19.3673 -8.06102 -1.98968
m_count 42
Average Scan Matching Score=1035.17
neff= 14.4703
Registering Scans:Done
[ INFO] [126.791703251]: === TRAJECTORY TREE ITERATION TIME: 300 [ms]
update frame 43
update ld=0 ad=0.10472
Laser Pose= -19.3673 -8.06102 -2.0944
m_count 43
Average Scan Matching Score=1075.64
More information about the ros-users
mailing list