ndt_frame_viewer.hpp
Go to the documentation of this file.
00001 #include <pointcloud_utils.h>
00002 
00003 namespace ndt_feature_reg
00004 {
00005 
00006 template <typename PointT>
00007 NDTFrameViewer<PointT>::NDTFrameViewer(NDTFrameProc<PointT> *proc)
00008 {
00009     //_frames.push_back(f);
00010     _proc = proc;
00011     initViewer();
00012 }
00013 /*
00014 template <typename PointT>
00015 NDTFrameViewer<PointT>::NDTFrameViewer(NDTFrame<PointT> *f0, NDTFrame<PointT> *f1, NDTFrameProc<PointT> &proc) : _proc(proc)
00016 {
00017      _frames.push_back(f0);
00018      _frames.push_back(f1);
00019      initViewer();
00020 } */
00021 
00022 template <typename PointT>
00023 void
00024 NDTFrameViewer<PointT>::initViewer()
00025 {
00026     _viewer.reset (new pcl::visualization::PCLVisualizer ("3D Viewer"));
00027     _viewer->setBackgroundColor( 0, 0, 0.01 );
00028     _viewer->addCoordinateSystem(1.0);
00029 }
00030 
00031 /*
00032 template <typename PointT>
00033 void
00034 NDTFrameViewer<PointT>::showPC()
00035 {
00036      for (size_t i = 0; i < _proc.frames.size(); i++)
00037      {
00038           std::string name = "cloud" + toString(i);
00039           pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp;
00040           *tmp = _proc.frames[i].get
00041           = _proc.createColoredFeaturePC(*(_frames[i]), getPCLColor(i));
00042           _viewer->addPointCloud<PointT> (pc.makeShared(), name);
00043      }
00044 }*/
00045 
00046 template <typename PointT>
00047 void
00048 NDTFrameViewer<PointT>::showFeaturePC()
00049 {
00050 
00051     pcl::PointCloud<pcl::PointXYZRGB>::Ptr big_cloud (new pcl::PointCloud<pcl::PointXYZRGB>());
00052     std::string name = "featcloud"; // + toString(i);
00053     for (size_t i = 0; i < _proc->frames.size(); i++)
00054     {
00055         pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = _proc->createColoredFeaturePC(*_proc->frames[i], this->getPCLColor(i));
00056         lslgeneric::transformPointCloudInPlace(_proc->transformVector[i],*tmp);
00057 
00058         //lslgeneric::writeToVRML(name.c_str(),*tmp);
00059         // TODO -> check why this is needed here!?!
00060         //pcl::io::savePCDFile ("tmp.pcd", *tmp);
00061         //pcl::io::loadPCDFile ("tmp.pcd", *tmp);
00062 
00063         *big_cloud += *tmp;
00064     }
00065     big_cloud->is_dense = false;
00066     pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(big_cloud);
00067     _viewer->addPointCloud<pcl::PointXYZRGB> (big_cloud, rgb, name);
00068     lslgeneric::writeToVRML(name.c_str(),*big_cloud);
00069 }
00070 
00071 template <typename PointT>
00072 void
00073 NDTFrameViewer<PointT>::showNDT()
00074 {
00075 
00076 }
00077 
00078 template <typename PointT>
00079 void
00080 NDTFrameViewer<PointT>::showMatches(const std::vector<std::pair<int,int> > &matches)
00081 {
00082     assert(_proc->frames.size() == 2);
00083 
00084     for (size_t i = 0; i < matches.size(); i++)
00085     {
00086         const PointT& pt1 = _proc->frames[0]->getKeyPointCenter(matches[i].first);
00087         const PointT& pt2 = _proc->frames[1]->getKeyPointCenter(matches[i].second);
00088 
00089         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T = _proc->transformVector[1];
00090         Eigen::Vector3d pt;
00091         pt<<pt2.x,pt2.y,pt2.z;
00092         pt = T*pt;
00093         PointT pt3;
00094         pt3.x = pt(0);
00095         pt3.y = pt(1);
00096         pt3.z = pt(2);
00097 
00098         std::string name = "line" + toString(i);
00099         _viewer->addLine<PointT, PointT>(pt1, pt3, 0, 1, 1, name);
00100     }
00101 }
00102 
00103 template <typename PointT>
00104 void
00105 NDTFrameViewer<PointT>::showMatches(const std::vector<cv::DMatch> &matches)
00106 {
00107     std::vector<std::pair<int,int> > tmp;
00108     _proc->convertMatches(matches, tmp);
00109     this->showMatches(tmp);
00110 }
00111 
00112 template <typename PointT>
00113 bool
00114 NDTFrameViewer<PointT>::wasStopped()
00115 {
00116     return _viewer->wasStopped();
00117 }
00118 
00119 template <typename PointT>
00120 void
00121 NDTFrameViewer<PointT>::spinOnce()
00122 {
00123     _viewer->spinOnce(100);
00124 }
00125 
00126 }


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