ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to use posest package in vlsam stack

asked 2011-06-09 02:14:09 -0600

Jordi Pages gravatar image

updated 2011-06-14 21:37:15 -0600

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,

  r_image[imagePairCounter] = cv::Mat_<uint8_t>(r_image_msg->height, r_image_msg->width,

  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_model_.left().cx(); = 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)
      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);

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

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


I am having seg faults when using posest. They occur somewhere in the call to detect() inside FrameProc::setMonoFrame() in vslam/frame_common/src/frame.cpp. I got seg fault using commented out detector as well. Don't have OpenCV source right now so that's as far as I got. Using vslam bag file.
Thomas D gravatar image Thomas D  ( 2011-07-19 12:20:14 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2011-06-17 11:48:44 -0600

ethanrublee gravatar image

There are so many variables in doing vslam that you need to examine each step individually to discover sources of error. You do not appear to be doing anything incorrectly. The vslam package is still very experimental.

edit flag offensive delete link more

answered 2011-09-28 17:59:50 -0600

JollyGood gravatar image

Have you tested the run_posest.cpp under test directory? It should work well.

edit flag offensive delete link more

Question Tools


Asked: 2011-06-09 02:14:09 -0600

Seen: 511 times

Last updated: Sep 28 '11