Ask Your Question
1

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

Comments

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 imageThomas D ( 2011-07-19 12:20:14 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
0

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
0

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

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

Seen: 416 times

Last updated: Sep 28 '11