ndt_frame_viewer.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of AASS Research Center nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 #ifndef NDTFRAMEVIEWER_HH
00036 #define NDTFRAMEVIEWER_HH
00037 
00038 #include <ndt_feature_reg/ndt_frame.h>
00039 #include <ndt_feature_reg/ndt_frame_proc.h>
00040 #include "pcl/visualization/pcl_visualizer.h"
00041 
00042 
00043 namespace ndt_feature_reg
00044 {
00045 template <typename PointT>
00046 inline void viewKeypointMatches(NDTFrameProc<PointT> *proc, int delay)
00047 {
00048     if (proc->frames.size() < 2)
00049         return;
00050     cv::Mat display;
00051     int i = proc->frames.size()-1;
00052     cv::drawMatches(proc->frames[i-1]->img, proc->frames[i-1]->kpts, proc->frames[i]->img, proc->frames[i]->kpts, proc->pe.inliers, display);
00053     const std::string window_name = "matches";
00054     cv::namedWindow(window_name,0);
00055     cv::imshow(window_name, display);
00056     cv::waitKey(delay);
00057 }
00058 template <typename PointT>
00059 inline void viewKeypointMatchesFirst(NDTFrameProc<PointT> *proc, int delay)
00060 {
00061     if (proc->frames.size() < 2)
00062         return;
00063     cv::Mat display;
00064     int i = proc->frames.size()-1;
00065     cv::drawMatches(proc->frames[0]->img, proc->frames[0]->kpts, proc->frames[i]->img, proc->frames[i]->kpts, proc->pe.inliers, display);
00066     const std::string window_name = "matches";
00067     cv::namedWindow(window_name,0);
00068     cv::imshow(window_name, display);
00069     cv::waitKey(delay);
00070 }
00071 
00072 template <typename PointT>
00073 class NDTFrameViewer
00074 {
00075 public:
00076     NDTFrameViewer(NDTFrameProc<PointT> *proc);
00077     //NDTFrameViewer(NDTFrame<PointT> *f0, NDTFrame<PointT> *f1, NDTFrameProc<PointT> &proc);
00078     void showPC();
00079     void showFeaturePC();
00080     void showNDT();
00081     void showMatches(const std::vector<cv::DMatch> &matches);
00082     void showMatches(const std::vector<std::pair<int,int> > &matches);
00083     boost::shared_ptr<pcl::visualization::PCLVisualizer>& getViewerPtr()
00084     {
00085         return _viewer;
00086     }
00087     bool wasStopped();
00088     void spinOnce();
00089 private:
00090     void initViewer();
00091     //std::vector<NDTFrame<PointT>*> _frames;
00092     boost::shared_ptr<pcl::visualization::PCLVisualizer> _viewer;
00093     NDTFrameProc<PointT> *_proc;
00094 
00095     inline pcl::PointXYZRGB getPCLColor(int r, int g, int b)
00096     {
00097         pcl::PointXYZRGB ret;
00098         ret.r = r;
00099         ret.g = g;
00100         ret.b = b;
00101         return ret;
00102     }
00103 
00104     inline pcl::PointXYZRGB getPCLColor(size_t i)
00105     {
00106         pcl::PointXYZRGB ret;
00107 
00108         switch (i)
00109         {
00110         case 0:
00111             ret.r = 255;
00112             ret.g = 0;
00113             ret.b = 0;
00114             return ret;
00115         case 1:
00116             ret.r = 0;
00117             ret.g = 255;
00118             ret.b = 0;
00119             return ret;
00120         case 2:
00121             ret.r = 0;
00122             ret.g = 0;
00123             ret.b = 255;
00124             return ret;
00125         default:
00126             ret.r = 0;
00127             ret.g = 0;
00128             ret.b = 0;
00129         }
00130         return ret;
00131     }
00132 
00133 };
00134 }
00135 #include <ndt_feature_reg/impl/ndt_frame_viewer.hpp>
00136 
00137 #endif


ndt_feature_reg
Author(s): Henrik Andreasson, Todor Stoyanov
autogenerated on Mon Jan 6 2014 11:36:19