Depth Image from unity causing the unity bridge connection to crash! EDIT: Need camera Calibration Matrix for depth image to point cloud generation

[Stavya1993]

As per the code given in test/unity_bridge :

`
TEST(UnityBridge, HandleOutputDepth) {
  Logger logger{"Test HandleOutputDepth"};
  UnityBridge unity_bridge;
  QuadrotorDynamics dyn = QuadrotorDynamics(1.0, 0.2);
  std::shared_ptr<Quadrotor> quad = std::make_shared<Quadrotor>(dyn);
  std::shared_ptr<RGBCamera> rgb = std::make_shared<RGBCamera>();
  rgb->setPostProcesscing(std::vector<bool>{true, false, false});
  quad->addRGBCamera(rgb);
  unity_bridge.addQuadrotor(quad);

  EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));

  FrameID frame_id = 1;
  unity_bridge.getRender(frame_id);
  bool handle_output = unity_bridge.handleOutput();

  EXPECT_TRUE(handle_output);

  cv::Mat test_img;

  EXPECT_TRUE(rgb->getRGBImage(test_img));
  EXPECT_TRUE(rgb->getDepthMap(test_img));
  EXPECT_FALSE(rgb->getSegmentation(test_img));
  EXPECT_FALSE(rgb->getOpticalFlow(test_img));

  // timeout flightmare
  usleep(5 * 1e6);
}
`

I followed a similar logic to build a publisher from my flight_pilot_node. However the unity bridge connection seems to crash after only 4-5 published messages. I even used image transporter to compress my messages and tried giving a rate with sufficient sleep duration to ensure that my data overflow was not causing issues. However, I haven't achieved much success till now.

Sharing my code edits here:

flight_pilot_node.cpp:

#include <ros/ros.h>
#include <sensor_msgs/Image.h>

#include "flightros/flight_pilot.hpp"

int main(int argc, char** argv) {
  ros::init(argc, argv, "flight_pilot");
  flightros::FlightPilot pilot(ros::NodeHandle(), ros::NodeHandle("~"));
  
  ros::Publisher pub_depth_image; // Added by Stavya bhatia

  // Initiate publisher for depth Image - Added by Stavya bhatia
  ros::NodeHandle n_;
  // image_transport::ImageTransport it(n_);
  pub_depth_image = n_.advertise<sensor_msgs::Image>("/sdepth_image_raw",1);

  while(ros::ok()){
    pilot.depthCamera(pub_depth_image);
    // spin the ros
    ros::spinOnce();
  }
  return 0;
}

flight_pilot.hpp

1. Added callback function
   void depthCamera(ros::Publisher msg);
2. Added depth camera variable in private:
   // Depth camera variables - cv::Mat depth_image; bool handle_output;

flight_pilot.cpp:
Function depthCamera added

void FlightPilot::depthCamera(ros::Publisher pub_depth_image){
    cv_bridge::CvImage img_bridge;

    handle_output = unity_bridge_ptr_->handleOutput();
    rgb_camera_->getDepthMap(depth_image);
    sensor_msgs::Image msg ;
    // img_bridge = cv_bridge::
    // msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", depth_image).toImageMsg();
    std_msgs::Header header; // empty header
    header.seq = 1; // user defined counter
    header.stamp = ros::Time::now(); // time
    img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, depth_image);
    img_bridge.toImageMsg(msg); // from cv_bridge to sensor_msgs::Image
    // pub_img.publish(msg); // ros::Publisher pub_img = node.advertise<sensor_msgs::Image>("topic", queuesize);
    pub_depth_image.publish(msg);
}

This is an extra time - sensitive task and would really appreciate if I can get some help in understanding if I am doing something wrong or is this a unity-ros issue that I cannot account for.

Regards

[riddhishah18]

I was facing a similar problem last week. Will really appreciate a solution for this one

[anna-meszaros]

Been facing a similar issue with the rgb camera feed. Any feedback would be a massive help.

[yun-long]

We will look into it, and solve it as soon as possible. (within this week)

[slimeth]

@Stavya1993 https://github.com/uzh-rpg/flightmare/blob/dev/samples/flightros/src/camera/camera.cpp I've written an example of how to publish the images on a ROS topic. This sample is written for the soon published binary release and needs minor modification to work with the previous release. I did not encounter any unity_bridge connection crashes with this code. I hope the sample helps you!

[Stavya1993]

@slimeth: Thanks so much, I am really surprised, I followed a very similar code with and without using image transport.
In fact the only difference being earlier I had declared all the depth image publishing and camera functions inside the Flightpilot class given theres already a camera initialized with the drone. Now following the link, I have made some of the variables pertaining to the camera public in Flightpilot and called it directly in the flight_pilot_node using exactly the same code you have used in the camera example.
This is great.

I perhaps have one last question around this topic. What is the camera calibration values in this case?

I found the depth scale, but also need the intrinsic matrix and cant seem to find it.

[vanshilshah97]

@slimeth thanks for resolving the previous issue. I also wanted the camera calibration matrix for the camera spawned in flightmare. wasn't able to find it ? Your help will be really appreciated

[Stavya1993]

Hi, Ive managed to resolve everything I need for my task except the calibration matrix. Any help in this will be highly appreciated.

[yun-long]

Hi, you don't extract the calibration matrix from Unity, instead, it is calculated on the client-side (ROS/C++).

Since both the image dimensions (width x height) and field of view (FOV) are defined by the user. (see https://github.com/uzh-rpg/flightmare/blob/master/flightlib/include/flightlib/sensors/rgb_camera.hpp#L59-L61)

you can compute the focal length using this formula: f = ( image.height / 2.0 ) / tan( (M_PI * FOV/180.0 )/2.0 )
and fx=fy. Hence, the camera intrinsic matrix is [ [fx, 0, image.width/2], [0, fy, image.height/2], [0, 0, 1] ].

[Stavya1993]

Perfect! Thank you so so much! Sorry for bothering so much again.

Think what you guys have made is absolutely beautiful!

[BananaGorilla98]

Hi @Stavya1993 . May I know if you are able to retrieve images in RVIZ by subscribing those topics?

[Stavya1993]

Yes @BananaGorilla98

[BananaGorilla98]

@Stavya1993 would you mind to share how you do it? I was trying the method mentioned here but still could not get anything in my RVIZ.

[Stavya1993]

Sure.
I basically used the fligbt_pilot_node code and added the get_depth_image function to the RGB camera already instantiated in the flight_pilot. For this I basically followed the same code from camera.cpp shared by @slimeth above. I did have to make some private values associated with the RGB camera in flight pilot to public for accessing them in the flight_pilot_node.
This was already working for me. I can share my source code in a bit when I get to my workstation.

[Stavya1993]

FLIGHT_PILOT_NODE for obtaining depth camera image

#include <ros/ros.h>

#include "flightros/flight_pilot.hpp"

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <eigen3/Eigen/Eigen>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "cmath"

int main(int argc, char** argv) {
  ros::init(argc, argv, "flight_pilot");
  ros::NodeHandle nh("");
  ros::NodeHandle pnh("~");

  flightros::FlightPilot pilot(nh, pnh);

  // Image transport for compressed image publishing
  image_transport::ImageTransport it(pnh);
  // image_transport::Publisher rgb_pub_;
  image_transport::Publisher depth_pub_;

  // Publishers for depth image
  depth_pub_ = it.advertise("/depth", 1);
  ros::Publisher cinfo_pub = nh.advertise<sensor_msgs::CameraInfo>("/camera_info", 1);
  
  // Generating camera info 
  // Camera Info Message Containing the intrinsic properties of the camera for depth_image_proc
  sensor_msgs::CameraInfo camera_info_msg ;
  camera_info_msg.header.frame_id = "camera";
  camera_info_msg.width = 640;
  camera_info_msg.height = 480;
  camera_info_msg.K =  {240,0,320,0,240,240,0,0,1} ;     
  camera_info_msg.P = {240,0,320,0,0,240,240,0,0,0,1,0} ;

  // Colour saturation in camera from rpg_flghtmare_unity
  float r = 0.19215687;
  float g = 0.3019608;
  float b = 0.4745098;  // Camera pose
  
  while (ros::ok() && pilot.unity_render_ && pilot.unity_ready_) {
    cv::Mat img;
    
    // Using RGB Camera on drone to get depth image
    pilot.rgb_camera_->getDepthMap(img);

    // Conversion of 8UC3 to 8UC1 for depth_proc_image
    cv::Mat bgr[3],img_new,img_final;   
    cv::split(img,bgr);
    // Converting bgr to Grey scale - NOTE: cv::toConvert not working!
    bgr[0] *= b;
    bgr[1] *= g;
    bgr[2] *= r;
    cv::add(bgr[0], bgr[1],img_new);
    cv::add(img_new,bgr[2],img_new);
    img_new.convertTo(img_final, CV_16UC1,10);  //Changed from 20
    // img_final = img_final/1000;

    // Converting image to ROS message and generating static transform for pcl mounting
    sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(std_msgs::Header(),"16UC1", img_final).toImageMsg();      
    depth_msg->header.frame_id = "camera";

   
    depth_msg->header.stamp = ros::Time::now();
    depth_pub_.publish(depth_msg);

    // // Publishing camera calibration information
    camera_info_msg.header.stamp = ros::Time::now();
    cinfo_pub.publish(camera_info_msg);

    // spin the ros
    ros::spinOnce();
  }
  
return 0;
}

[Stavya1993]

Hi,

The flight_pilot has unity ready and unity render as private instantiations in the flight_pilot.h file in include. Move them to public instantiations and it should work.

[BananaGorilla98]

@Stavya1993 Thanks for sharing your code! I have tried your code and it built successfully. However, the RVIZ still not able to show the images. May I know if there is any configuration that I need to do?