Skip to content

Commit c373769

Browse files
gerni17gerni17
committed
adding the pointcloud package, with class probability as well as dino feature extractor, additionally I added the first tests
1 parent fc14aa9 commit c373769

File tree

21 files changed

+1181
-17
lines changed

21 files changed

+1181
-17
lines changed

‎elevation_mapping_cupy/config/parameters.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ publishers:
8181
basic_layers: ['elevation', 'traversability']
8282
fps: 2.0
8383
elevation_map_filter:
84-
layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb']
84+
layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb','person']
8585
basic_layers: ['min_filter']
8686
fps: 3.0
8787

‎elevation_mapping_cupy/config/sensor_parameter.yaml‎

Lines changed: 23 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,29 @@ subscribers:
44
# fusion: ['average','average']
55
# topic_name: '/elevation_mapping/pointcloud_semantic'
66
front_cam:
7-
channels: ['rgb','feat_0','feat_1','c_prob_0']
7+
channels: ['rgb','feat_0','feat_1','person','grass']
88
fusion: ['color','average','average','class_average']
99
topic_name: '/elvation_mapping/pointcloud_semantic'
10+
semantic_segmentation: True
11+
segmentation_model: 'detectron_coco_panoptic_fpn_R_101_3x'
12+
feature_config:
13+
name: 'DINO'
14+
interpolation: 'bilinear'
15+
model: "vit_small"
16+
patch_size: 16
17+
dim: 5
18+
dropout: False
19+
dino_feat_type: "feat"
20+
input_size: [80, 160]
21+
projection_type: "nonlinear"
22+
23+
cam_info_topic: "/zed2i/zed_node/depth/camera_info"
24+
image_topic: "/zed2i/zed_node/left/image_rect_color"
25+
depth_topic: "/zed2i/zed_node/depth/depth_registered"
26+
cam_frame: "zed2i_right_camera_optical_frame"
27+
confidence_topic: "/zed2i/zed_node/confidence/confidence_map"
28+
confidence_threshold: 10
29+
30+
31+
1032

‎elevation_mapping_cupy/launch/semantic_elevation_lonomy_single.launch‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,9 @@
55
<param name="/use_sim_time" type="bool" value="$(arg use_sim_time)" />
66
<param name="robot_description" command="$(find xacro)/xacro '--inorder' $(arg model)" />
77

8-
<!-- <node pkg="elevation_mapping_cupy" type="pointcloud.py" name="pointcloud" output="screen" >-->
9-
<!-- <param name="/use_sim_time" type="boolean" value="$(arg use_sim_time)" />-->
10-
<!-- <param name="cam_name" type="string" value="semantic_parameters" />-->
11-
<!-- <rosparam command="load" file="$(find elevation_mapping_cupy)/config/semantic_parameters.yaml" />-->
12-
<!-- </node>-->
8+
<node pkg="semantic_pointcloud" type="pointcloud_node.py" name="semantic_pointcloud" args="front_cam" output="screen">
9+
<rosparam command="load" file="$(find elevation_mapping_cupy)/config/sensor_parameter.yaml" />
10+
</node>
1311

1412
<!-- Elevation mapping node -->
1513
<node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen" >

‎elevation_mapping_cupy/rviz/lonomy_single.rviz‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ Panels:
44
Name: Displays
55
Property Tree Widget:
66
Expanded:
7+
- /ElevationMapFilter1
78
- /Image1
89
Splitter Ratio: 0.5768463015556335
910
Tree Height: 428
@@ -54,16 +55,16 @@ Visualization Manager:
5455
Autocompute Intensity Bounds: true
5556
Class: grid_map_rviz_plugin/GridMap
5657
Color: 200; 200; 200
57-
Color Layer: rgb
58-
Color Transformer: ColorLayer
58+
Color Layer: person
59+
Color Transformer: IntensityLayer
5960
Enabled: true
6061
Height Layer: smooth
6162
Height Transformer: GridMapLayer
6263
History Length: 1
6364
Invert Rainbow: false
64-
Max Color: 255; 255; 255
65+
Max Color: 204; 0; 0
6566
Max Intensity: 10
66-
Min Color: 0; 0; 0
67+
Min Color: 52; 101; 164
6768
Min Intensity: 0
6869
Name: ElevationMapFilter
6970
Show Grid Lines: true
@@ -448,9 +449,9 @@ Visualization Manager:
448449
Invert Z Axis: false
449450
Name: Current View
450451
Near Clip Distance: 0.009999999776482582
451-
Pitch: 0.12979847192764282
452+
Pitch: 0.5547983050346375
452453
Target Frame: base_footprint
453-
Yaw: 0.9738447666168213
454+
Yaw: 1.4138503074645996
454455
Saved: ~
455456
Window Geometry:
456457
Displays:

‎elevation_mapping_cupy/script/elevation_mapping_cupy/custom_kernels.py‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -682,7 +682,7 @@ def sum_kernel(
682682
if (valid) {
683683
if (inside) {
684684
for ( int it=0;it<pcl_channels[1];it++){
685-
T feat = p[i * pcl_channels[0] + pcl_chan[it]];
685+
U feat = p[i * pcl_channels[0] + pcl_chan[it]];
686686
atomicAdd(&newmap[get_map_idx(idx, map_lay[it])], feat);
687687
}
688688
}
@@ -715,7 +715,7 @@ def average_kernel(
715715
U cnt = new_elmap[get_map_idx(i, 2)];
716716
if (cnt>0){
717717
for ( int it=0;it<pcl_channels[1];it++){
718-
U feat = newmap[get_map_idx(i, it*3)]/(1*cnt);
718+
U feat = newmap[get_map_idx(i, map_lay[it])]/(1*cnt);
719719
map[get_map_idx(i, map_lay[it])] = feat;
720720
}
721721
}
@@ -748,11 +748,11 @@ def class_average_kernel(
748748
for ( int it=0;it<pcl_channels[1];it++){
749749
U prev_val = map[get_map_idx(i, map_lay[it])];
750750
if (prev_val==0){
751-
U val = newmap[get_map_idx(i, it*3)]/(1*cnt);
751+
U val = newmap[get_map_idx(i, map_lay[it])]/(1*cnt);
752752
map[get_map_idx(i, map_lay[it])] = val;
753753
}
754754
else{
755-
U val = prev_val /2 + newmap[get_map_idx(i, it*3)]/(2*cnt);
755+
U val = prev_val /2 + newmap[get_map_idx(i, map_lay[it])]/(2*cnt);
756756
map[get_map_idx(i, map_lay[it])] = val;
757757
}
758758
}

‎sensor_processing/semantic_pointcloud/CMakeLists.txt‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(semantic_pointcloud)
3+
4+
find_package(PythonInterp 3 REQUIRED)
5+
find_package(PythonLibs 3 REQUIRED)
6+
7+
if(PYTHONLIBS_FOUND)
8+
message(STATUS "Using Python Libraries at: " ${PYTHON_LIBRARIES})
9+
message(STATUS "Using Python include directories at: " ${PYTHON_INCLUDE_DIRS})
10+
else()
11+
message(WARNING "Could not find Python Libraries")
12+
endif()
13+
14+
15+
find_package(catkin REQUIRED COMPONENTS
16+
message_generation
17+
rospy
18+
tf
19+
tf_conversions
20+
sensor_msgs
21+
std_msgs
22+
geometry_msgs
23+
)
24+
25+
catkin_package()
26+
27+
catkin_python_setup()
28+
29+
30+
31+
catkin_install_python(PROGRAMS script/semantic_pointcloud/pointcloud_node.py
32+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
33+

‎sensor_processing/semantic_pointcloud/config/sensor_parameter.yaml‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
subscribers:
2+
# sensor_name:
3+
# channels: ['feat_0','feat_1']
4+
# fusion: ['average','average']
5+
# topic_name: '/elevation_mapping/pointcloud_semantic'
6+
front_cam:
7+
channels: ['rgb','feat_0','feat_1','person','grass']
8+
fusion: ['color','average','average','class_average']
9+
topic_name: '/elvation_mapping/pointcloud_semantic'
10+
semantic_segmentation: True
11+
segmentation_model: 'detectron_coco_panoptic_fpn_R_101_3x'
12+
feature_config:
13+
name: 'DINO'
14+
interpolation: 'bilinear'
15+
model: "vit_small"
16+
patch_size: 16
17+
dim: 5
18+
dropout: False
19+
dino_feat_type: "feat"
20+
input_size: [80, 160]
21+
projection_type: "nonlinear"
22+
23+
cam_info_topic: "/zed2i/zed_node/depth/camera_info"
24+
image_topic: "/zed2i/zed_node/left/image_rect_color"
25+
depth_topic: "/zed2i/zed_node/depth/depth_registered"
26+
cam_frame: "zed2i_right_camera_optical_frame"
27+
confidence_topic: "/zed2i/zed_node/confidence/confidence_map"
28+
confidence_threshold: 10
29+
30+
31+

‎sensor_processing/semantic_pointcloud/launch/semantic_pointcloud.launch‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
2+
<launch>
3+
<!-- Semantic pointcloud node -->
4+
<node pkg="semantic_pointcloud" type="pointcloud_node.py" name="semantic_pointcloud" args="front_cam" output="screen">
5+
<rosparam command="load" file="$(find semantic_pointcloud)/config/sensor_parameter.yaml" />
6+
</node>
7+
</launch>

‎sensor_processing/semantic_pointcloud/package.xml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>semantic_pointcloud</name>
4+
<version>0.0.0</version>
5+
<description>The semantic_pointcloud package</description>
6+
<maintainer email="[email protected]">Gian Erni</maintainer>
7+
8+
9+
10+
<license>MIT</license>
11+
12+
13+
<url type="github">https://github.com/leggedrobotics/elevation_mapping_semantic_cupy</url>
14+
15+
16+
<author email="[email protected]">Gian Erni</author>
17+
18+
<buildtool_depend>catkin</buildtool_depend>
19+
<depend>rospy</depend>
20+
<depend>roslib</depend>
21+
<depend>tf</depend>
22+
<depend>tf_conversions</depend>
23+
<depend>sensor_msgs</depend>
24+
<depend>std_msgs</depend>
25+
<depend>geometry_msgs</depend>
26+
27+
28+
</package>

‎sensor_processing/semantic_pointcloud/script/semantic_pointcloud/DINO/__init__.py‎

Whitespace-only changes.

0 commit comments

Comments
 (0)