No semantic point cloud generated

[frankSARU]

Hello there! I'm a student from Kyutech and I have a problem when running this project with a realsense D435i RGB-D camera. The problem is I cannot get the semantic point cloud. I can get semantic image and normal point cloud like below.

I don't know if octomap_generator problem is relevant with this issue, you can see from picture that I also get only 1 voxel in octomap. And as showed in console, I also got many warnings like this
[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.ERROR: Error in search: [2724.48 468.161 1206.56] is out of OcTree bounds!

Hope someone who meet the same issue can help me work it out!

[codieboomboom]

Hi which type of camera are you running? Do you what is the DepthMapFactor of your camera?
Is it possible to attach a snapshot from rosrun rqt_graph rqt_graph when you try to run the system for reference?

This might be related to this issue #17

[frankSARU]

Hi which type of camera are you running? Do you what is the DepthMapFactor of your camera?
Is it possible to attach a snapshot from rosrun rqt_graph rqt_graph when you try to run the system for reference?

This might be related to this issue #17

Hi! Thanks for your respond and this is my rqt_graph. My camera type is Intel Realsense D435i and I don't know how to check my DepthMapFactor. I've checked issue #17 before but I can't quite understand their solution.

[codieboomboom]

Hi, the rqt_graph looks okay btw! You are very close

I think you may need to declare your own DepthMapFactor in the .yaml file for your camera.
Then you might need to import it inside your semantic_cloud.py file and divide the Depth image by this value.

For how to define the depth map factor, you may refer to the following file from ORB_SLAM3, you may search around for the corresponding DepthMapFactor value for Intel Realsense camera.

For importing it into the python file, you may consult the ROS website.

https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Examples/ROS/ORB_SLAM3/Asus.yaml

[frankSARU]

Thanks, that works. I got a octomap! But I have another problem that my system have no world_frame so I bound the world frame id to /camera_link, that makes octomap cannot generated properly. I'll show you the picture below.
How can I fix this problem? (BTW I also meet same problem in RTAB-Map but RTAB-Map can work as normal)

[codieboomboom]

hi, i think you can check out what frame your realsense camera is generating (roscd realsense2_camera maybe?). Also check the source code, especially the Tracking.cc and also those .yaml files?

I had the same problem but forgot how to resolved it, the last i remember was modifying one of those above file

[frankSARU]

hi, i think you can check out what frame your realsense camera is generating (roscd realsense2_camera maybe?). Also check the source code, especially the Tracking.cc and also those .yaml files?

I had the same problem but forgot how to resolved it, the last i remember was modifying one of those above file

Ok, thank you. It seems like there is nothing generate a frame like "/map" or "/world" so the frame is fixed with camera_link which makes camera cannot move. In RTAB-Map, the launch file generated a /map frame so camera_link can move around. I am not very familiar with C++ so maybe I should ask somebody to help me with the source codes.

[codieboomboom]

You can try to explore the launch file for the realsense camera. I don't think you need to modify any source code for this.
You're welcome!

[jason-yspjf]

Hello, I have no idea about how to run this project with a realsense D435, and luckily I saw your issue. Could you please tell me what I should do and how to modify the code,such as the .yaml file. Hope for your reply, many thanks! @frankSARU

[try-it-to]

Thanks, that works. I got a octomap! But I have another problem that my system have no world_frame so I bound the world frame id to /camera_link, that makes octomap cannot generated properly. I'll show you the picture below.
How can I fix this problem? (BTW I also meet same problem in RTAB-Map but RTAB-Map can work as normal)

Hello! Can you give me your semantic_cloud.py file?Thanks a lot !

[frankSARU]

Hello, I have no idea about how to run this project with a realsense D435, and luckily I saw your issue. Could you please tell me what I should do and how to modify the code,such as the .yaml file. Hope for your reply, many thanks! @frankSARU

Hello! I just checked the issue #17 and found that we need to change 2 lines in semantic_slam/semantic_cloud/include/color_pcl_generator/color_pcl_generator.py, look at the line 118-119, the value of depth_img.reshape(-1,1) needs to be devided by DepthFactor of D435. After I check on my calibration file, the DepthFactor of D435i is 1000.
So you need to change line 118-119 like below:
self.xyd_vect[:,0:2] = self.xy_index * depth_img.reshape(-1,1)/1000
self.xyd_vect[:,2:3] = depth_img.reshape(-1,1)/1000
That maybe works for you.

[try-it-to]

Hello, I have no idea about how to run this project with a realsense D435, and luckily I saw your issue. Could you please tell me what I should do and how to modify the code,such as the .yaml file. Hope for your reply, many thanks! @frankSARU

Hello! I just checked the issue #17 and found that we need to change 2 lines in semantic_slam/semantic_cloud/include/color_pcl_generator/color_pcl_generator.py, look at the line 118-119, the value of depth_img.reshape(-1,1) needs to be devided by DepthFactor of D435. After I check on my calibration file, the DepthFactor of D435i is 1000.
So you need to change line 118-119 like below:
self.xyd_vect[:,0:2] = self.xy_index * depth_img.reshape(-1,1)/1000
self.xyd_vect[:,2:3] = depth_img.reshape(-1,1)/1000
That maybe works for you.

Thanks a lot!

[jason-yspjf]

Hello, I have no idea about how to run this project with a realsense D435, and luckily I saw your issue. Could you please tell me what I should do and how to modify the code,such as the .yaml file. Hope for your reply, many thanks! @frankSARU

Hello! I just checked the issue #17 and found that we need to change 2 lines in semantic_slam/semantic_cloud/include/color_pcl_generator/color_pcl_generator.py, look at the line 118-119, the value of depth_img.reshape(-1,1) needs to be devided by DepthFactor of D435. After I check on my calibration file, the DepthFactor of D435i is 1000.
So you need to change line 118-119 like below:
self.xyd_vect[:,0:2] = self.xy_index * depth_img.reshape(-1,1)/1000
self.xyd_vect[:,2:3] = depth_img.reshape(-1,1)/1000
That maybe works for you.

Thanks a lot! But actually I am facing the problem that when I run "roslaunch semantic_slam camera.launch" with D435, it comes out [ INFO] [1611812367.601686804]: No matching device found.... waiting for devices. Reason: std::__cxx11::string openni2_wrapper::OpenNI2Driver::resolveDeviceURI(const string&) @ /tmp/binarydeb/ros-kinetic-openni2-camera-0.4.2/src/openni2_driver.cpp @ 737 : Invalid device number 1, there are 0 devices connected and nothing shows in rviz. Does it mean that I need to modify the launch file?

[frankSARU]

Hello, I have no idea about how to run this project with a realsense D435, and luckily I saw your issue. Could you please tell me what I should do and how to modify the code,such as the .yaml file. Hope for your reply, many thanks! @frankSARU

Hello! I just checked the issue #17 and found that we need to change 2 lines in semantic_slam/semantic_cloud/include/color_pcl_generator/color_pcl_generator.py, look at the line 118-119, the value of depth_img.reshape(-1,1) needs to be devided by DepthFactor of D435. After I check on my calibration file, the DepthFactor of D435i is 1000.
So you need to change line 118-119 like below:
self.xyd_vect[:,0:2] = self.xy_index * depth_img.reshape(-1,1)/1000
self.xyd_vect[:,2:3] = depth_img.reshape(-1,1)/1000
That maybe works for you.

Thanks a lot! But actually I am facing the problem that when I run "roslaunch semantic_slam camera.launch" with D435, it comes out [ INFO] [1611812367.601686804]: No matching device found.... waiting for devices. Reason: std::__cxx11::string openni2_wrapper::OpenNI2Driver::resolveDeviceURI(const string&) @ /tmp/binarydeb/ros-kinetic-openni2-camera-0.4.2/src/openni2_driver.cpp @ 737 : Invalid device number 1, there are 0 devices connected and nothing shows in rviz. Does it mean that I need to modify the launch file?

I see. That's because the original camera.launch is for openni2 supported cameras, for realsense, you need to modify to your own camera launch file. Please check your realsense2_camera package and use rs_rgbd.launch file.
You can launch this file straightly or just include this launch file to your semantic_slam/launch/camera.launch.

[jason-yspjf]

Hello, I have no idea about how to run this project with a realsense D435, and luckily I saw your issue. Could you please tell me what I should do and how to modify the code,such as the .yaml file. Hope for your reply, many thanks! @frankSARU

Hello! I just checked the issue #17 and found that we need to change 2 lines in semantic_slam/semantic_cloud/include/color_pcl_generator/color_pcl_generator.py, look at the line 118-119, the value of depth_img.reshape(-1,1) needs to be devided by DepthFactor of D435. After I check on my calibration file, the DepthFactor of D435i is 1000.
So you need to change line 118-119 like below:
self.xyd_vect[:,0:2] = self.xy_index * depth_img.reshape(-1,1)/1000
self.xyd_vect[:,2:3] = depth_img.reshape(-1,1)/1000
That maybe works for you.

Thanks a lot! But actually I am facing the problem that when I run "roslaunch semantic_slam camera.launch" with D435, it comes out [ INFO] [1611812367.601686804]: No matching device found.... waiting for devices. Reason: std::__cxx11::string openni2_wrapper::OpenNI2Driver::resolveDeviceURI(const string&) @ /tmp/binarydeb/ros-kinetic-openni2-camera-0.4.2/src/openni2_driver.cpp @ 737 : Invalid device number 1, there are 0 devices connected and nothing shows in rviz. Does it mean that I need to modify the launch file?

I see. That's because the original camera.launch is for openni2 supported cameras, for realsense, you need to modify to your own camera launch file. Please check your realsense2_camera package and use rs_rgbd.launch file.
You can launch this file straightly or just include this launch file to your semantic_slam/launch/camera.launch.

Thanks a lot! With your tips, I can now successfully run camera.launch and slam.launch with D435, but another problem occurs, there is still nothing in rviz except the raw rgb images like below


rviz doesn't show the semantic-image and the octomap as well. It seems I choose the wrong topic lists, but when I change them, nothing else happens. do you have any idea and please give me some advice.

[jason-yspjf]

[jason-yspjf]

And this is my rosgraph

It seems the biggest difference from yours is that /camera/depth and /camera/color don't link to /semantic-cloud but to /rgbd? I'm really not familiar with ros and don't know whether my problem is related to this. Hope for your reply!

[germal]

Hey @AnhTuDo1998 and @frankSARU
Maybe did you try or manage to make the semantic pointcloud work using the rtabmap rgbd map ( i mean replacing with rtabmap the Orb2 rgbd map ) ?
I tried a very basic approach putting the semantic topic and the mapcloud in rviz but it crashes
Thanks for your help,
Regards

[codieboomboom]

Hi @germal, were your topics published? like can show in rostopic hz /topic ? Can this run on its own (like no octomap generation node?)

How about your system's resource usage ?

[germal]

Hello @AnhTuDo1998 and thanks for your reply !
Yes the topics are published but with a slightly different frequency

rostopic hz /semantic_pcl/semantic_pcl

average rate: 0.204
min: 4.559s max: 6.710s std dev: 0.32935s window: 88
rostopic hz /rtabmap/cloud_map
average rate: 0.984
min: 0.361s max: 1.641s std dev: 0.16457s window: 415
rostopic hz /octomap_full
average rate: 0.254
min: 3.031s max: 4.853s std dev: 0.91060s window:

The ROS master system is relatively "under stress" , but rviz subscribes on a remotely connected ROS client laptop with ram 16Gb.

top - 09:13:11 up 26 min, 11 users, load average: 8,10, 7,44, 4,77
Tasks: 317 total, 3 running, 314 sleeping, 0 stopped, 0 zombie
%Cpu(s): 66,0 us, 23,0 sy, 0,0 ni, 5,9 id, 0,0 wa, 4,4 hi, 0,8 si, 0,0 st
KiB Mem : 4059272 total, 143036 free, 3649484 used, 266752 buff/cache
KiB Swap: 4095984 total, 1518308 free, 2577676 used. 235248 avail Mem

PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND
8945 mluser 20 0 1425408 21868 6612 R 87,6 0,5 13:51.08 rgbd
9150 mluser 20 0 736632 141296 6868 R 73,0 3,5 8:07.73 octomap_generat
8181 mluser 20 0 10,341g 42116 11364 S 49,6 1,0 10:48.97 nodelet
9149 mluser 20 0 12,211g 458184 295000 S 46,9 11,3 8:18.24 python2.7
9701 mluser 20 0 2678828 182612 42988 S 39,8 4,5 4:10.60 rtabmap

I would use the mapdata of Rtabmap and the semantic data of semantic_slam to create a map but they can only appear in 2 different rviz instances. When I try to subscribe them in the same rviz instance , rviz crashes immediately.
The topic that triggers the crash is /octomap_full published under ColorOccupancyGrid plugin ( not the /semantic_pcl/semantic_pcl )
The rviz file that I use is the one that I've found on this repo .
Do you find any solution to fuse rtabmap and semantic cloud , I mean how to feed the slam mapping data of rtabmap in the octomap generator ?
Thanks a lot in advance !

[wk524629918]

hi, i think you can check out what frame your realsense camera is generating (roscd realsense2_camera maybe?). Also check the source code, especially the Tracking.cc and also those .yaml files?
I had the same problem but forgot how to resolved it, the last i remember was modifying one of those above file

Ok, thank you. It seems like there is nothing generate a frame like "/map" or "/world" so the frame is fixed with camera_link which makes camera cannot move. In RTAB-Map, the launch file generated a /map frame so camera_link can move around. I am not very familiar with C++ so maybe I should ask somebody to help me with the source codes.

Hi, I had the same problem about no world_frame. Have you solved the problem?

[shaheer34mts]

anybody found the solution to no "world" frame issue ?
i am getting this error message:

[ WARN] [1620043419.754639065]: MessageFilter [target=/world ]: Dropped 100.00% of messages so far. Please turn the [ros.octomap_generator.message_filter] rosconsole logger to DEBUG for more information

[codieboomboom]

@shaheer34mts try renaming "world" to "/world" in the octomap_generator.yaml file. Let me know if this fixed the problem?

[shaheer34mts]

@shaheer34mts try renaming "world" to "/world" in the octomap_generator.yaml file. Let me know if this fixed the problem?

Hi @AnhTuDo1998 It does not solve the problem, however, if I set fixed frame to "/camera_link" then I can see the octomap generated (as can bee seen in the attached picture). But since my camer_link is now fixed link, it does not move and I cannot generate a proper map. and as you can see in the picture, it still shows an error that it cannot find transform between /camera_link and /world. For some reason I am not able to generate a /world frame. Can you kindly tell me how to generate it?
Secondly, I am NOT running ORB_SLAM3 node, i just run the realsense camera node to publish rgb and depth stream, and then I launch "semantic_mapping.launch" file to generate the octomap. It still generates a (faulty) octomap.
Even if I run ORB_SLAM3 node seperately, it does not make any difference.
Kindly guide me how to move forward.
Thanks in advance.

[codieboomboom]

@shaheer34mts would it be possible for you to print out a /tf tree using the following commands as reference? Please show the output here.

rosrun tf view_frames
evince frames.pdf

May I know exactly the model of the camera u are using?

[shaheer34mts]

@AnhTuDo1998 I am trying with both D435 and D435i. The attached file is for D435
frames.pdf

[codieboomboom]

@shaheer34mts This is without semantic_mapping node running ?

[shaheer34mts]

@AnhTuDo1998 semantic_mapping is running, but SLAM node is not running.

[codieboomboom]

Would it possible to share with me how you are launching the camera? if possible please show an extract of the launch file and any nodelet file you are using to run the camera here. To my best memory, this might be a problem of camera transformation frame (Realsense might not have what the author said to be /camera_rgb_optical_frame in its default configuration of the launch file)

[frankSARU]

@shaheer34mts try renaming "world" to "/world" in the octomap_generator.yaml file. Let me know if this fixed the problem?

Hi @AnhTuDo1998 It does not solve the problem, however, if I set fixed frame to "/camera_link" then I can see the octomap generated (as can bee seen in the attached picture). But since my camer_link is now fixed link, it does not move and I cannot generate a proper map. and as you can see in the picture, it still shows an error that it cannot find transform between /camera_link and /world. For some reason I am not able to generate a /world frame. Can you kindly tell me how to generate it? Secondly, I am NOT running ORB_SLAM3 node, i just run the realsense camera node to publish rgb and depth stream, and then I launch "semantic_mapping.launch" file to generate the octomap. It still generates a (faulty) octomap. Even if I run ORB_SLAM3 node seperately, it does not make any difference. Kindly guide me how to move forward. Thanks in advance.

In fact I am having a similar problem to yours, I am also using a D435 camera, have you made any progress?

[frankSARU]

@shaheer34mts try renaming "world" to "/world" in the octomap_generator.yaml file. Let me know if this fixed the problem?

My problem solved with this method! Thank you! Now I can create true semantic octomap, here's the result.
(My room is messy, sorry ; )

(I run this project with rtabmap so I have colored octomap & binary map together, if you run this project purely, you will only get semantic map)

[YvonneZhao0305]

Hello @AnhTuDo1998 and thanks for your reply ! Yes the topics are published but with a slightly different frequency

rostopic hz /semantic_pcl/semantic_pcl

average rate: 0.204 min: 4.559s max: 6.710s std dev: 0.32935s window: 88 rostopic hz /rtabmap/cloud_map average rate: 0.984 min: 0.361s max: 1.641s std dev: 0.16457s window: 415 rostopic hz /octomap_full average rate: 0.254 min: 3.031s max: 4.853s std dev: 0.91060s window:

The ROS master system is relatively "under stress" , but rviz subscribes on a remotely connected ROS client laptop with ram 16Gb.

top - 09:13:11 up 26 min, 11 users, load average: 8,10, 7,44, 4,77 Tasks: 317 total, 3 running, 314 sleeping, 0 stopped, 0 zombie %Cpu(s): 66,0 us, 23,0 sy, 0,0 ni, 5,9 id, 0,0 wa, 4,4 hi, 0,8 si, 0,0 st KiB Mem : 4059272 total, 143036 free, 3649484 used, 266752 buff/cache KiB Swap: 4095984 total, 1518308 free, 2577676 used. 235248 avail Mem

PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND 8945 mluser 20 0 1425408 21868 6612 R 87,6 0,5 13:51.08 rgbd 9150 mluser 20 0 736632 141296 6868 R 73,0 3,5 8:07.73 octomap_generat 8181 mluser 20 0 10,341g 42116 11364 S 49,6 1,0 10:48.97 nodelet 9149 mluser 20 0 12,211g 458184 295000 S 46,9 11,3 8:18.24 python2.7 9701 mluser 20 0 2678828 182612 42988 S 39,8 4,5 4:10.60 rtabmap

I would use the mapdata of Rtabmap and the semantic data of semantic_slam to create a map but they can only appear in 2 different rviz instances. When I try to subscribe them in the same rviz instance , rviz crashes immediately. The topic that triggers the crash is /octomap_full published under ColorOccupancyGrid plugin ( not the /semantic_pcl/semantic_pcl ) The rviz file that I use is the one that I've found on this repo . Do you find any solution to fuse rtabmap and semantic cloud , I mean how to feed the slam mapping data of rtabmap in the octomap generator ? Thanks a lot in advance !

I have the same problem, I don't know how to add semantic information(semantic pointclouds) to rtabmap.

[chijie1998]

Hello @AnhTuDo1998 and thanks for your reply ! Yes the topics are published but with a slightly different frequency
rostopic hz /semantic_pcl/semantic_pcl
average rate: 0.204 min: 4.559s max: 6.710s std dev: 0.32935s window: 88 rostopic hz /rtabmap/cloud_map average rate: 0.984 min: 0.361s max: 1.641s std dev: 0.16457s window: 415 rostopic hz /octomap_full average rate: 0.254 min: 3.031s max: 4.853s std dev: 0.91060s window:
The ROS master system is relatively "under stress" , but rviz subscribes on a remotely connected ROS client laptop with ram 16Gb.
top - 09:13:11 up 26 min, 11 users, load average: 8,10, 7,44, 4,77 Tasks: 317 total, 3 running, 314 sleeping, 0 stopped, 0 zombie %Cpu(s): 66,0 us, 23,0 sy, 0,0 ni, 5,9 id, 0,0 wa, 4,4 hi, 0,8 si, 0,0 st KiB Mem : 4059272 total, 143036 free, 3649484 used, 266752 buff/cache KiB Swap: 4095984 total, 1518308 free, 2577676 used. 235248 avail Mem
PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND 8945 mluser 20 0 1425408 21868 6612 R 87,6 0,5 13:51.08 rgbd 9150 mluser 20 0 736632 141296 6868 R 73,0 3,5 8:07.73 octomap_generat 8181 mluser 20 0 10,341g 42116 11364 S 49,6 1,0 10:48.97 nodelet 9149 mluser 20 0 12,211g 458184 295000 S 46,9 11,3 8:18.24 python2.7 9701 mluser 20 0 2678828 182612 42988 S 39,8 4,5 4:10.60 rtabmap
I would use the mapdata of Rtabmap and the semantic data of semantic_slam to create a map but they can only appear in 2 different rviz instances. When I try to subscribe them in the same rviz instance , rviz crashes immediately. The topic that triggers the crash is /octomap_full published under ColorOccupancyGrid plugin ( not the /semantic_pcl/semantic_pcl ) The rviz file that I use is the one that I've found on this repo . Do you find any solution to fuse rtabmap and semantic cloud , I mean how to feed the slam mapping data of rtabmap in the octomap generator ? Thanks a lot in advance !

I have the same problem, I don't know how to add semantic information(semantic pointclouds) to rtabmap.

I have successfully added semantic information to rtabmap, but not using semantic pointclouds. Instead I remap the rtabmap rgb input to semantic image output from the node. I also created a synced camera info for the semantic image and depth image.

The problem that causes the semantic pointclouds does not works in rtabmap might be due to the extra information/fields in /semantic_pcl/semantic_pcl such as confidences levels.

To save yourself from all these trouble, I would suggest to stay away from this package and just use rtabmap instead if you want to customize with your own camera since rtabmap have its built in SLAM and octomap generator. Any image segmentation node will do the job :) However, this packages does have its advantages such as the recoloring the points based on the confidences levels. I believe that rtabmap cant do this.

Also on how to feed the slam mapping from rtabmap into this package octomap generator, just change the world_frame_id in octomap_generator.yaml to "map". The octomap generator uses tf listener to read the odom from world frame (map) while the rtabmap publish the tf using slam to frame "map".

[thanhnguyencanh]

In fact I am having a similar problem to yours, I am also using a D435 camera, have you made any progress?

[jolags313]

Hello @frankSARU @codieboomboom ; I am also trying to run this on a D435i camera, but I have been having some trouble. When I run roslaunch semantic_slam slam.launch, the process just dies immediately, and I get the following output

SUMMARY
========

PARAMETERS
 * /rosdistro: noetic
 * /rosversion: 1.15.15

NODES
  /
    rgbd (orb_slam2/rgbd)

ROS_MASTER_URI=http://localhost:11311

process[rgbd-1]: started with pid [23023]

ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: RGB-D
[rgbd-1] process has died [pid 23023, exit code -11, cmd /home/conlab/ss_ws/devel/lib/orb_slam2/rgbd /home/conlab/ss_ws/src/semantic_slam/ORB_SLAM2/Examples/ROS/ORB_SLAM2/../../../Vocabulary/ORBvoc.txt /home/conlab/ss_ws/src/semantic_slam/semantic_slam/params/d435i.yaml __name:=rgbd __log:=/home/conlab/.ros/log/87c1673a-c154-11ee-a59a-01f5218c67b6/rgbd-1.log].
log file: /home/conlab/.ros/log/87c1673a-c154-11ee-a59a-01f5218c67b6/rgbd-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

I am using the rs_rgbd.launch file, and I changed the semantic_cloud.yaml parameters to

semantic_pcl:
  color_image_topic: "/camera/color/image_raw"
  depth_image_topic: "/camera/depth_registered/points" 

I also made a settings file d435i.yaml to use, which I reference in slam.launch. I believe everything is set up correctly, but it isn't working. Could you please tell me how you were able to get it working?