tf jumps a lot - Githubissues

MrGitGo commented 5 years ago

I'm trying to build a 3d map. I have a tof camera. My porblem is that the base_link(i guess) is jumping a lot. So it rotates and changes it's position when a submap is published. And I also do not get a good map result:

[ INFO] [1548760820.484216713]: I0129 12:20:20.000000 29712 submap_3d.cc:321] Added submap 1 [ INFO] [1548760820.485049553]: I0129 12:20:20.000000 29712 map_builder_bridge.cc:130] Added trajectory with ID '0'. [ INFO] [1548760820.691109502]: Opened camera : PICOMONSTAR850GLASS BNO055 init complete [ INFO] [1548760822.000468632]: I0129 12:20:22.000000 29712 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '636843576219399290'. [ INFO] [1548760822.000766575]: I0129 12:20:22.000000 29712 local_trajectory_builder_3d.cc:254] Extrapolator not yet initialized. [ INFO] [1548760825.228800242]: I0129 12:20:25.000000 29712 submap_3d.cc:321] Added submap 2 [ INFO] [1548760829.233395669]: I0129 12:20:29.000000 29712 submap_3d.cc:321] Added submap 3 [ INFO] [1548760830.046670119]: I0129 12:20:30.000000 29757 constraint_builder_3d.cc:296] Node (0, 10) with 31 points on submap (0, 0) differs by translation 0.08 rotation 0.006 with score 67.8%. [ INFO] [1548760830.046824771]: I0129 12:20:30.000000 29757 constraint_builder_3d.cc:311] 1 computations resulted in 1 additional constraints. Score histogram: Count: 1 Min: 0.678166 Max: 0.678166 Mean: 0.678166 Rotational score histogram: Count: 1 Min: 0.939993 Max: 0.939993 Mean: 0.939993 Low resolution score histogram: Count: 1 Min: 0.703455 Max: 0.703455 Mean: 0.703455 [ INFO] [1548760830.053132655]: I0129 12:20:30.000000 29757 pose_graph_3d.cc:608] Translational residuals histogram: Count: 17 Min: 0.000000 Max: 0.049497 Mean: 0.013767 0.039597, 0.044547) # Count: 1 (5.882353%) Total: 16 (94.117645%) [0.044547, 0.049497] # Count: 1 (5.882353%) Total: 17 (100.000000%) [ INFO] [1548760830.053337641]: I0129 12:20:30.000000 29757 pose_graph_3d.cc:610 [0.014849, 0.019799) ## Count: 2 (11.764706%) Total: 12 (70.588234%) [0.019799, 0.024748) #### Count: 3 (17.647058%) Total: 15 (88.235291%) 0.039597, 0.044547) # Count: 1 (5.882353%) Total: 16 (94.117645%) [0.044547, 0.049497] # Count: 1 (5.882353%) Total: 17 (100.000000%) [ INFO] [1548760830.053337641]: I0129 12:20:30.000000 29757 pose_graph_3d.cc:610 [0.039597, 0.044547) # Count: 1 (5.882353%) Total: 16 (94.117645%) [0.044547, 0.049497] # Count: 1 (5.882353%) Total: 17 (100.000000%) [ INFO] [1548760830.053337641]: I0129 12:20:30.000000 29757 pose_graph_3d.cc:610] Rotational residuals histogram: Count: 17 Min: 0.000000 Max: 0.196431 Mean: 0.072938 [0.000000, 0.019643) ############# Count: 11 (64.705879%) Total: 11 (64.705879%) [0.019643, 0.039286) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.039286, 0.058929) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.058929, 0.078572) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.078572, 0.098215) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.098215, 0.117859) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.117859, 0.137502) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.137502, 0.157145) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.157145, 0.176788) Count: 0 (0.000000%) Total: 11 (64.705879%) [0.176788, 0.196431] ####### Count: 6 (35.294117%) Total: 17 (100.000000%) [ INFO] [1548760832.429969455]: I0129 12:20:32.000000 29712 submap_3d.cc:321] Added submap 4 [ INFO] [1548760835.602751146]: I0129 12:20:35.000000 29712 collated_trajectory_builder.cc:72] imu rate: 100.00 Hz 1.00e-02 s +/- 1.56e-03 s (pulsed at 97.12% real time) [ INFO] [1548760835.602905030]: I0129 12:20:35.000000 29712 collated_trajectory_builder.cc:72] odom rate: 14.25 Hz 7.02e-02 s +/- 5.47e-04 s (pulsed at 99.88% real time) [ INFO] [1548760835.603002243]: I0129 12:20:35.000000 29712 collated_trajectory_builder.cc:72] points2 rate: 2.50 Hz 4.00e-01 s +/- 2.08e-04 s (pulsed at 100.02% real time) [ INFO] [1548760836.030842281]: I0129 12:20:36.000000 29712 submap_3d.cc:321] Added submap 5 [ INFO] [1548760837.241335977]: I0129 12:20:37.000000 29758 constraint_builder_3d.cc:311] 0 computations resulted in 0 additional constraints. Score histogram: Count: 1 Min: 0.678166 Max: 0.678166 Mean: 0.678166 Rotational score histogram: Count: 1 Min: 0.939993 Max: 0.939993 Mean: 0.939993 Low resolution score histogram: Count: 1 Min: 0.703455 Max: 0.703455 Mean: 0.703455 [ INFO] [1548760837.253684557]: I0129 12:20:37.000000 29758 pose_graph_3d.cc:608] Translational residuals histogram: Count: 39 Min: 0.000250 Max: 0.061434 Mean: 0.011261 [0.000250, 0.006368) ############ Count: 24 (61.538460%) Total: 24 (61.538460%) [0.006368, 0.012487) # Count: 2 (5.128205%) Total: 26 (66.666664%) [0.012487, 0.018605) ## Count: 3 (7.692307%) Total: 29 (74.358971%) [0.018605, 0.024724) ## Count: 4 (10.256411%) Total: 33 (84.615387%) 0.049197, 0.055316) Count: 0 (0.000000%) Total: 38 (97.435898%) [0.055316, 0.061434] # Count: 1 (2.564103%) Total: 39 (100.000000%) [ INFO] [1548760837.253959942]: I0129 12:20:37.000000 29758 pose_graph_3d.cc:610 0.049197, 0.055316) Count: 0 (0.000000%) Total: 38 (97.435898%) [0.055316, 0.061434] # Count: 1 (2.564103%) Total: 39 (100.000000%) [ INFO] [1548760837.253959942]: I0129 12:20:37.000000 29758 pose_graph_3d.cc:610 [ INFO] [1548760837.253959942]: I0129 12:20:37.000000 29758 pose_graph_3d.cc:610] Rotational residuals histogram: Count: 39 Min: 0.005910 Max: 0.537817 Mean: 0.227491 [0.005910, 0.059101) ##### Count: 10 (25.641026%) Total: 10 (25.641026%) [0.059101, 0.112292) # Count: 2 (5.128205%) Total: 12 (30.769230%) [0.112292, 0.165482) Count: 0 (0.000000%) Total: 12 (30.769230%) [0.165482, 0.218673) ######## Count: 16 (41.025642%) Total: 28 (71.794868%) [0.218673, 0.271864) # Count: 1 (2.564103%) Total: 29 (74.358971%) [0.271864, 0.325055) Count: 0 (0.000000%) Total: 29 (74.358971%) [0.325055, 0.378245) Count: 0 (0.000000%) Total: 29 (74.358971%) [0.378245, 0.431436) Count: 0 (0.000000%) Total: 29 (74.358971%) [0.431436, 0.484627) Count: 0 (0.000000%) Total: 29 (74.358971%) [0.484627, 0.537817] ##### Count: 10 (25.641026%) Total: 39 (100.000000%) [ INFO] [1548760839.633974062]: I0129 12:20:39.000000 29712 submap_3d.cc:321] Added submap 6 [ INFO] [1548760843.645441358]: I0129 12:20:43.000000 29712 submap_3d.cc:321] Added submap 7 [ INFO] [1548760844.449495115]: I0129 12:20:44.000000 29761 constraint_builder_3d.cc:296] Node (0, 30) with 28 points on submap (0, 0) differs by translation 0.11 rotation 0.072 with score 74.2%. [ INFO] [1548760846.050528667]: I0129 12:20:46.000000 29758 constraint_builder_3d.cc:311] 1 computations resulted in 1 additional constraints. Score histogram: Count: 2 Min: 0.678166 Max: 0.742465 Mean: 0.710315 [0.678166, 0.684596) ########## Count: 1 (50.000000%) Total: 1 (50.000000%) [0.684596, 0.691026) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.691026, 0.697456) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.697456, 0.703885) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.703885, 0.710315) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.710315, 0.716745) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.716745, 0.723175) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.723175, 0.729605) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.729605, 0.736035) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.736035, 0.742465] ########## Count: 1 (50.000000%) Total: 2 (100.000000%) Rotational score histogram: Count: 2 Min: 0.880237 Max: 0.939993 Mean: 0.910115 [0.880237, 0.886213) ########## Count: 1 (50.000000%) Total: 1 (50.000000%) [0.886213, 0.892188) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.892188, 0.898164) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.898164, 0.904139) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.904139, 0.910115) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.910115, 0.916090) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.916090, 0.922066) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.922066, 0.928041) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.928041, 0.934017) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.934017, 0.939993] ########## Count: 1 (50.000000%) Total: 2 (100.000000%) Low resolution score histogram: Count: 2 Min: 0.703455 Max: 0.869654 Mean: 0.786554 [0.703455, 0.720075) ########## Count: 1 (50.000000%) Total: 1 (50.000000%) [0.720075, 0.736695) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.736695, 0.753314) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.753314, 0.769934) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.769934, 0.786554) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.786554, 0.803174) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.803174, 0.819794) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.819794, 0.836414) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.836414, 0.853034) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.853034, 0.869654] ########## Count: 1 (50.000000%) Total: 2 (100.000000%) [ INFO] [1548760846.070449619]: I0129 12:20:46.000000 29758 pose_graph_3d.cc:608] Translational residuals histogram: Count: 61 Min: 0.000929 Max: 0.061600 Mean: 0.015248 [0.000929, 0.006996) ###### Count: 18 (29.508196%) Total: 18 (29.508196%) [0.006996, 0.013063) #### Count: 13 (21.311476%) Total: 31 (50.819672%) [0.013063, 0.019130) ### Count: 9 (14.754098%) Total: 40 (65.573769%) [0.019130, 0.025197) #### Count: 13 (21.311476%) Total: 53 (86.885246%) 0.049466, 0.055533) Count: 0 (0.000000%) Total: 60 (98.360657%) [0.055533, 0.061600] Count: 1 (1.639344%) Total: 61 (100.000000%) [ INFO] [1548760846.071406424]: I0129 12:20:46.000000 29758 pose_graph_3d.cc:610 0.049466, 0.055533) Count: 0 (0.000000%) Total: 60 (98.360657%) [0.055533, 0.061600] Count: 1 (1.639344%) Total: 61 (100.000000%) [ INFO] [1548760846.071406424]: I0129 12:20:46.000000 29758 pose_graph_3d.cc:610 [ INFO] [1548760846.071406424]: I0129 12:20:46.000000 29758 pose_graph_3d.cc:610] Rotational residuals histogram: Count: 61 Min: 0.002053 Max: 2.381143 Mean: 1.093419 [0.002053, 0.239962) ### Count: 10 (16.393442%) Total: 10 (16.393442%) [0.239962, 0.477871) #### Count: 11 (18.032787%) Total: 21 (34.426231%) [0.477871, 0.715780) Count: 0 (0.000000%) Total: 21 (34.426231%) [0.715780, 0.953689) ### Count: 10 (16.393442%) Total: 31 (50.819672%) [0.953689, 1.191598) ### Count: 10 (16.393442%) Total: 41 (67.213112%) [1.191598, 1.429507) Count: 0 (0.000000%) Total: 41 (67.213112%) [1.429507, 1.667416) Count: 0 (0.000000%) Total: 41 (67.213112%) [1.667416, 1.905325) Count: 0 (0.000000%) Total: 41 (67.213112%) [1.905325, 2.143234) ### Count: 10 (16.393442%) Total: 51 (83.606560%) [2.143234, 2.381143] ### Count: 10 (16.393442%) Total: 61 (100.000000%) [ INFO] [1548760846.831896360]: I0129 12:20:46.000000 29712 submap_3d.cc:321] Added submap 8 [ INFO] [1548760850.810213134]: I0129 12:20:50.000000 29712 collated_trajectory_builder.cc:72] imu rate: 99.93 Hz 1.00e-02 s +/- 1.70e-03 s (pulsed at 100.02% real time) [ INFO] [1548760850.810364745]: I0129 12:20:50.000000 29712 collated_trajectory_builder.cc:72] odom rate: 14.19 Hz 7.05e-02 s +/- 4.27e-03 s (pulsed at 102.33% real time) [ INFO] [1548760850.810455975]: I0129 12:20:50.000000 29712 collated_trajectory_builder.cc:72] points2 rate: 2.50 Hz 4.00e-01 s +/- 7.03e-05 s (pulsed at 100.00% real time) [ INFO] [1548760850.839743680]: I0129 12:20:50.000000 29712 submap_3d.cc:321] Added submap 9 [ INFO] [1548760853.242562281]: I0129 12:20:53.000000 29761 constraint_builder_3d.cc:311] 1 computations resulted in 0 additional constraints. Score histogram: Count: 2 Min: 0.678166 Max: 0.742465 Mean: 0.710315 [0.678166, 0.684596) ########## Count: 1 (50.000000%) Total: 1 (50.000000%) [0.684596, 0.691026) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.691026, 0.697456) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.697456, 0.703885) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.703885, 0.710315) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.710315, 0.716745) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.716745, 0.723175) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.723175, 0.729605) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.729605, 0.736035) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.736035, 0.742465] ########## Count: 1 (50.000000%) Total: 2 (100.000000%) Rotational score histogram: Count: 2 Min: 0.880237 Max: 0.939993 Mean: 0.910115 [0.880237, 0.886213) ########## Count: 1 (50.000000%) Total: 1 (50.000000%) [0.886213, 0.892188) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.892188, 0.898164) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.898164, 0.904139) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.904139, 0.910115) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.910115, 0.916090) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.916090, 0.922066) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.922066, 0.928041) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.928041, 0.934017) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.934017, 0.939993] ########## Count: 1 (50.000000%) Total: 2 (100.000000%) Low resolution score histogram: Count: 2 Min: 0.703455 Max: 0.869654 Mean: 0.786554 [0.703455, 0.720075) ########## Count: 1 (50.000000%) Total: 1 (50.000000%) [0.720075, 0.736695) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.736695, 0.753314) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.753314, 0.769934) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.769934, 0.786554) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.786554, 0.803174) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.803174, 0.819794) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.819794, 0.836414) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.836414, 0.853034) Count: 0 (0.000000%) Total: 1 (50.000000%) [0.853034, 0.869654] ########## Count: 1 (50.000000%) Total: 2 (100.000000%) ^C[ INFO] [1548760853.289100301]: I0129 12:20:53.000000 29761 pose_graph_3d.cc:608] Translational residuals histogram: Count: 83 Min: 0.000562 Max: 0.061548 Mean: 0.012715 [0.000562, 0.006661) ######## Count: 33 (39.759037%) Total: 33 (39.759037%) [0.006661, 0.012759) #### Count: 17 (20.481928%) Total: 50 (60.240963%) [0.012759, 0.018858) ## Count: 10 (12.048193%) Total: 60 (72.289154%) [0.018858, 0.024957) #### Count: 15 (18.072289%) Total: 75 (90.361443%) 0.049351, 0.055450) Count: 0 (0.000000%) Total: 82 (98.795181%) [0.055450, 0.061548] Count: 1 (1.204819%) Total: 83 (100.000000%) [ INFO] [1548760853.289329734]: I0129 12:20:53.000000 29761 pose_graph_3d.cc:610 0.049351, 0.055450) Count: 0 (0.000000%) Total: 82 (98.795181%) [0.055450, 0.061548] Count: 1 (1.204819%) Total: 83 (100.000000%) [ INFO] [1548760853.289329734]: I0129 12:20:53.000000 29761 pose_graph_3d.cc:610 [ INFO] [1548760853.289329734]: I0129 12:20:53.000000 29761 pose_graph_3d.cc:610] Rotational residuals histogram: Count: 83 Min: 0.003513 Max: 2.875265 Mean: 1.092871 [0.003513, 0.290688) ## Count: 10 (12.048193%) Total: 10 (12.048193%) [0.290688, 0.577863) ## Count: 10 (12.048193%) Total: 20 (24.096386%) [0.577863, 0.865039) # Count: 4 (4.819277%) Total: 24 (28.915663%) [0.865039, 1.152214) ##### Count: 19 (22.891565%) Total: 43 (51.807228%) [1.152214, 1.439389) ####### Count: 30 (36.144577%) Total: 73 (87.951805%) [1.439389, 1.726564) Count: 0 (0.000000%) Total: 73 (87.951805%) [1.726564, 2.013739) Count: 0 (0.000000%) Total: 73 (87.951805%) [2.013739, 2.300915) Count: 0 (0.000000%) Total: 73 (87.951805%) [2.300915, 2.588090) Count: 0 (0.000000%) Total: 73 (87.951805%) [2.588090, 2.875265] ## Count: 10 (12.048193%) Total: 83 (100.000000%)

My urdf:

<robot name="robot">
  <material name="yellow">
      <color rgba="1.0 1.0 0 1" />                            
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

   <link name="base_link"> 
   <visual>
      <geometry>
        <box size="0.597 0.398   0.22"/>
      </geometry>
      <material name="yellow">
         <color rgba="1.0 1.0 0 1" />
      </material>
   </visual>
   <origin  xyz="0.0 0.0 0.0" />
 </link>

<!-- rpy="0.0 0.0 1.5707"-->
    <!--interaction module-->
  <link name="interaction">
    <visual>
      <geometry>
        <box size="0.6 0.4 0.13"/>
      </geometry>
      <material name="orange" >
        <color rgba="1 0.5 0 1" />
      </material>
    </visual>
  </link>
  <joint name="interaction_link" type="prismatic">  
    <parent link="base_link" />
    <child link="interaction" />
    <axis xyz="0 0 1"/>
    <limit effort="1000.0" lower="0.0" upper="0.2" velocity="0.5"/>
    <origin xyz="0 0 0.2" />
  </joint>

  <!--sensor start-->

  <link name="royale_camera_link">
  </link>
  <joint name="royale_camera_link" type="fixed">  
    <parent link="interaction" />
    <child link="royale_camera_link" />
    <origin rpy="0.0 0.0 3.1415" xyz="0.3 0.0 0.05" />
  </joint>

  <link name="imu">
  </link>
  <joint name="imu_link" type="fixed">  
    <parent link="base_link" />
    <child link="imu" />
    <!--origin xyz="-0.002 0.002 0.001"/-->
    <origin xyz="0.0 0.0 0.0"/>
  </joint>

  <!--sensor end-->
</robot>

And my lua:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu",

  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,--0.1
  submap_publish_period_sec = 0.3,--0.1
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-5,
  rangefinder_sampling_ratio = .5,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,

}

-- if point cloud frequency is low e.g. 10 hz then decrease submaps.num_range_data to 5, optimize_every_n_nodes to 30

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 6

TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1 --150

TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0 = 1
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1 = 6
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation_weight = 2e3 -- 15?
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight =  4e3 -- 5?
TRAJECTORY_BUILDER_3D.submaps.high_resolution = .25 --.25
TRAJECTORY_BUILDER_3D.submaps.low_resolution = 0.45 --0.45
TRAJECTORY_BUILDER_3D.submaps.num_range_data = 5 --120

TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.min_num_points = 130 --150
TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_length = 1. --2.
TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range = 12. --15.

TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.min_num_points = 80 --200
TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_length = 2. --4.
TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range = 40. --60.

TRAJECTORY_BUILDER_3D.voxel_filter_size = 0.15 --0.15

POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 4. --5.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 0.8 --1.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.angular_search_window = math.rad(11.) --15.

POSE_GRAPH.global_constraint_search_after_n_seconds = 20. --10.

--POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d.ceres_solver_options.max_num_iterations = 5 --10

POSE_GRAPH.matcher_rotation_weight = 0.3
POSE_GRAPH.optimization_problem.huber_scale = 5e2 -- 5 * 10²
POSE_GRAPH.optimize_every_n_nodes = 10 --Global SLAM | Disable it with 0 --320
POSE_GRAPH.constraint_builder.sampling_ratio = .01 --.03
POSE_GRAPH.global_sampling_ratio =  0.001--0.003
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 9 --10
POSE_GRAPH.constraint_builder.min_score = 0.12 --0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

my tf tree:

rqt

And sometimes I get the warning: "queue too short for velocity" or "dropped range data"

gaschler commented 5 years ago

Please post the text output of the rosbag_validate tool.

MrGitGo commented 5 years ago

https://www.dropbox.com/s/fcg1epbj10m8k50/2019-02-04-08-35-31.bag?dl=0

This is the link to my bag file

Duke-Allen commented 5 years ago

@Semih67 Hello, I am using a 16-lines LIDAR and IMU to build 3D map by cartographer. I tun the parameters and run it with no error, the results are as follows: 2019-02-27 21-56-15 2019-02-27 21-56-43

There is a big gap with the real environment. I think the map is messy. What is the problem of this result? How can I solve it? This is the validate result of my bag:

E0227 22:30:57.092067 19056 rosbag_validate_main.cc:365] Average IMU linear acceleration is 10.6084 m/s^2, expected is [9.5, 10.5] m/s^2. Linear acceleration data should include gravity and be given in m/s^2. I0227 22:30:57.093359 19056 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/IMU" (frame_id: "imu"): Count: 37968 Min: 0.004133 Max: 0.016211 Mean: 0.010000 [0.004133, 0.005340) Count: 2 (0.005268%) Total: 2 (0.005268%) [0.005340, 0.006548) Count: 3 (0.007901%) Total: 5 (0.013169%) [0.006548, 0.007756) Count: 8 (0.021070%) Total: 13 (0.034239%) [0.007756, 0.008964) Count: 828 (2.180784%) Total: 841 (2.215023%) [0.008964, 0.010172) ################### Count: 35958 (94.706070%) Total: 36799 (96.921089%) [0.010172, 0.011380) Count: 345 (0.908660%) Total: 37144 (97.829750%) [0.011380, 0.012588) Count: 820 (2.159714%) Total: 37964 (99.989464%) [0.012588, 0.013796) Count: 1 (0.002634%) Total: 37965 (99.992096%) [0.013796, 0.015004) Count: 0 (0.000000%) Total: 37965 (99.992096%) [0.015004, 0.016211] Count: 3 (0.007901%) Total: 37968 (100.000000%) E0227 22:30:57.094650 19056 rosbag_validate_main.cc:382] Point data (frame_id: "rslidar") has a large gap, largest is 0.101409 s, recommended is [0.0005, 0.05] s with no jitter. I0227 22:30:57.094745 19056 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/rslidar_points" (frame_id: "rslidar"): Count: 3766 Min: 0.100213 Max: 0.101409 Mean: 0.100801 [0.100213, 0.100333) Count: 1 (0.026553%) Total: 1 (0.026553%) [0.100333, 0.100452) Count: 1 (0.026553%) Total: 2 (0.053107%) [0.100452, 0.100572) Count: 9 (0.238980%) Total: 11 (0.292087%) [0.100572, 0.100691) Count: 33 (0.876261%) Total: 44 (1.168348%) [0.100691, 0.100811) ############## Count: 2717 (72.145515%) Total: 2761 (73.313858%) [0.100811, 0.100931) ##### Count: 972 (25.809877%) Total: 3733 (99.123741%) [0.100931, 0.101050) Count: 25 (0.663834%) Total: 3758 (99.787575%) [0.101050, 0.101170) Count: 6 (0.159320%) Total: 3764 (99.946892%) [0.101170, 0.101289) Count: 1 (0.026553%) Total: 3765 (99.973450%) [0.101289, 0.101409] Count: 1 (0.026553%) Total: 3766 (100.000000%)

2019-02-27 22-16-52

Here are my files: demo_rsldiar_3d.launch rslidar_3d.launch mydemo_3d.rviz rslidar_2d.urdf rslidar_scan_3d.lua my data bag

Do you know the problem?

cartographer-project / cartographer_ros

tf jumps a lot #1174