diff --git a/Depth_ROS b/Depth_ROS new file mode 100644 index 0000000..358afd3 --- /dev/null +++ b/Depth_ROS @@ -0,0 +1,79 @@ +import rospy +from sensor_msgs.msg import CompressedImage +from cv_bridge import CvBridge +import cv2 +import numpy as np +import torch +import torch.nn.functional as F +from torchvision.transforms import Compose +from depth_anything.dpt import DepthAnything +from depth_anything.util.transform import Resize, NormalizeImage, PrepareForNet + + +def image_callback(msg): + bridge = CvBridge() + try: + # Convert the compressed image to a cv2 format + raw_frame = bridge.compressed_imgmsg_to_cv2(msg, desired_encoding='passthrough') + frame_height, frame_width, _ = raw_frame.shape + + frame = cv2.cvtColor(raw_frame, cv2.COLOR_BGR2RGB) / 255.0 + frame = transform({'image': frame})['image'] + frame = torch.from_numpy(frame).unsqueeze(0).to(DEVICE) + + with torch.no_grad(): + depth = depth_anything(frame) + + # Interpolating the depth to match the frame size + depth = F.interpolate(depth[None], (frame_height, frame_width), mode='bilinear', align_corners=False)[0, 0] + depth = (depth - depth.min()) / (depth.max() - depth.min()) * 255.0 + depth = depth.cpu().numpy().astype(np.uint8) + + # Convert depth to color map for better visualization + depth_color = cv2.applyColorMap(depth, cv2.COLORMAP_INFERNO) + + # Create split region and combine the raw frame and depth frame + split_region = np.ones((frame_height, margin_width, 3), dtype=np.uint8) * 255 + combined_frame = cv2.hconcat([raw_frame, split_region, depth_color]) + + # Display the result + cv2.imshow('Depth', depth_color) + cv2.waitKey(1) + + except Exception as e: + rospy.logerr("Error processing image: {}".format(e)) + + +if __name__ == '__main__': + # Initialize ROS node + rospy.init_node('depth_estimation_from_camera', anonymous=True) + + # Set up the device and model + DEVICE = 'cuda' if torch.cuda.is_available() else 'cpu' + depth_anything = DepthAnything.from_pretrained('LiheYoung/depth_anything_vits14').to(DEVICE).eval() + + total_params = sum(param.numel() for param in depth_anything.parameters()) + print('Total parameters: {:.2f}M'.format(total_params / 1e6)) + + margin_width = 50 + + # Transformations for the input frame + transform = Compose([ + Resize( + width=518, + height=518, + resize_target=False, + keep_aspect_ratio=True, + ensure_multiple_of=14, + resize_method='lower_bound', + image_interpolation_method=cv2.INTER_CUBIC, + ), + NormalizeImage(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]), + PrepareForNet(), + ]) + + # Set up ROS subscriber to the video/compressed topic + rospy.Subscriber('/image/video/compressed', CompressedImage, image_callback) + + # Keep the node running to continuously receive images + rospy.spin()