visualization.cpp
Go to the documentation of this file.
00001 #include <opencv2/highgui/highgui.hpp>
00002 #include <opencv2/imgproc/imgproc.hpp>
00003 #include <visual_odometry.hpp>
00004 #include <iostream>
00005 
00006 #include "visualization.hpp"
00007 
00008 
00009 void _drawPyramidLevelMatches(const fovis::VisualOdometry* odometry, int level, 
00010     cv::Mat& canvas)
00011 {
00012   using namespace fovis;
00013 
00014   cv::Mat target_canvas(canvas.rowRange(0, canvas.rows/2));
00015   cv::Mat reference_canvas(canvas.rowRange(canvas.rows/2, canvas.rows));
00016  
00017   const PyramidLevel* reference_level = odometry->getReferenceFrame()->getLevel(0);
00018   for (int i = 0; i < reference_level->getNumKeypoints(); ++i)
00019   {
00020     const KeyPoint& kp = reference_level->getKeypoint(i);
00021     cv::Point2f center(kp.u, kp.v);
00022     cv::circle(reference_canvas, center, (level+1)*10, cv::Scalar(255, 0, 0));
00023   }
00024   const PyramidLevel* target_level = odometry->getTargetFrame()->getLevel(0);
00025   for (int i = 0; i < target_level->getNumKeypoints(); ++i)
00026   {
00027     const KeyPoint& kp = target_level->getKeypoint(i);
00028     cv::Point2f center(kp.u, kp.v);
00029     cv::circle(target_canvas, center, (level+1)*10, cv::Scalar(255, 0, 0));
00030   }
00031 }
00032 
00033 void _drawMatch(const fovis::FeatureMatch& match, cv::Mat& canvas)
00034 {
00035   using namespace fovis;
00036   cv::Mat target_canvas(canvas.rowRange(0, canvas.rows/2));
00037   cv::Mat reference_canvas(canvas.rowRange(canvas.rows/2, canvas.rows));
00038   const KeyPoint& target_keypoint = match.target_keypoint->kp;
00039   const KeyPoint& ref_keypoint = match.ref_keypoint->kp;
00040   int target_level = match.target_keypoint->pyramid_level;
00041   int ref_level = match.ref_keypoint->pyramid_level;
00042   cv::Point2f ref_center(
00043       ref_keypoint.u*(ref_level+1), ref_keypoint.v*(ref_level+1));
00044   cv::Point2f target_center(
00045       target_keypoint.u*(target_level+1), target_keypoint.v*(target_level+1));
00046   cv::Scalar color(0, 255, 0);
00047   if (!match.inlier)
00048     color = cv::Scalar(0, 0, 255);
00049   cv::circle(reference_canvas, ref_center, 
00050       (match.ref_keypoint->pyramid_level+1)*10, color);
00051   cv::circle(target_canvas, target_center, 
00052       (match.target_keypoint->pyramid_level+1)*10, color);
00053   cv::Point2f global_ref_center(ref_center.x, ref_center.y + canvas.rows/2);
00054   cv::line(canvas, target_center, global_ref_center, color);
00055   // motion flow
00056   // cv::line(canvas, target_center, ref_center, color);
00057 }
00058 
00059 void _drawKeypoint(const fovis::KeypointData& kp_data, cv::Mat& canvas)
00060 {
00061   cv::Point2f center(kp_data.rect_base_uv.x(), kp_data.rect_base_uv.y());
00062   cv::Scalar color;
00063   if (kp_data.has_depth)
00064   {
00065     color = cv::Scalar(255, 0, 0);
00066   }
00067   else
00068   {
00069     color = cv::Scalar(0, 0, 0);
00070   }
00071   cv::circle(canvas, center, (kp_data.pyramid_level+1)*10, color);
00072 }
00073 
00074 template<typename T>
00075 std::string toStr(T t)
00076 {
00077   std::stringstream ss;
00078   ss << t;
00079   return ss.str();
00080 }
00081 
00082 std::vector<std::string> _createInfoStrings(const fovis::VisualOdometry* odometry)
00083 {
00084   std::vector<std::string> infostrings;
00085   infostrings.push_back(std::string("Status: ") + fovis::MotionEstimateStatusCodeStrings[odometry->getMotionEstimateStatus()]);
00086   infostrings.push_back(toStr(odometry->getTargetFrame()->getNumDetectedKeypoints()) + " keypoints");
00087   infostrings.push_back(toStr(odometry->getTargetFrame()->getNumKeypoints()) + " filtered keypoints");
00088   infostrings.push_back(toStr(odometry->getMotionEstimator()->getNumMatches()) + " matches");
00089   infostrings.push_back(toStr(odometry->getMotionEstimator()->getNumInliers()) + " inliers");
00090   return infostrings;
00091 }
00092 
00093 cv::Mat fovis_ros::visualization::paint(const fovis::VisualOdometry* odometry)
00094 {
00095   using namespace fovis;
00096   const OdometryFrame* reference_frame = odometry->getReferenceFrame();
00097   const OdometryFrame* target_frame = odometry->getTargetFrame();
00098 
00099   int width = target_frame->getLevel(0)->getWidth();
00100   int height = target_frame->getLevel(0)->getHeight();
00101 
00102   // We have to const cast here because there is no 
00103   // cv::Mat constructor for const data.
00104   // The data will be copied later anyways.
00105   const cv::Mat reference_image(height, width, CV_8U, 
00106       const_cast<unsigned char*>(
00107         reference_frame->getLevel(0)->getGrayscaleImage()));
00108   const cv::Mat target_image(height, width, CV_8U,
00109       const_cast<unsigned char*>(
00110         target_frame->getLevel(0)->getGrayscaleImage()),
00111       target_frame->getLevel(0)->getGrayscaleImageStride());
00112 
00113   cv::Mat canvas(2*height, width, CV_8U);
00114   cv::Mat upper_canvas(canvas.rowRange(0, height));
00115   cv::Mat lower_canvas(canvas.rowRange(height, 2*height));
00116   target_image.copyTo(upper_canvas);
00117   reference_image.copyTo(lower_canvas);
00118   cv::cvtColor(canvas, canvas, CV_GRAY2BGR);
00119 
00120   for (int level = 0; level < reference_frame->getNumLevels(); ++level)
00121   {
00122     const PyramidLevel* pyramid_level = reference_frame->getLevel(level);
00123     for (int i = 0; i < pyramid_level->getNumKeypoints(); ++i)
00124     {
00125       _drawKeypoint(*(pyramid_level->getKeypointData(i)), canvas);
00126     }
00127   }
00128       
00129   const MotionEstimator* motion_estimator = odometry->getMotionEstimator(); 
00130   for (int i = 0; i < motion_estimator->getNumMatches(); ++i)
00131   {
00132     _drawMatch(motion_estimator->getMatches()[i], canvas);
00133   }
00134   std::vector<std::string> infostrings = _createInfoStrings(odometry);
00135   for (size_t i = 0; i < infostrings.size(); ++i)
00136   {
00137     cv::putText(canvas, infostrings[i], cv::Point(10, 40*(i + 1)),
00138           CV_FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 255), 3);
00139   }
00140   return canvas;
00141 }
00142 
00143 


fovis_ros
Author(s): Stephan Wirth
autogenerated on Mon Sep 8 2014 10:17:46