tools.h
Go to the documentation of this file.
00001 #ifndef TOOLS
00002 #define TOOLS
00003 
00004 #include <ros/ros.h>
00005 #include <fstream>
00006 #include <pcl/common/common.h>
00007 #include <image_geometry/stereo_camera_model.h>
00008 #include <sensor_msgs/image_encodings.h>
00009 #include <sensor_msgs/Image.h>
00010 #include <stereo_msgs/DisparityImage.h>
00011 #include <cv_bridge/cv_bridge.h>
00012 #include <nav_msgs/Odometry.h>
00013 #include <boost/filesystem.hpp>
00014 #include <g2o/types/slam3d/vertex_se3.h>
00015 
00016 namespace enc = sensor_msgs::image_encodings;
00017 namespace fs  = boost::filesystem;
00018 
00019 using namespace std;
00020 
00021 namespace tools
00022 {
00023 
00024 class Tools
00025 {
00026 
00027 public:
00028 
00029   // Definitions
00030   typedef pcl::PointXYZRGB        Point;
00031   typedef pcl::PointCloud<Point>  PointCloud;
00032 
00033 
00038   static tf::Transform vector4fToTransform(Eigen::Vector4f in)
00039   {
00040     tf::Transform out;
00041     out.setIdentity();
00042     tf::Vector3 t_out(in[0], in[1], in[2]);
00043     out.setOrigin(t_out);
00044     return out;
00045   }
00046 
00052   static tf::Transform transformVector4f(Eigen::Vector4f in, tf::Transform transform)
00053   {
00054     tf::Transform in_tf = vector4fToTransform(in);
00055     return transform * in_tf;
00056   }
00057 
00062   static Eigen::Isometry3d tfToIsometry(tf::Transform in)
00063   {
00064     tf::Vector3 t_in = in.getOrigin();
00065     tf::Quaternion q_in = in.getRotation();
00066     Eigen::Vector3d t_out(t_in.x(), t_in.y(), t_in.z());
00067     Eigen::Quaterniond q_out;
00068     q_out.setIdentity();
00069     q_out.x() = q_in.x();
00070     q_out.y() = q_in.y();
00071     q_out.z() = q_in.z();
00072     q_out.w() = q_in.w();
00073     Eigen::Isometry3d out = (Eigen::Isometry3d)q_out;
00074     out.translation() = t_out;
00075     return out;
00076   }
00077 
00082   static tf::Transform isometryToTf(Eigen::Isometry3d in)
00083   {
00084     Eigen::Vector3d t_in = in.translation();
00085     Eigen::Quaterniond q_in = (Eigen::Quaterniond)in.rotation();
00086     tf::Vector3 t_out(t_in.x(), t_in.y(), t_in.z());
00087     tf::Quaternion q_out(q_in.x(), q_in.y(), q_in.z(), q_in.w());
00088     tf::Transform out(q_out, t_out);
00089     return out;
00090   }
00091 
00097   static tf::Transform odomTotf(nav_msgs::Odometry odom_msg)
00098   {
00099     // Get the data
00100     double tx = odom_msg.pose.pose.position.x;
00101     double ty = odom_msg.pose.pose.position.y;
00102     double tz = odom_msg.pose.pose.position.z;
00103 
00104     double qx = odom_msg.pose.pose.orientation.x;
00105     double qy = odom_msg.pose.pose.orientation.y;
00106     double qz = odom_msg.pose.pose.orientation.z;
00107     double qw = odom_msg.pose.pose.orientation.w;
00108 
00109     // Sanity check
00110     if(qx == 0.0 && qy == 0.0 && qz == 0.0 && qw == 1.0)
00111     {
00112       tf::Transform odom;
00113       odom.setIdentity();
00114       return odom;
00115     }
00116     else
00117     {
00118       tf::Vector3 tf_trans(tx, ty, tz);
00119       tf::Quaternion tf_q (qx, qy, qz, qw);
00120       tf::Transform odom(tf_q, tf_trans);
00121       return odom;
00122     }
00123   }
00124 
00131   static bool imgMsgToMat(sensor_msgs::Image l_img_msg,
00132                           sensor_msgs::Image r_img_msg,
00133                           cv::Mat &l_img, cv::Mat &r_img)
00134   {
00135     // Convert message to cv::Mat
00136     cv_bridge::CvImagePtr l_img_ptr, r_img_ptr;
00137     try
00138     {
00139       l_img_ptr = cv_bridge::toCvCopy(l_img_msg, enc::BGR8);
00140       r_img_ptr = cv_bridge::toCvCopy(r_img_msg, enc::BGR8);
00141     }
00142     catch (cv_bridge::Exception& e)
00143     {
00144       ROS_ERROR("[StereoSlam:] cv_bridge exception: %s", e.what());
00145       return false;
00146     }
00147 
00148     // Set the images
00149     l_img = l_img_ptr->image;
00150     r_img = r_img_ptr->image;
00151     return true;
00152   }
00153 
00160   static void getCameraModel(sensor_msgs::CameraInfo l_info_msg,
00161                              sensor_msgs::CameraInfo r_info_msg,
00162                              image_geometry::StereoCameraModel &stereo_camera_model,
00163                              cv::Mat &camera_matrix)
00164   {
00165     // Get the binning factors
00166     int binning_x = l_info_msg.binning_x;
00167     int binning_y = l_info_msg.binning_y;
00168 
00169     // Get the stereo camera model
00170     stereo_camera_model.fromCameraInfo(l_info_msg, r_info_msg);
00171 
00172     // Get the projection/camera matrix
00173     const cv::Mat P(3,4, CV_64FC1, const_cast<double*>(l_info_msg.P.data()));
00174     camera_matrix = P.colRange(cv::Range(0,3)).clone();
00175 
00176     // Are the images scaled?
00177     if (binning_x > 1 || binning_y > 1)
00178     {
00179       camera_matrix.at<double>(0,0) = camera_matrix.at<double>(0,0) / binning_x;
00180       camera_matrix.at<double>(0,2) = camera_matrix.at<double>(0,2) / binning_x;
00181       camera_matrix.at<double>(1,1) = camera_matrix.at<double>(1,1) / binning_y;
00182       camera_matrix.at<double>(1,2) = camera_matrix.at<double>(1,2) / binning_y;
00183     }
00184   }
00185 
00190   static tf::Transform getVertexPose(g2o::VertexSE3* v)
00191   {
00192     Eigen::Isometry3d pose_eigen = v->estimate();
00193     tf::Transform pose_tf = tools::Tools::isometryToTf(pose_eigen);
00194     return pose_tf;
00195   }
00196 
00202   static double poseDiff3D(tf::Transform pose_1, tf::Transform pose_2)
00203   {
00204     tf::Vector3 d = pose_1.getOrigin() - pose_2.getOrigin();
00205     return sqrt(d.x()*d.x() + d.y()*d.y() + d.z()*d.z());
00206   }
00207 
00213   static double poseDiff2D(tf::Transform pose_1, tf::Transform pose_2)
00214   {
00215     tf::Vector3 d = pose_1.getOrigin() - pose_2.getOrigin();
00216     return sqrt(d.x()*d.x() + d.y()*d.y());
00217   }
00218 
00224   static bool sortByMatching(const pair<int, float> d1, const pair<int, float> d2)
00225   {
00226     return (d1.second < d2.second);
00227   }
00228 
00234  static bool sortByDistance(const pair<int, double> d1, const pair<int, double> d2)
00235  {
00236    return (d1.second < d2.second);
00237  }
00238 
00245   static tf::Transform buildTransformation(cv::Mat rvec, cv::Mat tvec)
00246   {
00247     if (rvec.empty() || tvec.empty())
00248       return tf::Transform();
00249 
00250     tf::Vector3 axis(rvec.at<double>(0, 0),
00251                      rvec.at<double>(1, 0),
00252                      rvec.at<double>(2, 0));
00253     double angle = norm(rvec);
00254     tf::Quaternion quaternion(axis, angle);
00255 
00256     tf::Vector3 translation(tvec.at<double>(0, 0), tvec.at<double>(1, 0),
00257         tvec.at<double>(2, 0));
00258 
00259     return tf::Transform(quaternion, translation);
00260   }
00261 
00262   static cv::Point3f transformPoint(cv::Point3f point, tf::Transform base)
00263   {
00264     tf::Vector3 p_tf(point.x, point.y, point.z);
00265     tf::Vector3 p_tf_world = base * p_tf;
00266     cv::Point3f new_point(p_tf_world.x(), p_tf_world.y(), p_tf_world.z());
00267     return new_point;
00268   }
00269 
00276   static void ratioMatching(cv::Mat desc_1, cv::Mat desc_2, double ratio, vector<cv::DMatch> &matches)
00277   {
00278     matches.clear();
00279     if (desc_1.rows < 10 || desc_2.rows < 10) return;
00280 
00281     cv::Mat match_mask;
00282     const int knn = 2;
00283     cv::Ptr<cv::DescriptorMatcher> descriptor_matcher;
00284     descriptor_matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
00285     vector<vector<cv::DMatch> > knn_matches;
00286     descriptor_matcher->knnMatch(desc_1, desc_2, knn_matches, knn, match_mask);
00287     for (uint m=0; m<knn_matches.size(); m++)
00288     {
00289       if (knn_matches[m].size() < 2) continue;
00290       if (knn_matches[m][0].distance <= knn_matches[m][1].distance * ratio)
00291         matches.push_back(knn_matches[m][0]);
00292     }
00293   }
00294 
00295   static string convertTo5digits(int in)
00296   {
00297     uint val = (uint)in;
00298     string output;
00299     int digits = 5;
00300     while (digits-- > 0) {
00301         output += ('0' + val % 10);
00302         val /= 10;
00303     }
00304     reverse(output.begin(), output.end());
00305     return output;
00306   }
00307 };
00308 
00309 } // namespace
00310 
00311 #endif // TOOLS


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jul 14 2016 04:09:13