Visionworks with ROS (blank disparity map)

Robotics & Edge Computing Jetson & Embedded Systems Jetson TX1
1

I have tried to convert the VisionWorks stereoMatching into a ros package. I used the base from https://github.com/ShreyasSkandan/ros_tegra_stereo and replaced the single image concept with subscribing to left and right rectified images.

The code is running but all my disparity images are black with 0 values. I do not get any error on the code and the parameters I use are valid as well.

The main code which uses the stereoMatching from VisionWorks

using namespace message_filters;
using namespace sensor_msgs;
using namespace cv;
using namespace std;

#define IMG_WIDTH 1280 // change 
#define IMG_HEIGHT 1024 // change

cv::Mat rmap[2][2];
vx_image left_rect;
vx_image right_rect;
vx_image disparity;
StereoMatching::StereoMatchingParams params;
ovxio::ContextGuard context;
StereoMatching::ImplementationType implementationType;
int counter_global = 0;
int baseline_opt;
vx_uint32 plane_index = 0;
vx_rectangle_t rect = {0u,0u,1280u,1024u}; // change

image_transport::Publisher pub;
image_transport::Publisher left_pcpub;
image_transport::Publisher right_pcpub;

static bool read(const std::string &nf, StereoMatching::StereoMatchingParams &config, std::string &message)
{
    std::unique_ptr<nvxio::ConfigParser> parser(nvxio::createConfigParser());
    parser->addParameter("min_disparity",
                         nvxio::OptionHandler::integer(
                             &config.min_disparity,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(256)));
    parser->addParameter("max_disparity",
                         nvxio::OptionHandler::integer(
                             &config.max_disparity,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(256)));
    parser->addParameter("P1",
                         nvxio::OptionHandler::integer(
                             &config.P1,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(256)));
    parser->addParameter("P2",
                         nvxio::OptionHandler::integer(
                             &config.P2,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(256)));
    parser->addParameter("sad",
                         nvxio::OptionHandler::integer(
                             &config.sad,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(31)));
    parser->addParameter("bt_clip_value",
                         nvxio::OptionHandler::integer(
                             &config.bt_clip_value,
                             nvxio::ranges::atLeast(15) & nvxio::ranges::atMost(95)));
    parser->addParameter("max_diff",
                         nvxio::OptionHandler::integer(
                             &config.max_diff));
    parser->addParameter("uniqueness_ratio",
                         nvxio::OptionHandler::integer(
                             &config.uniqueness_ratio,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(100)));
    parser->addParameter("scanlines_mask",
                         nvxio::OptionHandler::integer(
                             &config.scanlines_mask,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(256)));
    parser->addParameter("flags",
                         nvxio::OptionHandler::integer(
                             &config.flags,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(3)));
    parser->addParameter("ct_win_size",
                         nvxio::OptionHandler::integer(
                             &config.ct_win_size,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(5)));
    parser->addParameter("hc_win_size",
                         nvxio::OptionHandler::integer(
                             &config.hc_win_size,
                             nvxio::ranges::atLeast(0) & nvxio::ranges::atMost(5)));
    message = parser->parse(nf);
    return message.empty();
}

void callback(const ImageConstPtr& left, const ImageConstPtr& right) {
  // conversion to rosmsgs::Image to cv::Mat using cv_bridge
  nvx::Timer read_rect_timer;
  read_rect_timer.tic();
  cv_bridge::CvImagePtr cv_left;
  try
    {
      cv_left = cv_bridge::toCvCopy(left);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

  cv_bridge::CvImagePtr cv_right;
  try
      {
        cv_right = cv_bridge::toCvCopy(right);
      }
      catch (cv_bridge::Exception& e)
      {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
      }

   left_rect = nvx_cv::createVXImageFromCVMat(context,cv_left->image);
    NVXIO_CHECK_REFERENCE(left_rect);
    right_rect = nvx_cv::createVXImageFromCVMat(context,cv_right->image);
    NVXIO_CHECK_REFERENCE(right_rect);
    std::cout<<cv_left->image.rows<<" "<<cv_left->image.cols<<std::endl;

    disparity = vxCreateImage(context, cv_left->image.cols, cv_left->image.rows, VX_DF_IMAGE_U8);
    NVXIO_CHECK_REFERENCE(disparity);
    std::unique_ptr<StereoMatching> stereo(StereoMatching::createStereoMatching(context,
					   params,implementationType,left_rect, right_rect, disparity));
    stereo->run(); 
    nvx_cv::VXImageToCVMatMapper map(disparity,0, NULL, VX_READ_AND_WRITE, VX_MEMORY_TYPE_HOST);
    cv::Mat disp = map.getMat();
    cv::imwrite("disparity.png",disp);


    cv_bridge::CvImage out_disp;
    out_disp.header = cv_left->header;
    out_disp.encoding = sensor_msgs::image_encodings::MONO8;
    out_disp.image = disp;
    pub.publish(out_disp.toImageMsg());


    //char savefilename[50];
    //snprintf(savefilename,sizeof(savefilename),"/home/nvidia/saveddisp-3/disparity%05d.png",counter_global);
    //counter_global = counter_global + 1;
    //printf("%s",savefilename);
    //cv::imwrite(savefilename,disp);

    //cv::waitKey(1);
    double timer = read_rect_timer.toc();
    std::cout << "Time Elapsed For Rect + SGBM : " << timer << " ms" << std::endl << std::endl;
    vxReleaseImage(&left_rect);
    vxReleaseImage(&right_rect);
    vxReleaseImage(&disparity);
}


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

  std::string configFile = "/home/ubuntu/catkin_ws/src/ros_tegra_stereo/data/stereo_matching_params.ini"; // change

  implementationType = StereoMatching::LOW_LEVEL_API;
  std::string error;
  if (!read(configFile, params, error)) {
      std::cerr << error;
      return 0;
  }
  std::cout << "SGBM Parameters loaded\n" ;

  vxDirective(context, VX_DIRECTIVE_ENABLE_PERFORMANCE);
  vxRegisterLogCallback(context, &ovxio::stdoutLogCallback, vx_false_e);

  left_rect = vxCreateImage(context, IMG_WIDTH, IMG_HEIGHT, VX_DF_IMAGE_RGBX);
  NVXIO_CHECK_REFERENCE(left_rect);
  right_rect = vxCreateImage(context, IMG_WIDTH, IMG_HEIGHT, VX_DF_IMAGE_RGBX);
  NVXIO_CHECK_REFERENCE(right_rect);
  disparity = vxCreateImage(context, IMG_WIDTH, IMG_HEIGHT, VX_DF_IMAGE_U8);
  NVXIO_CHECK_REFERENCE(disparity);

  image_transport::ImageTransport it(nh);
  pub = it.advertise("/disparityimage",1); // change
  message_filters::Subscriber<Image> left_sub(nh, "/camera/left/image_rect", 1);
  message_filters::Subscriber<Image> right_sub(nh, "/camera/right/image_rect", 1);

  //time syncronizer to publish 2 images in the same callback function
	typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
	Synchronizer<MySyncPolicy> sync(MySyncPolicy(1), left_sub, right_sub);
	//TimeSynchronizer<Image, Image> sync(left_sub, right_sub, 50);

  //call calback each time a new message arrives
  sync.registerCallback(boost::bind(&callback, _1, _2));
  ros::spin();
}

Since I subscribe to mono images in ROS, i commented out the color disparity option and kept the remaining code of stereo_matching from VisionWorks as default as well.

SGBM::SGBM(vx_context context, const StereoMatchingParams& params,
               vx_image left, vx_image right, vx_image disparity)
    {
        vx_df_image format = VX_DF_IMAGE_VIRT;
        vx_uint32 width = 0;
        vx_uint32 height = 0;
        vx_uint32 D = params.max_disparity - params.min_disparity;

        NVXIO_SAFE_CALL( vxQueryImage(left, VX_IMAGE_ATTRIBUTE_FORMAT, &format, sizeof(format)) );
        NVXIO_SAFE_CALL( vxQueryImage(left, VX_IMAGE_ATTRIBUTE_WIDTH, &width, sizeof(width)) );
        NVXIO_SAFE_CALL( vxQueryImage(left, VX_IMAGE_ATTRIBUTE_HEIGHT, &height, sizeof(height)) );

        main_graph_ = vxCreateGraph(context);
        NVXIO_CHECK_REFERENCE(main_graph_);

        vx_image left_gray = vxCreateVirtualImage(main_graph_, width, height, VX_DF_IMAGE_U8);
        NVXIO_CHECK_REFERENCE(left_gray);

        vx_image right_gray = vxCreateVirtualImage(main_graph_, width, height, VX_DF_IMAGE_U8);
        NVXIO_CHECK_REFERENCE(right_gray);

        //left_cvt_color_node_ = vxColorConvertNode(main_graph_, left, left_gray);
        //NVXIO_CHECK_REFERENCE(left_cvt_color_node_);

        //right_cvt_color_node_ = vxColorConvertNode(main_graph_, right, right_gray);
        //NVXIO_CHECK_REFERENCE(right_cvt_color_node_);

        vx_image left_census = NULL, right_census = NULL;

        // apply census transform, if requested
        if (params.ct_win_size > 1)
        {
            left_census =  vxCreateVirtualImage(main_graph_, width, height, VX_DF_IMAGE_U32);
            NVXIO_CHECK_REFERENCE(left_census);
            right_census = vxCreateVirtualImage(main_graph_, width, height, VX_DF_IMAGE_U32);
            NVXIO_CHECK_REFERENCE(right_census);

            left_census_node_ = nvxCensusTransformNode(main_graph_, left_gray, left_census, params.ct_win_size);
            NVXIO_CHECK_REFERENCE(left_census_node_);
            right_census_node_ = nvxCensusTransformNode(main_graph_, right_gray, right_census, params.ct_win_size);
            NVXIO_CHECK_REFERENCE(right_census_node_);
        }
        else
        {
            left_census_node_ = NULL;
            right_census_node_ = NULL;
        }

        vx_image convolved_cost = vxCreateVirtualImage(main_graph_, width * D, height, VX_DF_IMAGE_U8);
        NVXIO_CHECK_REFERENCE(convolved_cost);

        // nvxConvolveCostNode can be seen as a simple "filtering" function for
        // the evaluated cost volume. By setting the `sad` parameter to 1 we can
        // omit it overall.
        vx_int32 sad = params.sad;
        if (sad > 1)
        {
            vx_image cost = vxCreateVirtualImage(main_graph_, width * D, height, VX_DF_IMAGE_U8);
            NVXIO_CHECK_REFERENCE(cost);

            // census transformed images should be compared by hamming cost
            if (params.ct_win_size > 1)
            {
                compute_cost_node_ = nvxComputeCostHammingNode(main_graph_, left_census, right_census, cost,
                                                               params.min_disparity, params.max_disparity,
                                                               params.hc_win_size);
            }
            else
            {
                compute_cost_node_ = nvxComputeModifiedCostBTNode(main_graph_, left_gray, right_gray, cost,
                                                                  params.min_disparity, params.max_disparity,
                                                                  params.bt_clip_value);
            }
            NVXIO_CHECK_REFERENCE(compute_cost_node_);

            convolve_cost_node_ = nvxConvolveCostNode(main_graph_, cost, convolved_cost,
                                                      D, sad);
            NVXIO_CHECK_REFERENCE(convolve_cost_node_);

            vxReleaseImage(&cost);
        }
        else
        {
            if (params.ct_win_size > 1)
            {
                compute_cost_node_ = nvxComputeCostHammingNode(main_graph_, left_census, right_census, convolved_cost,
                                                               params.min_disparity, params.max_disparity,
                                                               1);
            }
            else
            {
                compute_cost_node_ = nvxComputeModifiedCostBTNode(main_graph_, left_gray, right_gray, convolved_cost,
                                                                  params.min_disparity, params.max_disparity,
                                                                  params.bt_clip_value);
            }
            NVXIO_CHECK_REFERENCE(compute_cost_node_);

            convolve_cost_node_ = NULL;
        }

        vx_image aggregated_cost = vxCreateVirtualImage(main_graph_, width * D, height, VX_DF_IMAGE_S16);
        NVXIO_CHECK_REFERENCE(aggregated_cost);

        aggregate_cost_scanlines_node_ = nvxAggregateCostScanlinesNode(main_graph_, convolved_cost, aggregated_cost,
                                                                       D, params.P1, params.P2, params.scanlines_mask);
        NVXIO_CHECK_REFERENCE(aggregate_cost_scanlines_node_);

        vx_image disparity_short = vxCreateVirtualImage(main_graph_, width, height, VX_DF_IMAGE_S16);
        NVXIO_CHECK_REFERENCE(disparity_short);

        compute_disparity_node_ = nvxComputeDisparityNode(main_graph_, aggregated_cost, disparity_short,
                                                          params.min_disparity, params.max_disparity,
                                                          params.uniqueness_ratio, params.max_diff);
        NVXIO_CHECK_REFERENCE(compute_disparity_node_);

        vx_int32 shift = 4;
        vx_scalar s_shift = vxCreateScalar(context, VX_TYPE_INT32, &shift);
        NVXIO_CHECK_REFERENCE(s_shift);
        convert_depth_node_ = vxConvertDepthNode(main_graph_, disparity_short, disparity, VX_CONVERT_POLICY_SATURATE, s_shift);
        NVXIO_CHECK_REFERENCE(convert_depth_node_);

        vxReleaseScalar(&s_shift);
        vxReleaseImage(&disparity_short);
        vxReleaseImage(&aggregated_cost);
        vxReleaseImage(&convolved_cost);javascript:void(0);
        vxReleaseImage(&right_census);
        vxReleaseImage(&left_census);
        vxReleaseImage(&right_gray);
        vxReleaseImage(&left_gray);

        NVXIO_SAFE_CALL( vxVerifyGraph(main_graph_) );
    }

Any suggestions of what I could be doing wrong? Thanks!

2

Hi,

Want to confirm environment first:

Do you setup ROS with the JetsonHacks page?

3

I downloaded ROS from the website directly. The original application works when I have a single image which has both the left and right images (like the VisionWorks example).

But for my case, I have cameras which are monochrome and I get two images which I sync using approximate sync. But I get blank disparity in this case.

4

Hi,

Please try:

  1. Make sure your camera is working inside the ROS node. You can try it via v4l2 or OpenCV.

  2. Could you try the ROS setup of JetsonHacks? No sure if there is some missing configuration.

Thanks, and please let us know the results.

5

I am not using the cameras, I am using a rosbag which has the camera topics. I provide direct rectified images to the visionworks ros implementation.

I have the required configurations for ROS as I don’t get any dependency errors. The code runs, but it published disparity maps of 0’s.

I have several doubts,

  1. Does Visionworks work with monochrome images? (The sample video contains color images, my rosbag has monochrome)
  2. Does vision works need any additional code to work on 2 images instead of a single image where the left image is at the top and right image at the bottom? (This is a very specific format as all opencv and other standard stereo matching functions require 2 separate input images)
  3. Is there any documentation of how the algorithm works? The documentation currently packaged with visionworks doesn't give insight as to how the disparity is calculated. This could help in debugging as well
  4. When I run the stereo matching with color images which are not in the same format as the test images provided by NVIDIA, I get a ``` terminate called after throwing an instance of 'std::runtime_error' what(): vxVerifyGraph(main_graph_) failure [status = -10] in file /home/ubuntu/catkin_ws/src/ros_tegra_stereo_tu/src/stereo_matching.cpp line 189 ```

Thank you!

6

Hi,

For a monochrome image, may require some turning.

  1. Yes for most functions. Details, please check openVX spec:
    https://www.khronos.org/registry/OpenVX/specs/1.1/OpenVX_Specification_1_1.pdf

  2. Please change the ROI location to your use-case.

vx_rectangle_t left_rect { 0, 0, sourceParams.frameWidth, sourceParams.frameHeight / 2 };
vx_image left  = vxCreateImageFromROI(top_bottom, &left_rect);
NVXIO_CHECK_REFERENCE(left);
vx_rectangle_t right_rect { 0, sourceParams.frameHeight / 2, sourceParams.frameWidth, sourceParams.frameHeight };
vx_image right = vxCreateImageFromROI(top_bottom, &right_rect);
NVXIO_CHECK_REFERENCE(right);
  1. Our document describes the flow clearly:
#Flow
      (left frame)              (right frame)
           |                          |
      [ScaleImage] (down)        [ScaleImage] (down)
           |                          |
      (to gray)    (to gray)
           |                          |
           +---------+     +----------+
                     |     |
               [SemiGlobalMatching]
                        |
                  [ConvertDepth] (to 8-bit)
                        |
                   [ScaleImage] (up)
                        |
                    [Multiply]
                        |
                 (disparity image)

Do you look into the right place? It should be:

VisionWorks Toolkit Reference Documentation
VisionWorks API
Samples and Demos
Demo Applications
Stereo Matching Demo App

  1. It looks like your graph is incorrect. Please check our document for details:

VisionWorks Toolkit Reference Documentation
VisionWorks API
Tutorials
VisionWorks Quick Start (Graph Mode)