[Realsense D455 Visual SLAM] Pose jumps after initialization #183
Description
I am trying to get isaac ros visual slam to give me an accurate pose using the realsense D455 but have run into a problem where the pose is normal for some time initially and then shoots off / jumps to some random pose that is definitely incorrect. For example, if I am moving in +/- x I should expect to see that /visual_slam/tracking/vo_pose is increasing/decreasing in x accordingly. It does that for a bit and then the pose is completely wrong and it shoots off even though I am for example not moving or still just moving in +/- x. In the source code I have disabled localization and mapping (though yes I could just do it in the launch). I also have the ground constraint enabled for odometry for the current physical setup I need it for. I found that when the constraint is enabled it is shooting off to a much smaller value like what is shown below compared to x y and z being in the 100s range and sometimes 1000s.
Here's an example of the pose it would shoot off to:
x: -6.530872821807861
y: -42.238014221191406
z: 41.82627487182617
This is what my isaac_ros_visual_slam_realsense.launch.py looks like:
import launch
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
"""Launch file which brings up visual slam node configured for RealSense."""
realsense_camera_node = ComposableNode(
name='camera',
namespace='camera',
package='realsense2_camera',
plugin='realsense2_camera::RealSenseNodeFactory',
parameters=[{
'enable_infra1': True,
'enable_infra2': True,
'enable_color': True,
'enable_depth': False,
'align_depth.enable': True,
'depth_module.emitter_enabled': 0,
'depth_module.profile': '640x360x30',
'rgb_camera.profile': '848x480x5',
'rgb_camera.enable_auto_exposure': False,
'rgb_camera.exposure': 50,
'clip_distance': 5.0,
'enable_gyro': True,
'enable_accel': True,
'gyro_fps': 200,
'accel_fps': 200,
'unite_imu_method': 2,
'intra_process_comms': True
}]
)
visual_slam_node = ComposableNode(
name='visual_slam_node',
package='isaac_ros_visual_slam',
plugin='isaac_ros::visual_slam::VisualSlamNode',
parameters=[{
'denoise_input_images': False,
'rectified_images': True,
'enable_debug_mode': False,
'debug_dump_path': '/tmp/cuvslam',
'enable_slam_visualization': False,
'enable_landmarks_view': False,
'enable_observations_view': False,
'map_frame': 'map',
'odom_frame': 'odom',
'base_frame': 'camera_link',
'input_imu_frame': 'camera_gyro_optical_frame',
'enable_imu_fusion': True,
'gyro_noise_density': 0.000244,
'gyro_random_walk': 0.000019393,
'accel_noise_density': 0.001862,
'accel_random_walk': 0.003,
'calibration_frequency': 200.0,
'img_jitter_threshold_ms': 60.0,
'enable_ground_constraint_in_odometry': True
}],
remappings=[('stereo_camera/left/image', 'camera/infra1/image_rect_raw'),
('stereo_camera/left/camera_info', 'camera/infra1/camera_info'),
('stereo_camera/right/image', 'camera/infra2/image_rect_raw'),
('stereo_camera/right/camera_info', 'camera/infra2/camera_info'),
('visual_slam/imu', 'camera/imu')]
)
visual_slam_launch_container = ComposableNodeContainer(
name='visual_slam_launch_container',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
visual_slam_node,
realsense_camera_node
],
output='screen'
)
return launch.LaunchDescription([visual_slam_launch_container])
I do also notice that right at the beginning I get the delta is above threshold warning, but it's only initially and then it's fine and that warning goes away.
Any help here would be appreciated!