wavemap
wavemap copied to clipboard
ethz-asl
Do you sopport Realsense camera ? (such as D435i)
I want to combine wavemap with D435i depth camera for uav path planning.Is that possible?
Hi @Tfly6,
Yes that's supported; we've used wavemap with D435 cameras in several projects in the past with good results.
To build maps, wavemap will need two things:
- The intrinsic calibration for your depth sensor
- Sensor poses for each measurement
For the calibration and general settings, you can find an example config for a single depth camera here. Don't forget to update the camera intrinsics to match your camera. One quick way to find these values is by looking at the CameraInfo message published by your sensor's ROS driver.
For measurement poses, if you publish the odometry for your UAV as a TF and add a static TF between the robot's base frame and the frame_id of your depth camera, wavemap can automatically look up the pose of the camera at the time of each measurement.
Hi @victorreijgwart,
I am attempting to integrate wavemap with PX4 SITL using an Iris drone model equipped with a D435i camera. Following your recommendations, I modified the relevant configuration files, but the wavemap visualization still does not appear in RVIZ.
general:
world_frame: "map"
logging_level: debug
allow_reset_map_service: true
map:
type: hashed_chunked_wavelet_octree
min_cell_width: { meters: 0.02 }
map_operations:
- type: threshold_map
once_every: { seconds: 2.0 }
- type: prune_map
once_every: { seconds: 10.0 }
- type: publish_map
once_every: { seconds: 2.0 }
measurement_integrators:
panoptic_mapping_camera:
projection_model:
type: pinhole_camera_projector
width: 640
height: 480
fx: 319.9348449707031
fy: 319.9348449707031
cx: 320.0
cy: 240.0
measurement_model:
type: continuous_ray
range_sigma: { meters: 0.01 }
scaling_free: 0.2
scaling_occupied: 0.4
integration_method:
type: hashed_chunked_wavelet_integrator
min_range: { meters: 0.1 }
max_range: { meters: 5.0 }
inputs:
- type: depth_image_topic
topic_name: "/camera/depth/image_raw"
measurement_integrator_names: panoptic_mapping_camera
topic_queue_length: 10
max_wait_for_pose: { seconds: 1.0 }
(map frame to base_link frame have published by mavros)
<launch>
<!-- Wavemap params -->
<arg name="param_file"
default="$(find wavemap_ros)/config/test.yaml"
doc="Name of the file from which to read wavemap's server params."/>
<!-- Run the Kinect driver -->
<!-- <node pkg="nodelet" type="nodelet" name="manager" args="manager">
<param name="num_worker_threads" value="4"/>
</node> -->
<!-- Link TF frames -->
<node pkg="tf"
type="static_transform_publisher"
name="tf_camera"
args="0.1 0 0.0 -1.57 0 -1.57 base_link D435i::camera_depth_frame 100"/>
<!-- Run wavemap -->
<include file="$(find wavemap_ros)/launch/wavemap_server.launch"
pass_all_args="true"/>
</launch>
This is terminal output.
[ INFO] [1745396842.896104721]: Creating thread pool with 16 threads.
[ INFO] [1745396842.943060909]: rviz version 1.14.25
[ INFO] [1745396842.943097826]: compiled against Qt version 5.12.8
[ INFO] [1745396842.943105058]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1745396842.948268424]: Forcing OpenGl version 0.
[ INFO] [1745396843.488710268, 3737.772000000]: Stereo is NOT SUPPORTED
[ INFO] [1745396843.488755882, 3737.772000000]: OpenGL device: NVIDIA GeForce RTX 3060/PCIe/SSE2
[ INFO] [1745396843.488768036, 3737.772000000]: OpenGl version: 4.6 (GLSL 4.6).
[ WARN] [1745396844.304494979, 3738.588000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3737.528000000; skipping depth image.
[DEBUG] [1745396844.311069062, 3738.592000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 14.
[DEBUG] [1745396845.550901452, 3739.832000000]: Trying to publish message of type [wavemap_msgs/Map/8a1a96f7340d6825cf4c47ebe8cbee97] on a publisher with type [wavemap_msgs/Map/8a1a96f7340d6825cf4c47ebe8cbee97]
[DEBUG] [1745396845.551095061, 3739.832000000]: Integrated new depth image in 1.23999s. Total integration time: 1.23999s.
[DEBUG] [1745396845.557158258, 3739.840000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 13.
[DEBUG] [1745396846.806693140, 3741.088000000]: Integrated new depth image in 1.24949s. Total integration time: 2.48948s.
[DEBUG] [1745396846.812399086, 3741.092000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 12.
[DEBUG] [1745396848.143963548, 3742.424000000]: Integrated new depth image in 1.33153s. Total integration time: 3.821s.
[DEBUG] [1745396848.149749289, 3742.432000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 11.
[DEBUG] [1745396849.410764252, 3743.692000000]: Integrated new depth image in 1.26097s. Total integration time: 5.08197s.
[DEBUG] [1745396849.416692546, 3743.696000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 10.
[DEBUG] [1745396850.729470919, 3745.012000000]: Integrated new depth image in 1.31273s. Total integration time: 6.3947s.
[DEBUG] [1745396850.735117717, 3745.016000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 9.
[DEBUG] [1745396852.064540618, 3746.344000000]: Integrated new depth image in 1.32938s. Total integration time: 7.72408s.
[DEBUG] [1745396852.070395076, 3746.352000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 8.
[DEBUG] [1745396853.274787894, 3747.556000000]: Integrated new depth image in 1.20435s. Total integration time: 8.92843s.
[DEBUG] [1745396853.280566010, 3747.560000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 7.
[DEBUG] [1745396854.483522638, 3748.764000000]: Integrated new depth image in 1.20291s. Total integration time: 10.1313s.
[ WARN] [1745396854.484935575, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.376000000; skipping depth image.
[ WARN] [1745396854.484971009, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.404000000; skipping depth image.
[ WARN] [1745396854.484998695, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.432000000; skipping depth image.
[ WARN] [1745396854.485024363, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.460000000; skipping depth image.
[ WARN] [1745396854.485056614, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.488000000; skipping depth image.
[ WARN] [1745396854.485084822, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.516000000; skipping depth image.
[ WARN] [1745396854.485112144, 3748.764000000]: Waited 1s but still could not look up pose for depth image with frame "D435i::camera_depth_frame" in world frame "map" at timestamp 3738.548000000; skipping depth image.
[DEBUG] [1745396854.490631836, 3748.772000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 9.
[DEBUG] [1745396855.733833573, 3750.012000000]: Integrated new depth image in 1.24316s. Total integration time: 11.3745s.
[DEBUG] [1745396855.739338561, 3750.020000000]: Inserting depth image with [640, 480] points. Remaining pointclouds in queue: 8.
[DEBUG] [1745396856.968005834, 3751.248000000]: Integrated new depth image in 1.22862s. Total integration time: 12.6031s.