odometer_base.hpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/image_encodings.h>
00003 #include <image_geometry/pinhole_camera_model.h>
00004 #include <cv_bridge/cv_bridge.h>
00005 #include <nav_msgs/Odometry.h>
00006 #include <geometry_msgs/PoseStamped.h>
00007 
00008 #include <fovis_ros/FovisInfo.h>
00009 
00010 #include <visual_odometry.hpp>
00011 #include <stereo_depth.hpp>
00012 
00013 #include <tf/transform_listener.h>
00014 #include <tf/transform_broadcaster.h>
00015 
00016 #include "visualization.hpp"
00017 
00018 namespace fovis_ros
00019 {
00020 
00024 class OdometerBase
00025 {
00026 
00027 protected:
00028 
00029   OdometerBase() : 
00030     visual_odometer_(NULL),
00031     rectification_(NULL),
00032     depth_source_(NULL),
00033     visual_odometer_options_(fovis::VisualOdometry::getDefaultOptions()),
00034     nh_local_("~"),
00035     it_(nh_local_)
00036   {
00037     loadParams();
00038     odom_pub_ = nh_local_.advertise<nav_msgs::Odometry>("odometry", 1);
00039     pose_pub_ = nh_local_.advertise<geometry_msgs::PoseStamped>("pose", 1);
00040     info_pub_ = nh_local_.advertise<FovisInfo>("info", 1);
00041     features_pub_ = it_.advertise("features", 1);
00042   }
00043 
00044   virtual ~OdometerBase()
00045   {
00046     if (visual_odometer_) delete visual_odometer_;
00047     if (rectification_) delete rectification_;
00048   }
00049 
00050   const fovis::VisualOdometryOptions& getOptions() const
00051   {
00052     return visual_odometer_options_;
00053   }
00054 
00058   void setDepthSource(fovis::DepthSource* source)
00059   {
00060     depth_source_ = source;
00061   }
00062 
00063   static void rosToFovis(const image_geometry::PinholeCameraModel& camera_model,
00064       fovis::CameraIntrinsicsParameters& parameters)
00065   {
00066     parameters.cx = camera_model.cx();
00067     parameters.cy = camera_model.cy();
00068     parameters.fx = camera_model.fx();
00069     parameters.fy = camera_model.fy();
00070     parameters.width = camera_model.reducedResolution().width;
00071     parameters.height = camera_model.reducedResolution().height;
00072   }
00073 
00078   void process(
00079       const sensor_msgs::ImageConstPtr& image_msg, 
00080       const sensor_msgs::CameraInfoConstPtr& info_msg)
00081   {
00082     ros::WallTime start_time = ros::WallTime::now();
00083 
00084     bool first_run = false;
00085     if (visual_odometer_ == NULL)
00086     {
00087       first_run = true;
00088       initOdometer(info_msg);
00089     }
00090     ROS_ASSERT(visual_odometer_ != NULL);
00091     ROS_ASSERT(depth_source_ != NULL);
00092 
00093     // convert image if necessary
00094     uint8_t *image_data;
00095     cv_bridge::CvImageConstPtr cv_ptr = 
00096       cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8);
00097     image_data = cv_ptr->image.data;
00098     int step = cv_ptr->image.step[0];
00099 
00100     ROS_ASSERT(step == static_cast<int>(image_msg->width));
00101 
00102     // pass image to odometer
00103     visual_odometer_->processFrame(image_data, depth_source_);
00104 
00105     // skip visualization on first run as no reference image is present
00106     if (!first_run && features_pub_.getNumSubscribers() > 0)
00107     {
00108       cv_bridge::CvImage cv_image;
00109       cv_image.header.stamp = image_msg->header.stamp;
00110       cv_image.header.frame_id = image_msg->header.frame_id;
00111       cv_image.encoding = sensor_msgs::image_encodings::BGR8;
00112       cv_image.image = visualization::paint(visual_odometer_);
00113       features_pub_.publish(cv_image.toImageMsg());
00114     }
00115 
00116     // create odometry and pose messages
00117     odom_msg_.header.stamp = image_msg->header.stamp;
00118     odom_msg_.header.frame_id = odom_frame_id_;
00119     odom_msg_.child_frame_id = base_link_frame_id_;
00120     
00121     pose_msg_.header.stamp = image_msg->header.stamp;
00122     pose_msg_.header.frame_id = base_link_frame_id_;
00123 
00124     // on success, start fill message and tf
00125     fovis::MotionEstimateStatusCode status = 
00126       visual_odometer_->getMotionEstimateStatus();
00127     if (status == fovis::SUCCESS)
00128     {
00129       // get pose and motion from odometer
00130       const Eigen::Isometry3d& pose = visual_odometer_->getPose();
00131       tf::Transform sensor_pose;
00132       eigenToTF(pose, sensor_pose);
00133       // calculate transform of odom to base based on base to sensor 
00134       // and sensor to sensor
00135       tf::StampedTransform current_base_to_sensor;
00136       getBaseToSensorTransform(
00137           image_msg->header.stamp, image_msg->header.frame_id, 
00138           current_base_to_sensor);
00139       tf::Transform base_transform = 
00140         initial_base_to_sensor_ * sensor_pose * current_base_to_sensor.inverse();
00141 
00142       // publish transform
00143       if (publish_tf_)
00144       {
00145         tf_broadcaster_.sendTransform(
00146             tf::StampedTransform(base_transform, image_msg->header.stamp,
00147             odom_frame_id_, base_link_frame_id_));
00148       }
00149 
00150       // fill odometry and pose msg
00151       tf::poseTFToMsg(base_transform, odom_msg_.pose.pose);
00152       pose_msg_.pose = odom_msg_.pose.pose;
00153 
00154       // can we calculate velocities?
00155       double dt = last_time_.isZero() ? 
00156         0.0 : (image_msg->header.stamp - last_time_).toSec();
00157       if (dt > 0.0)
00158       {
00159         const Eigen::Isometry3d& motion = visual_odometer_->getMotionEstimate();
00160         tf::Transform sensor_motion;
00161         eigenToTF(motion, sensor_motion);
00162         // in theory the first factor would have to be base_to_sensor of t-1
00163         // and not of t (irrelevant for static base to sensor anyways)
00164         tf::Transform delta_base_transform = 
00165           current_base_to_sensor * sensor_motion * current_base_to_sensor.inverse();
00166         // calculate twist from delta transform
00167         odom_msg_.twist.twist.linear.x = delta_base_transform.getOrigin().getX() / dt;
00168         odom_msg_.twist.twist.linear.y = delta_base_transform.getOrigin().getY() / dt;
00169         odom_msg_.twist.twist.linear.z = delta_base_transform.getOrigin().getZ() / dt;
00170         tf::Quaternion delta_rot = delta_base_transform.getRotation();
00171         double angle = delta_rot.getAngle();
00172         tf::Vector3 axis = delta_rot.getAxis();
00173         tf::Vector3 angular_twist = axis * angle / dt;
00174         odom_msg_.twist.twist.angular.x = angular_twist.x();
00175         odom_msg_.twist.twist.angular.y = angular_twist.y();
00176         odom_msg_.twist.twist.angular.z = angular_twist.z();
00177 
00178         // add covariance
00179         const Eigen::MatrixXd& motion_cov = visual_odometer_->getMotionEstimateCov();
00180         for (int i=0;i<6;i++)
00181           for (int j=0;j<6;j++)
00182             odom_msg_.twist.covariance[j*6+i] = motion_cov(i,j);
00183       }
00184       // TODO integrate covariance for pose covariance
00185       last_time_ = image_msg->header.stamp;
00186     }
00187     else
00188     {
00189       // Previous messages with the current timestamp will be published
00190       ROS_WARN_STREAM("fovis odometry status: " << 
00191           fovis::MotionEstimateStatusCodeStrings[status]);
00192       last_time_ = ros::Time(0);
00193     }
00194     odom_pub_.publish(odom_msg_);
00195     pose_pub_.publish(pose_msg_);
00196 
00197     // create and publish fovis info msg
00198     FovisInfo fovis_info_msg;
00199     fovis_info_msg.header.stamp = image_msg->header.stamp;
00200     fovis_info_msg.change_reference_frame = 
00201       visual_odometer_->getChangeReferenceFrames();
00202     fovis_info_msg.fast_threshold =
00203       visual_odometer_->getFastThreshold();
00204     const fovis::OdometryFrame* frame = 
00205       visual_odometer_->getTargetFrame();
00206     fovis_info_msg.num_total_detected_keypoints =
00207       frame->getNumDetectedKeypoints();
00208     fovis_info_msg.num_total_keypoints = frame->getNumKeypoints();
00209     fovis_info_msg.num_detected_keypoints.resize(frame->getNumLevels());
00210     fovis_info_msg.num_keypoints.resize(frame->getNumLevels());
00211     for (int i = 0; i < frame->getNumLevels(); ++i)
00212     {
00213       fovis_info_msg.num_detected_keypoints[i] =
00214         frame->getLevel(i)->getNumDetectedKeypoints();
00215       fovis_info_msg.num_keypoints[i] =
00216         frame->getLevel(i)->getNumKeypoints();
00217     }
00218     const fovis::MotionEstimator* estimator = 
00219       visual_odometer_->getMotionEstimator();
00220     fovis_info_msg.motion_estimate_status_code =
00221       estimator->getMotionEstimateStatus();
00222     fovis_info_msg.motion_estimate_status = 
00223       fovis::MotionEstimateStatusCodeStrings[
00224         fovis_info_msg.motion_estimate_status_code];
00225     fovis_info_msg.num_matches = estimator->getNumMatches();
00226     fovis_info_msg.num_inliers = estimator->getNumInliers();
00227     fovis_info_msg.num_reprojection_failures =
00228       estimator->getNumReprojectionFailures();
00229     fovis_info_msg.motion_estimate_valid = 
00230       estimator->isMotionEstimateValid();
00231     ros::WallDuration time_elapsed = ros::WallTime::now() - start_time;
00232     fovis_info_msg.runtime = time_elapsed.toSec();
00233     info_pub_.publish(fovis_info_msg);
00234   }
00235 
00236 
00237 private:
00238 
00242   void initOdometer(const sensor_msgs::CameraInfoConstPtr& info_msg)
00243   {
00244     // create rectification
00245     image_geometry::PinholeCameraModel model;
00246     model.fromCameraInfo(info_msg);
00247     fovis::CameraIntrinsicsParameters cam_params;
00248     rosToFovis(model, cam_params);
00249     fovis::Rectification* rectification = new fovis::Rectification(cam_params);
00250 
00251     // instanciate odometer
00252     visual_odometer_ = 
00253       new fovis::VisualOdometry(rectification, visual_odometer_options_);
00254 
00255     // store initial transform for later usage
00256     getBaseToSensorTransform(info_msg->header.stamp, 
00257         info_msg->header.frame_id,
00258         initial_base_to_sensor_);
00259 
00260     // print options
00261     std::stringstream info;
00262     info << "Initialized fovis odometry with the following options:\n";
00263     for (fovis::VisualOdometryOptions::iterator iter = visual_odometer_options_.begin();
00264         iter != visual_odometer_options_.end();
00265         ++iter)
00266     {
00267       std::string key = iter->first;
00268       std::replace(key.begin(), key.end(), '-', '_');
00269       info << key << " = " << iter->second << std::endl;
00270     }
00271     ROS_INFO_STREAM(info.str());
00272   }
00273 
00277   void loadParams()
00278   {
00279     nh_local_.param("odom_frame_id", odom_frame_id_, std::string("/odom"));
00280     nh_local_.param("base_link_frame_id", base_link_frame_id_, std::string("/base_link"));
00281     nh_local_.param("publish_tf", publish_tf_, true);
00282 
00283     for (fovis::VisualOdometryOptions::iterator iter = visual_odometer_options_.begin();
00284         iter != visual_odometer_options_.end();
00285         ++iter)
00286     {
00287       // NOTE: this only accepts parameters if given through
00288       // launch files with the argument "type" set to "string":
00289       //   e.g. <param name="fast_threshold_adaptive_gain" type="string" value="0.001"/>
00290       // Passing parameters through the command line does not work as rosparam
00291       // automagically treats numbers as int or float and we need strings here.
00292       std::string key = iter->first;
00293       // to comply with ROS standard of parameter naming
00294       std::replace(key.begin(), key.end(), '-', '_');
00295       if (nh_local_.hasParam(key))
00296       {
00297         std::string value;
00298         nh_local_.getParam(key, value);
00299         visual_odometer_options_[iter->first] = value;
00300       }
00301     }
00302   }
00303 
00304   void getBaseToSensorTransform(const ros::Time& stamp, 
00305       const std::string& sensor_frame_id, tf::StampedTransform& base_to_sensor)
00306   {
00307     std::string error_msg;
00308     if (tf_listener_.canTransform(
00309           base_link_frame_id_, sensor_frame_id, stamp, &error_msg))
00310     {
00311       tf_listener_.lookupTransform(
00312           base_link_frame_id_,
00313           sensor_frame_id,
00314           stamp, base_to_sensor);
00315     }
00316     else
00317     {
00318       ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be "
00319                               "available, will assume it as identity!", 
00320                               base_link_frame_id_.c_str(),
00321                               sensor_frame_id.c_str());
00322       ROS_DEBUG("Transform error: %s", error_msg.c_str());
00323       base_to_sensor.setIdentity();
00324     }
00325   }
00326 
00327   void eigenToTF(const Eigen::Isometry3d& pose, tf::Transform& transform)
00328   {
00329     tf::Vector3 origin(
00330         pose.translation().x(), pose.translation().y(), pose.translation().z());
00331     Eigen::Quaterniond rotation(pose.rotation());
00332     tf::Quaternion quat(rotation.x(), rotation.y(),
00333         rotation.z(), rotation.w());
00334     transform = tf::Transform(quat, origin);
00335   }
00336 
00337 
00338 private:
00339 
00340   fovis::VisualOdometry* visual_odometer_;
00341   fovis::Rectification* rectification_;
00342   fovis::DepthSource* depth_source_;
00343   fovis::VisualOdometryOptions visual_odometer_options_;
00344 
00345   ros::Time last_time_;
00346 
00347   // tf related
00348   std::string sensor_frame_id_;
00349   std::string odom_frame_id_;
00350   std::string base_link_frame_id_;
00351   bool publish_tf_;
00352   tf::StampedTransform initial_base_to_sensor_;
00353   tf::TransformListener tf_listener_;
00354   tf::TransformBroadcaster tf_broadcaster_;
00355   
00356   // Messages
00357   nav_msgs::Odometry odom_msg_;
00358   geometry_msgs::PoseStamped pose_msg_;
00359 
00360   ros::NodeHandle nh_local_;
00361 
00362   // publisher
00363   ros::Publisher odom_pub_;
00364   ros::Publisher pose_pub_;
00365   ros::Publisher info_pub_;
00366   image_transport::Publisher features_pub_;
00367   image_transport::ImageTransport it_;
00368 };
00369 
00370 } // end of namespace
00371 


fovis_ros
Author(s): Stephan Wirth
autogenerated on Fri Sep 12 2014 12:12:54