fixed .yaml fov

TODO: delete/cleanup padCloud bs. now debugging trellis
unibz-first · Apr 16, 2024 · 4d34452 · mcamurri · Apr 17, 2024 · 4d34452
1 parent 05fb260
commit 4d34452
Show file tree

Hide file tree

Showing 7 changed files with 204 additions and 173 deletions.
diff --git a/sloam/include/segmentation/inference.h b/sloam/include/segmentation/inference.h
@@ -90,6 +90,12 @@ class Segmentation {
 private:
 
     int countValidPoints(const Cloud::Ptr cloud);
+    void _padCloud(const Cloud::Ptr cloud,
+                   cv::Mat mask,
+                   std::set<int> tc_idcs,
+                   unsigned char val,
+                   Cloud::Ptr& tempCloud,
+                   bool organized);
     void _argmax(const float *in, cv::Mat& maskImg);
 
     void _preProcessRange(const cv::Mat& img, cv::Mat& pImg, float maxDist);

diff --git a/sloam/launch/rviz/sloam.rviz b/sloam/launch/rviz/sloam.rviz
@@ -36,7 +36,7 @@ Panels:
   - Class: rviz/Time
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: Image
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -484,25 +484,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 21.493854522705078
+      Distance: 22.796157836914062
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 42.673927307128906
-        Y: 36.22590637207031
-        Z: 1.09340238571167
+        X: 53.42161178588867
+        Y: 33.22412872314453
+        Z: 1.0934549570083618
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7347967624664307
+      Pitch: 0.4897969961166382
       Target Frame: base
-      Yaw: 5.915062427520752
+      Yaw: 5.975069046020508
     Saved:
       - Class: rviz/ThirdPersonFollower
         Distance: 16.295198440551758

diff --git a/sloam/launch/rviz/sloam_digiforest.rviz b/sloam/launch/rviz/sloam_digiforest.rviz
@@ -18,7 +18,7 @@ Panels:
         - /PointCloud23
         - /PointCloud24
       Splitter Ratio: 0.5045871734619141
-    Tree Height: 1113
+    Tree Height: 1063
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -248,7 +248,7 @@ Visualization Manager:
             Shaft Length: 1
             Shaft Radius: 0.05000000074505806
             Value: Axes
-          Topic: /quadrotor/odom
+          Topic: /vilens/odom
           Unreliable: false
           Value: true
         - Alpha: 0.10000000149011612
@@ -534,25 +534,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 12.250869750976562
+      Distance: 6.097325801849365
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -3.6336684226989746
-        Y: 5.238185882568359
-        Z: 1.0933893918991089
+        X: 1.451401710510254
+        Y: 3.0707178115844727
+        Z: 1.0933860540390015
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5297964215278625
+      Pitch: 0.3947966992855072
       Target Frame: base
-      Yaw: 1.850040316581726
+      Yaw: 1.7882027626037598
     Saved:
       - Class: rviz/ThirdPersonFollower
         Distance: 16.295198440551758
@@ -576,13 +576,13 @@ Visualization Manager:
         Yaw: 0.07368306815624237
 Window Geometry:
   Displays:
-    collapsed: true
-  Height: 1376
-  Hide Left Dock: true
+    collapsed: false
+  Height: 1403
+  Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000034200000496fc0200000008fb0000001200530065006c0065006300740069006f006e000000008e000001a00000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000ad00000496000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000154000004e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000075000004e9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000009b80000006afc0100000005fb0000000a0049006d0061006700650100000000000009b80000005e00fffffffb0000000a0049006d0061006700650100000000000009b80000000000000000fb0000000a0049006d0061006700650100000000000007380000000000000000fc00000000000007800000000000fffffffa000000000200000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d006100670065010000003d000000510000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650000000000000007380000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000009b80000049600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000026c00000464fc0200000008fb0000001200530065006c0065006300740069006f006e000000008e000001a00000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000fa00000464000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000154000004e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000075000004e9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000a00000000b7fc0100000005fb0000000a0049006d006100670065010000000000000a000000005e00fffffffb0000000a0049006d0061006700650100000000000009b80000000000000000fb0000000a0049006d0061006700650100000000000007380000000000000000fc00000000000007800000000000fffffffa000000000200000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d006100670065010000003d000000510000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650000000000000007380000041800fffffffb0000000800540069006d006501000000000000045000000000000000000000078e0000046400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -591,6 +591,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2488
-  X: 72
-  Y: 27
+  Width: 2560
+  X: 0
+  Y: 1080
diff --git a/sloam/params/sloam_digiforest.yaml b/sloam/params/sloam_digiforest.yaml
@@ -5,9 +5,9 @@ sloam:
   default_tree_radius: 0.6 # default tree radius if estimated value seems wrong (due to outliers for example)
   max_dist_to_centroid: 0.6 # consider points further than this parameter to the node centroid to be outliers
   max_ground_dist: 20.0 # max distance from sensor for ground point to be considered
-  min_ground_dist: 2.0 # min distance from sensor for ground point to be considered
+  min_ground_dist: 1.0 # min distance from sensor for ground point to be considered
   max_lidar_dist: 25.0 # max distance from sensor for a point to be considered
-  min_landmark_height: 1.0 # if a tree is smaller than this, it will be discarded
+  min_landmark_height: 0.6 # if a tree is smaller than this, it will be discarded
   min_landmark_size: 5 # min number of trellis nodes for a tree
   min_vertex_size: 4 # min required number of point in a trellis node
   min_tree_models: 5 # min number of tree models in observation to perform pose estimation
@@ -29,7 +29,7 @@ sloam:
   robot_frame_id: pandar  # old: base
   # TODO: include sloam/sloam/models/ dir, commit to remote origin unibz-first fork
   seg_model_path: $(env HOME)/Downloads/darknet21pp_hpc.onnx # path to segmentation model
-  seg_lidar_fov: 31 # lidar FOV
+  seg_lidar_fov: 16.0 # lidar FOV
   seg_lidar_w: 2000 # lidar width
   seg_lidar_h: 32 # lidar height
   cloud_topic: /hesai/pandar