triclops/flycapture for bumblebee stereo

edit

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 ...