How to use posest package in vlsam stack

edit

Hi, I have been trying to estimate the camera pose change between two stereo pairs using the pe::PoseEstimator3d class in the posest package. I am using the vslam_tutorial.bag bag file in order two obtain consecutive stereo pairs of images and stereo camera calibration parameters. Then, I use frame_common::FrameProc in order to extract keypoints from the images and finally the PoseEstimator3d is used to get the rigid transformation describing how the camera has moved. Nevertheless, either the matchings between stereo pairs or the estimated motion are pretty bad. Am I missing some step?

Please take a look at the code that I am using to test it:

#include <ros/ros.h>

#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>

#include <stereo_image_proc/processor.h>
#include <image_geometry/stereo_camera_model.h>

#include <posest/pe3d.h>

using namespace sensor_msgs;
using namespace stereo_msgs;
using namespace message_filters::sync_policies;

image_geometry::StereoCameraModel cam_model_;
cv::Mat_<uint8_t> l_image[2], r_image[2];
std::auto_ptr<pe::PoseEstimator3d> posest;
int imagePairCounter;

void imageCb(const ImageConstPtr& l_image_msg,
             const CameraInfoConstPtr& l_info_msg,
             const ImageConstPtr& r_image_msg,
             const CameraInfoConstPtr& r_info_msg)
{
  /// @todo Convert (share) with new cv_bridge
  assert(l_image_msg->encoding == sensor_msgs::image_encodings::MONO8);
  assert(r_image_msg->encoding == sensor_msgs::image_encodings::MONO8);

  // Update the camera model
  cam_model_.fromCameraInfo(l_info_msg, r_info_msg);

    // Create cv::Mat views onto all buffers
  l_image[imagePairCounter] = cv::Mat_<uint8_t>(l_image_msg->height, l_image_msg->width,
                                                const_cast<uint8_t*>(&l_image_msg->data[0]),
                                                l_image_msg->step);

  r_image[imagePairCounter] = cv::Mat_<uint8_t>(r_image_msg->height, r_image_msg->width,
                                                const_cast<uint8_t*>(&r_image_msg->data[0]),
                                                r_image_msg->step);

  ROS_INFO_STREAM("Stereo pair " << imagePairCounter << " available" );

  imagePairCounter = (imagePairCounter + 1) % 2;


  if ( imagePairCounter == 0 ) //we have two consecutive stereo pairs
  {
    ROS_INFO("Proceeding to estimate relative pose...");
    frame_common::CamParams cam_params;
    cam_params.fx = cam_model_.left().fx();
    cam_params.fy = cam_model_.left().fy();
    cam_params.cx = cam_model_.left().cx();
    cam_params.cy = cam_model_.left().cy();
    cam_params.tx = cam_model_.baseline();

    frame_common::FrameProc frameProc(50);
    frame_common::Frame frame[2];

    for (int i = 0; i < 2; ++i)
    {
      frame[i].setCamParams(cam_params);
      ROS_INFO_STREAM("Setting frame " << i);
      frameProc.setStereoFrame(frame[i], l_image[i], r_image[i]);
    }

    ROS_INFO("Estimating pose");
    posest->estimate(frame[0], frame[1]);

    ROS_INFO_STREAM("\t\tNumber of matches: " << posest->matches.size());
    ROS_INFO_STREAM("\t\tNumber of inliers: " << posest->inliers.size());
    ROS_INFO_STREAM("\t\tRotation:\n"         << posest->rot << "\n");
    ROS_INFO_STREAM("\t\tTranslation: ("      << posest->trans.transpose() << ")\n");


    cv::Mat imgDisplay;
    cv::drawMatches(frame[0].img, frame[0].kpts, frame[1].img, frame[1].kpts, posest->inliers, imgDisplay);
    cv::imshow("matches between two consecutive frames", imgDisplay);
    cv::waitKey(0);
  }
}

int main(int argc, char** argv) 
{
  ros::init(argc, argv, "pal_posest3d_from_topics_test_node");
  ros::NodeHandle nh;
  ROS_INFO("Starting...");

  posest.reset(new pe::PoseEstimator3d(50000, //ransac iterations
                                       true,  //use Levenberg-Marquardt polishing 
                                       10.0,   //min match disparity 10.0
                                       3.0,   //max inlier distance along x or y axis 3.0
                                       3.0    //max inlier depth distance 3.0
                                      ));

  //window size for matching keypoints between two images
  //posest.wx = 92; //92
  //posest.wy = 48; //48 ...