What can I do once it works?

OdedHorowits commented 3 months ago

Hi!

So, after making it work comes the real question - How can use the results of the algorithm? Lets say I move my drone around and map the area. How can I use that map? Can I use it on-the-fly for navigation?

In details: The ORB_SLAM3 app publishes into to topics:

ros2 topic echo /drone0/map_data
ros2 topic echo /drone0/map_points

ros2 topic info /drone0/map_data says it uses slam_msgs/msg/MapData and ros2 topic info /drone0/map_points says it uses sensor_msgs/msg/PointCloud2

Trying to echo /drone0/map_points gave me nothing. I though that the reason is the msg is too big, but using rviz I get nothing also when adding a PointCloud that subscribes to this topic. Echoing slam_msgs/msg/MapData however gives me some results. The structure of the msg is a following:

std_msgs/Header header
    builtin_interfaces/Time stamp
        int32 sec
        uint32 nanosec
    string frame_id

#optimized graph
slam_msgs/MapGraph graph
    std_msgs/Header header
        builtin_interfaces/Time stamp
            int32 sec
            uint32 nanosec
        string frame_id
    int32[] poses_id
    geometry_msgs/PoseStamped[] poses
        std_msgs/Header header
            builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
            string frame_id
        Pose pose
            Point position
                float64 x
                float64 y
                float64 z
            Quaternion orientation
                float64 x 0
                float64 y 0
                float64 z 0
                float64 w 1

#graph data
slam_msgs/KeyFrame[] nodes
    int32 id
    geometry_msgs/Point[] word_pts
        float64 x
        float64 y
        float64 z

Using a python subscriber I can extract the results, that looks like this:

====================================================================

[INFO] [1719309314.196997704] [map_data_subscriber]: Graph Header:
[INFO] [1719309314.197535030] [map_data_subscriber]: Frame ID: map
[INFO] [1719309314.198119681] [map_data_subscriber]: Timestamp: 0s 0ns

[INFO] [1719309314.198958030] [map_data_subscriber]: Optimized Graph:
[INFO] [1719309314.199700617] [map_data_subscriber]:   Frame ID: 
[INFO] [1719309314.200628111] [map_data_subscriber]:   Timestamp: 0s 0ns
[INFO] [1719309314.201512677] [map_data_subscriber]: Poses ID: [0, 4, 1, 2, 3, 14, 38, 48, 62, 65, 67, 68, 73, 90, 84, 86, 70, 91, 89, 87, 88, 81]
[INFO] [1719309314.202609746] [map_data_subscriber]: Poses:
[INFO] [1719309314.203258511] [map_data_subscriber]:   Pose:
[INFO] [1719309314.203961693] [map_data_subscriber]:     Position: x=0.000, y=0.000, z=0.000
[INFO] [1719309314.205278722] [map_data_subscriber]:     Orientation: x=0.000, y=0.000, z=0.000, w=1.000
[INFO] [1719309314.206490162] [map_data_subscriber]:   Pose:
[INFO] [1719309314.209347471] [map_data_subscriber]:     Position: x=-0.013, y=0.002, z=2.565
[INFO] [1719309314.211288744] [map_data_subscriber]:     Orientation: x=0.002, y=-0.034, z=0.000, w=0.999
[INFO] [1719309314.212159562] [map_data_subscriber]:   Pose:
[INFO] [1719309314.212974634] [map_data_subscriber]:     Position: x=0.011, y=-0.000, z=1.097
[INFO] [1719309314.213891018] [map_data_subscriber]:     Orientation: x=0.003, y=-0.003, z=0.001, w=1.000
[INFO] [1719309314.214714715] [map_data_subscriber]:   Pose:
[INFO] [1719309314.215618662] [map_data_subscriber]:     Position: x=0.010, y=0.001, z=1.138
[INFO] [1719309314.216503208] [map_data_subscriber]:     Orientation: x=0.004, y=0.024, z=0.001, w=1.000
[INFO] [1719309314.217514307] [map_data_subscriber]:   Pose:
[INFO] [1719309314.218166416] [map_data_subscriber]:     Position: x=0.006, y=-0.005, z=1.731
[INFO] [1719309314.218971740] [map_data_subscriber]:     Orientation: x=-0.001, y=-0.032, z=0.001, w=1.000
[INFO] [1719309314.219560311] [map_data_subscriber]:   Pose:
[INFO] [1719309314.220058909] [map_data_subscriber]:     Position: x=-0.012, y=-0.019, z=3.652
[INFO] [1719309314.220551867] [map_data_subscriber]:     Orientation: x=-0.003, y=-0.037, z=0.002, w=0.999
[INFO] [1719309314.221097756] [map_data_subscriber]:   Pose:
[INFO] [1719309314.221565057] [map_data_subscriber]:     Position: x=-0.014, y=-0.002, z=3.654
[INFO] [1719309314.222104117] [map_data_subscriber]:     Orientation: x=-0.000, y=-0.026, z=0.001, w=1.000
[INFO] [1719309314.222826841] [map_data_subscriber]:   Pose:
[INFO] [1719309314.223358593] [map_data_subscriber]:     Position: x=-0.001, y=-0.032, z=3.603
[INFO] [1719309314.223897022] [map_data_subscriber]:     Orientation: x=0.001, y=0.029, z=0.000, w=1.000
[INFO] [1719309314.224433228] [map_data_subscriber]:   Pose:
[INFO] [1719309314.224994473] [map_data_subscriber]:     Position: x=-0.010, y=-0.024, z=3.634
[INFO] [1719309314.225489760] [map_data_subscriber]:     Orientation: x=0.002, y=-0.026, z=0.002, w=1.000
[INFO] [1719309314.226024291] [map_data_subscriber]:   Pose:
[INFO] [1719309314.226543916] [map_data_subscriber]:     Position: x=-0.029, y=0.005, z=3.665
[INFO] [1719309314.227078249] [map_data_subscriber]:     Orientation: x=0.006, y=-0.036, z=0.001, w=0.999
[INFO] [1719309314.227604373] [map_data_subscriber]:   Pose:
[INFO] [1719309314.228108954] [map_data_subscriber]:     Position: x=-0.028, y=-0.014, z=3.615
[INFO] [1719309314.228618967] [map_data_subscriber]:     Orientation: x=0.001, y=0.029, z=0.001, w=1.000
[INFO] [1719309314.229134493] [map_data_subscriber]:   Pose:
[INFO] [1719309314.229637485] [map_data_subscriber]:     Position: x=-0.008, y=-0.004, z=3.639
[INFO] [1719309314.230144051] [map_data_subscriber]:     Orientation: x=0.004, y=-0.029, z=0.002, w=1.000
[INFO] [1719309314.230628378] [map_data_subscriber]:   Pose:
[INFO] [1719309314.231133720] [map_data_subscriber]:     Position: x=-0.016, y=-0.002, z=3.632
[INFO] [1719309314.231777964] [map_data_subscriber]:     Orientation: x=0.002, y=-0.005, z=0.001, w=1.000
[INFO] [1719309314.232584568] [map_data_subscriber]:   Pose:
[INFO] [1719309314.233731438] [map_data_subscriber]:     Position: x=0.030, y=0.162, z=3.967
[INFO] [1719309314.234434491] [map_data_subscriber]:     Orientation: x=0.001, y=-0.019, z=-0.034, w=0.999
[INFO] [1719309314.234955198] [map_data_subscriber]:   Pose:
[INFO] [1719309314.235682070] [map_data_subscriber]:     Position: x=-0.038, y=-0.013, z=3.615
[INFO] [1719309314.236223849] [map_data_subscriber]:     Orientation: x=0.000, y=0.029, z=0.002, w=1.000
[INFO] [1719309314.236736824] [map_data_subscriber]:   Pose:
[INFO] [1719309314.237227303] [map_data_subscriber]:     Position: x=-0.020, y=-0.026, z=3.625
[INFO] [1719309314.237733662] [map_data_subscriber]:     Orientation: x=0.003, y=0.016, z=0.002, w=1.000
[INFO] [1719309314.240603471] [map_data_subscriber]:   Pose:
[INFO] [1719309314.242165247] [map_data_subscriber]:     Position: x=-0.034, y=-0.016, z=3.658
[INFO] [1719309314.243488185] [map_data_subscriber]:     Orientation: x=0.001, y=-0.022, z=0.001, w=1.000
[INFO] [1719309314.244487064] [map_data_subscriber]:   Pose:
[INFO] [1719309314.245269381] [map_data_subscriber]:     Position: x=0.027, y=-0.146, z=4.199
[INFO] [1719309314.246037474] [map_data_subscriber]:     Orientation: x=0.008, y=0.029, z=0.029, w=0.999
[INFO] [1719309314.246825802] [map_data_subscriber]:   Pose:
[INFO] [1719309314.247628231] [map_data_subscriber]:     Position: x=0.043, y=-0.192, z=4.195
[INFO] [1719309314.248443807] [map_data_subscriber]:     Orientation: x=0.002, y=0.023, z=0.032, w=0.999
[INFO] [1719309314.249202274] [map_data_subscriber]:   Pose:
[INFO] [1719309314.250213227] [map_data_subscriber]:     Position: x=0.004, y=-0.009, z=3.662
[INFO] [1719309314.251156173] [map_data_subscriber]:     Orientation: x=0.002, y=-0.025, z=0.001, w=1.000
[INFO] [1719309314.252336507] [map_data_subscriber]:   Pose:
[INFO] [1719309314.253064779] [map_data_subscriber]:     Position: x=0.011, y=-0.009, z=3.631
[INFO] [1719309314.253804713] [map_data_subscriber]:     Orientation: x=0.003, y=0.001, z=0.001, w=1.000
[INFO] [1719309314.254643644] [map_data_subscriber]:   Pose:
[INFO] [1719309314.255357690] [map_data_subscriber]:     Position: x=-0.013, y=-0.009, z=3.638
[INFO] [1719309314.256078394] [map_data_subscriber]:     Orientation: x=0.002, y=-0.020, z=0.002, w=1.000

[INFO] [1719309314.256807611] [map_data_subscriber]: Keyframes:

====================================================================

And some questions: why do the optimized graph have no Frame ID? why there are no keyframes?

Thanks

suchetanrs commented 3 months ago

Hi! Thanks for the new issue. No, ORB-SLAM3 cannot directly be used for navigation on the fly. Simply because it doesn't provide any 2D/3D occupancy map. What you can get out of this is an accurate localisation estimate and the optimised keyframe poses. These poses are the previously traversed areas of the robot. You'll need a separate mapping algorithm if you want to do this and later feed that map to a navigation stack.

OdedHorowits commented 3 months ago

Thank for your response. I edited the former post, plz check it. mainly, I ask -

Why do the optimized graph have no Frame ID?
Why there are no keyframes?
Why can be the problem with the PointCloud?

And, if I start flying around, would the poses array keep grow? Does it store all the previously traversed areas?

suchetanrs commented 3 months ago

Hi, So /drone0/map_points will have messages only if the ros_visualization parameter is set to true in the params file. This is still under development, I have not been able to test it.

To answer your questions.

The optimized graph poses don't have a frame ID because they are defined in the global frame. In this case, the map frame.
They keyframes are empty because I did not want to populate the tracked points for each keyframe. Maybe I should parametrize this, please feel free to let me know if you want to do this, maybe a raise PR for that if it works. However, the poses you can see from your python file are in fact, the keyframe poses.
Like I mentioned earlier, the problem could be the ros_visualization parameter. If it still does not show up with the value being true, I will need more information before proposing a solution.

OdedHorowits commented 3 months ago

Thank you so much for taking the time for thinking and answering. Regarding the last question - if I start flying around, would the poses array keep grow? does it store all the previously traversed areas?

suchetanrs commented 3 months ago

Yes, you are right. The pose array will keep growing as long as the SLAM is running and it doesn't lose tracking. Yes, it is the previously traversed areas.

OdedHorowits commented 2 months ago

Hi Does the behavior shown here is ok? doesn't seem so... Screenshot from 2024-07-15 12-05-28

OdedHorowits commented 2 months ago

Hi! Thanks for the new issue. No, ORB-SLAM3 cannot directly be used for navigation on the fly. Simply because it doesn't provide any 2D/3D occupancy map. What you can get out of this is an accurate localisation estimate and the optimised keyframe poses. These poses are the previously traversed areas of the robot. You'll need a separate mapping algorithm if you want to do this and later feed that map to a navigation stack.

Looking at the ORB-SLAM3: Map Viewer app, I see the following after moving the drone in a rectangular path: Screenshot from 2024-07-15 12-46-26

How do I get the data for those blue signs? That is actually the localization data, where the drone thinks he is.

suchetanrs commented 2 months ago

Hi, regarding the TF Issue, I'll have a look at what's going on. Can you give me more information about your setup? Are you running with or without odometry? If the drone doesn't provide odometry data then use the wrapper with the parameter no_odometry_mode set to true.

For the second comment, you should be able to find this information on a topic named map_data. I think this was the topic name or should be a name similar to this. The data you see in the viewer is published at a fixed frequency on this topic.

OdedHorowits commented 2 months ago

regarding the TF Issue

so the setting is correct, i.e. no_odometry_mode: true.

Can you give me more information about your setup?

I am using Aerostack2. What information can i give that would help?

suchetanrs commented 2 months ago

Hi, I tried to recreate this issue. I was unable to do it. I set no_odometry_mode to true and passed only two topics into the docker container. The rgb image and the depth image and it seems to run fine. Can you please try the following?

Make sure you have run colcon build --symlink-install after you have changed the parameter file. Try doing a clean build by deleting the build and install directories in the workspace.
Try to open rviz inside the docker container and make sure under the TF section there is no other node publishing the transform between map and base_footprint.

Make sure your parameter file looks like the following.

ORB_SLAM3_RGBD_ROS2:
ros__parameters:
robot_base_frame: base_footprint
global_frame: map
odom_frame: odom
robot_x: 0.0
robot_y: 0.0
visualization: true
ros_visualization: false
no_odometry_mode: true

Sorry for the late reply, the issue is still in a blackbox :p I think more debugging might be needed.

OdedHorowits commented 2 months ago

I don't see any TF section in rviz :-(

Edit: ok found that: in rviz (Clicking the "Add" button, then select the "TF" display type) That gave me: Screenshot from 2024-07-21 23-26-40

What makes me think about the first part of the params file -

ORB_SLAM3_RGBD_ROS2:
  ros__parameters:
    robot_base_frame: base_footprint
    global_frame: map
    odom_frame: odom

Using ros2 run rqt_tf_tree rqt_tf_tree I can create the following image: frames

However, it also repeatedly prints the following errors to the shell where I run the rqt_tf_tree tool:

         at line 209 in ./src/buffer_core.cpp
Error:   TF_NO_CHILD_FRAME_ID: Ignoring transform from authority "default_authority" because child_frame_id not set 
         at line 217 in ./src/buffer_core.cpp
Error:   TF_NO_FRAME_ID: Ignoring transform with child_frame_id ""  from authority "default_authority" because frame_id not set
         at line 224 in ./src/buffer_core.cpp

That is very similar to the errors I get when launching the orb_slam wrapper.

suchetanrs / ORB-SLAM3-ROS2-Docker

What can I do once it works? #9