How to use posest package in vlsam stack
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 ...