mono_vslam_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <image_transport/image_transport.h>
00003 #include <image_transport/subscriber_filter.h>
00004 #include <message_filters/subscriber.h>
00005 #include <message_filters/time_synchronizer.h>
00006 #include <dynamic_reconfigure/server.h>
00007 
00008 #include <cv_bridge/CvBridge.h>
00009 #include <image_geometry/pinhole_camera_model.h>
00010 #include <opencv2/legacy/legacy.hpp>
00011 
00012 #include <vslam_system/vslam_mono.h>
00013 #include <sba/visualization.h>
00014 #include <vslam_system/any_detector.h>
00015 #include <vslam_system/StereoVslamNodeConfig.h>
00016 
00017 #include <tf/transform_broadcaster.h>
00018 #include <tf/transform_listener.h>
00019 #include <nav_msgs/Odometry.h>
00020 #include <geometry_msgs/PoseStamped.h>
00021 
00022 static const double PI = 3.14159265;
00023 
00024 class StereoVslamNode
00025 {
00026 private:
00027   ros::NodeHandle nh_;
00028   image_transport::ImageTransport it_;
00029 
00030   // Subscriptions
00031   image_transport::SubscriberFilter l_image_sub_;
00032   message_filters::Subscriber<sensor_msgs::CameraInfo> l_info_sub_;
00033   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
00034 
00035   // Publications
00036   ros::Publisher cam_marker_pub_;
00037   ros::Publisher point_marker_pub_;
00038   image_transport::CameraPublisher vo_tracks_pub_;
00039   cv::Mat vo_display_;
00040   ros::Publisher odom_pub_;
00041   tf::TransformBroadcaster tf_broadcast_;
00042   tf::TransformListener tf_listener_;
00043   tf::Transform tf_transform_;
00044 
00045   // Processing state
00046   sensor_msgs::CvBridge l_bridge_, r_bridge_;
00047   image_geometry::PinholeCameraModel cam_model_;
00048   vslam::VslamSystemMono vslam_system_;
00049   cv::Ptr<cv::FeatureDetector> detector_;
00050 
00051   typedef dynamic_reconfigure::Server<vslam_system::StereoVslamNodeConfig> ReconfigureServer;
00052   ReconfigureServer reconfigure_server_;
00053 
00054 public:
00055 
00056   StereoVslamNode(const std::string& vocab_tree_file, const std::string& vocab_weights_file,
00057                   const std::string& calonder_trees_file)
00058     : it_(nh_), sync_(1),
00059       vslam_system_(vocab_tree_file, vocab_weights_file),
00060       detector_(new vslam_system::AnyDetector)
00061   {
00062     // Use calonder descriptor
00063     typedef cv::CalonderDescriptorExtractor<float> Calonder;
00064     vslam_system_.frame_processor_.setFrameDescriptor(new Calonder(calonder_trees_file));
00065     
00066     // Advertise outputs
00067     cam_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/cameras", 1);
00068     point_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/points", 1);
00069     vo_tracks_pub_ = it_.advertiseCamera("/vslam/vo_tracks/image", 1);
00070     odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/vo", 1);
00071 
00072     // Synchronize inputs
00073     l_image_sub_.subscribe(it_, "image_rect", 1);
00074     l_info_sub_ .subscribe(nh_, "camera_info", 1);
00075     sync_.connectInput(l_image_sub_, l_info_sub_);
00076     sync_.registerCallback( boost::bind(&StereoVslamNode::imageCb, this, _1, _2) );
00077 
00078     // Dynamic reconfigure for parameters
00079     ReconfigureServer::CallbackType f = boost::bind(&StereoVslamNode::configCb, this, _1, _2);
00080     reconfigure_server_.setCallback(f);
00081 
00082   }
00083 
00084   void configCb(vslam_system::StereoVslamNodeConfig& config, uint32_t level)
00085   {
00086     dynamic_cast<vslam_system::AnyDetector*>((cv::FeatureDetector*)detector_)->update(config);
00087     vslam_system_.frame_processor_.detector = detector_;
00088 
00089     vslam_system_.setPRRansacIt(config.pr_ransac_iterations);
00090     vslam_system_.setPRPolish(config.pr_polish);
00091     vslam_system_.setVORansacIt(config.vo_ransac_iterations);
00092     vslam_system_.setVOPolish(config.vo_polish);
00093   }
00094 
00095 
00096   void imageCb(const sensor_msgs::ImageConstPtr& l_image,
00097                const sensor_msgs::CameraInfoConstPtr& l_cam_info)
00098   {
00099     ROS_INFO("In callback, seq = %u", l_cam_info->header.seq);
00100     
00101     // Convert ROS messages for use with OpenCV
00102     cv::Mat left;
00103     try 
00104     {
00105       left  = l_bridge_.imgMsgToCv(l_image, "mono8");
00106     }
00107     catch (sensor_msgs::CvBridgeException& e) 
00108     {
00109       ROS_ERROR("Conversion error: %s", e.what());
00110       return;
00111     }
00112     cam_model_.fromCameraInfo(l_cam_info);
00113 
00114     frame_common::CamParams cam_params;
00115     cam_params.fx = cam_model_.fx();
00116     cam_params.fy = cam_model_.fy();
00117     cam_params.cx = cam_model_.cx();
00118     cam_params.cy = cam_model_.cy();
00119     cam_params.tx = 0.0;
00120 
00121     if (vslam_system_.addFrame(cam_params, left)) {
00123       int size = vslam_system_.sba_.nodes.size();
00124       if (size % 2 == 0) {
00125         // publish markers
00126         sba::drawGraph(vslam_system_.sba_, cam_marker_pub_, point_marker_pub_);
00127       }
00128 
00129       // Publish VO tracks
00130       if (vo_tracks_pub_.getNumSubscribers() > 0) {
00131         frame_common::drawVOtracks(left, vslam_system_.vo_.frames, vo_display_);
00132         IplImage ipl = vo_display_;
00133         sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(&ipl);
00134         msg->header = l_cam_info->header;
00135         vo_tracks_pub_.publish(msg, l_cam_info);
00136       }
00137       
00138       // Refine large-scale SBA.
00139       const int LARGE_SBA_INTERVAL = 10;
00140       if (size > 4 && size % LARGE_SBA_INTERVAL == 0) {
00141         ROS_INFO("Running large SBA on %d nodes", size);
00142         vslam_system_.refine();
00143       }
00144       
00145       // Publish odometry data to tf.
00146       ros::Time stamp = l_cam_info->header.stamp;
00147       std::string image_frame = l_cam_info->header.frame_id;
00148       Eigen::Vector4d trans = -vslam_system_.sba_.nodes.back().trans;
00149       Eigen::Quaterniond rot = vslam_system_.sba_.nodes.back().qrot.conjugate();
00150       
00151       trans.head<3>() = rot.toRotationMatrix()*trans.head<3>(); 
00152       
00153       tf_transform_.setOrigin(tf::Vector3(trans(0), trans(1), trans(2)));
00154       tf_transform_.setRotation(tf::Quaternion(rot.x(), rot.y(), rot.z(), rot.w()) );
00155       
00156       tf::Transform simple_transform;
00157       simple_transform.setOrigin(tf::Vector3(0, 0, 0));
00158       simple_transform.setRotation(tf::Quaternion(.5, -.5, .5, .5));
00159       
00160       tf_broadcast_.sendTransform(tf::StampedTransform(tf_transform_, stamp, image_frame, "visual_odom"));
00161       tf_broadcast_.sendTransform(tf::StampedTransform(simple_transform, stamp, "visual_odom", "pgraph"));
00162       
00163       
00164       
00165       // Publish odometry data on topic.
00166       if (odom_pub_.getNumSubscribers() > 0)
00167       {
00168         tf::StampedTransform base_to_image;
00169         tf::Transform base_to_visodom;
00170        
00171         try
00172         {
00173           tf_listener_.lookupTransform(image_frame, "/base_footprint",
00174                                stamp, base_to_image);
00175         }
00176         catch (tf::TransformException ex)
00177         {
00178             ROS_WARN("%s",ex.what());
00179             return;
00180         }
00181 
00182                                
00183         base_to_visodom = tf_transform_.inverse() * base_to_image;
00184         
00185         geometry_msgs::PoseStamped pose;
00186         nav_msgs::Odometry odom;
00187         pose.header.frame_id = "/visual_odom";
00188         pose.pose.position.x = base_to_visodom.getOrigin().x();
00189         pose.pose.position.y = base_to_visodom.getOrigin().y();
00190         pose.pose.position.z = base_to_visodom.getOrigin().z();
00191         pose.pose.orientation.x = base_to_visodom.getRotation().x();
00192         pose.pose.orientation.y = base_to_visodom.getRotation().y();
00193         pose.pose.orientation.z = base_to_visodom.getRotation().z();
00194         pose.pose.orientation.w = base_to_visodom.getRotation().w();
00195         
00196         odom.header.stamp = stamp;
00197         odom.header.frame_id = "/visual_odom";
00198         odom.child_frame_id = "/base_footprint";
00199         odom.pose.pose = pose.pose;
00200         /* odom.pose.covariance[0] = 1;
00201         odom.pose.covariance[7] = 1;
00202         odom.pose.covariance[14] = 1;
00203         odom.pose.covariance[21] = 1;
00204         odom.pose.covariance[28] = 1;
00205         odom.pose.covariance[35] = 1; */
00206         odom_pub_.publish(odom);
00207       }
00208     }
00209   }
00210 };
00211 
00212 int main(int argc, char** argv) {
00213   ros::init(argc, argv, "stereo_vslam");
00214   if (argc < 4) {
00215     printf("Usage: %s <vocab tree file> <vocab weights file> <calonder trees file>\n", argv[0]);
00216     return 1;
00217   }
00218   
00219   StereoVslamNode vslam(argv[1], argv[2], argv[3]);
00220   ros::spin();
00221 }


vslam_system
Author(s): Kurt Konolige, Patrick Mihelich, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:31