triclops/flycapture for bumblebee stereo

asked 2014-10-09 03:41:45 -0600

pmarinplaza gravatar image

updated 2014-10-09 03:45:17 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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

Miquel Massot gravatar imageMiquel Massot ( 2014-10-13 04:09:03 -0600 )edit

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 :(

pmarinplaza gravatar imagepmarinplaza ( 2014-11-07 05:13:23 -0600 )edit

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

Miquel Massot gravatar imageMiquel Massot ( 2014-11-10 01:47:19 -0600 )edit

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

pmarinplaza gravatar imagepmarinplaza ( 2015-04-08 05:46:36 -0600 )edit