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
00056
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
00103
00104
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