Robotics StackExchange | Archived questions

triclops/flycapture for bumblebee stereo

Hello,

I tried a little piece of code to make flycapture, triclos and ROS working together. In old versions of ros like fuerte or hydro we are not allow to make this libraries working with ROS because a bad performance of boost library (not compatible versions) Now I am working in Ubuntu 14.04 machine with Indigo and looks very promissing. I downloaded the third party libraries from pointGrey site (triclops-3.4.0.5-amd64, flycapture2-2.6.3.4-amd64)

This is my CMakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(xb3)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  std_msgs
  cv_bridge
  image_transport
)
find_package(OpenCV REQUIRED)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_LIBRARIES}
  /usr/include/triclops
  /usr/include/flycapture
)

add_executable(xb3_client src/xb3_client.cpp)
add_executable(triXB3 src/triXB3.cpp)

## Specify libraries to link a library or executable target against
target_link_libraries(xb3_client
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
)

target_link_libraries(triXB3
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  triclops
  pnmutils
  flycapture
  flycapture2bridge
  pthread
)

And this is my source code (due to the fact that is private and PG has the copyright I have to hide some parts). This is the main example of grabstereo with addition of some lines of code to publish image from camera to ROS.

//=============================================================================
// grabstereo
//
// Gets input from the Bumblebee, and performs stereo processing
// to create a disparity image. A rectified image from the reference camera
// and the disparity image are both written out.
//
//=============================================================================

//=============================================================================
// System Includes
//=============================================================================
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

//=============================================================================
// PGR Includes
//=============================================================================
#include "triclops.h"
#include "fc2triclops.h"

#include "ros/ros.h"
#include "image_transport/image_transport.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/image_encodings.h"

#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"

.... Some hidden and unmodified code ....

image_transport::CameraPublisher pub_right;

int
main( int argc, char** argv)
{
    ros::init(argc, argv, "xb3_devel");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    pub_right = it.advertiseCamera("stereo_camera/right/image_rect", 1);

    TriclopsContext   triclops;
    TriclopsInput     triclopsInput;

    // camera to be used to grab color image
    FC2::Camera camera;
    // grabbed unprocessed image
    FC2::Image grabbedImage;

    // connect camera
    camera.Connect();

... same unmodified code until the end ...

    return EXIT_SUCCESS;
}

.... rest of the functions unmodified  ....

int doStereo( TriclopsContext const & triclops, 
                TriclopsInput const & triclopsInput )
{
    TriclopsImage disparityImage, edgeImage, rectifiedImage;
    TriclopsError te;

    // Rectify the images
    te = triclopsRectify( triclops, const_cast<TriclopsInput *>(&triclopsInput) );
    _HANDLE_TRICLOPS_ERROR( "triclopsRectify()", te );

    // Do stereo processing
    te = triclopsStereo( triclops );
    _HANDLE_TRICLOPS_ERROR( "triclopsStereo()", te );

    // Retrieve the disparity image from the triclops context
    te = triclopsGetImage( triclops, 
                          TriImg_DISPARITY, 
                          TriCam_REFERENCE, 
                          &disparityImage );
    _HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te );

    // Retrieve the rectified image from the triclops context
    te = triclopsGetImage( triclops, 
                          TriImg_RECTIFIED, 
                          TriCam_REFERENCE, 
                          &rectifiedImage );
    _HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te );

    //__ my code __________________________________________

    cv_bridge::CvImage cv_r_img;
    sensor_msgs::CameraInfo r_ci;
    sensor_msgs::Image img;

    cv::Mat rec_img(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data);
//    cv::namedWindow("Disp", cv::WINDOW_AUTOSIZE );
//    cv::imshow("Disp", rec_img);
//    cv::waitKey(0);
    cv_r_img.image = rec_img;
    cv_r_img.encoding = sensor_msgs::image_encodings::MONO8;
    r_ci.width = rectifiedImage.ncols;
    r_ci.height = rectifiedImage.nrows;

    cv_r_img.toImageMsg(img);

    pub_right.publish(img, r_ci);

    //____end of my code________________________________________


    // Retrieve the edge image from the triclops context
    te = triclopsGetImage( triclops, 
                          TriImg_EDGE, 
                          TriCam_REFERENCE, 
                          &edgeImage );
    _HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te );

    // Save the disparity, reference and edge images

    const char * pDisparityFilename = "disparity.pgm";
    te = triclopsSaveImage( &disparityImage, const_cast<char *>(pDisparityFilename) );
    _HANDLE_TRICLOPS_ERROR( "triclopsSaveImage()", te );

    const char * pEdgeFilename = "edge.pgm";
    te = triclopsSaveImage( &edgeImage, const_cast<char *>(pEdgeFilename) );
    _HANDLE_TRICLOPS_ERROR( "triclopsSaveImage()", te );

    const char * pRectifiedFilename = "rectified.pgm";
    te = triclopsSaveImage( &rectifiedImage, const_cast<char *>(pRectifiedFilename) );
    _HANDLE_TRICLOPS_ERROR( "triclopsSaveImage()", te );

    return 0;
}

From here is just a matter of time you work with the API and change the specific params to change the resolution, images, etc.

Hope someone need it and don't waste more time working with unrectified images. Or close good camera calibration.

Asked by pmarinplaza on 2014-10-09 03:41:45 UTC

Comments

You've got an already available driver for bumblebee2 here.

Asked by Miquel Massot on 2014-10-13 04:09:03 UTC

But the calibration is really bad with the standar package camera calibration.Due to the calibration and rectification step, PointGrey cameras are so expensve. They give you the calibration before left the production building but you have to use their software :(

Asked by pmarinplaza on 2014-11-07 06:13:23 UTC

I haven't checked, but can you use their soft to retrieve the calibration data inside?

Asked by Miquel Massot on 2014-11-10 02:47:19 UTC

You can do a lookup table to generate the points position and then aply to your image

Asked by pmarinplaza on 2015-04-08 05:46:36 UTC

Answers