ndtMapBuilder.cc
Go to the documentation of this file.
00001 #include <ndt_map_hmt.h>
00002 #include <oc_tree.h>
00003 #include <lazy_grid.h>
00004 #include <cell_vector.h>
00005 
00006 #include "ros/ros.h"
00007 #include "pcl/point_cloud.h"
00008 #include "sensor_msgs/PointCloud2.h"
00009 #include "pcl/io/pcd_io.h"
00010 #include "pcl/features/feature.h"
00011 #include <cstdio>
00012 
00013 #include <opencv/cv.h>
00014 #include <opencv/highgui.h>
00015 #include <boost/shared_ptr.hpp>
00016 #include <boost/thread/mutex.hpp>
00017 //#include <cv_bridge/CvBridge.h>
00018 #include <cv_bridge/cv_bridge.h>
00019 #include <sensor_msgs/Image.h>
00020 
00021 #include <Eigen/Eigen>
00022 
00023 static int ctr = 0, ctr2 = 0;
00024 static cv::Mat color_img;
00025 static boost::mutex mutex;
00026 static bool fresh = false;
00027 
00028 void ndtCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
00029 {
00030     pcl::PointCloud<pcl::PointXYZ> cloud;
00031     pcl::fromROSMsg(*msg,cloud);
00032 
00033     ROS_INFO ("Received %d data points with the following fields: %s", (int)(msg->width * msg->height),
00034               pcl::getFieldsList (*msg).c_str ());
00035 
00036     //lslgeneric::NDTMap<pcl::PointXYZ> nd(new lslgeneric::OctTree<pcl::PointXYZ>());
00037     //lslgeneric::NDTMap<pcl::PointXYZ> nd(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.2));
00038     lslgeneric::NDTMapHMT<pcl::PointXYZ> nd(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.2),0,0,0,10,10,2,3.0);
00039 
00040     Eigen::Affine3d T;
00041     T.setIdentity();
00042     nd.addPointCloud(T.translation(),cloud);
00043     nd.computeNDTCells();
00044 
00045     ROS_INFO("Loaded point cloud");
00046     nd.writeTo("maps/");
00047     ctr++;
00048 
00049 }
00050 
00051 void ndtCallbackDepthImg(const sensor_msgs::ImageConstPtr &msg)
00052 {
00053 
00054     cv::Mat depth_img(cv_bridge::toCvCopy(msg, "")->image);
00055     cv::Mat color_img_loc;
00056 
00057     mutex.lock();
00058     if(!fresh)
00059     {
00060         mutex.unlock();
00061         return;
00062     }
00063     color_img.copyTo(color_img_loc);
00064     mutex.unlock();
00065 
00066 
00067     ROS_INFO ("Received image data");
00068 
00069     //lslgeneric::NDTMap<pcl::PointXYZ> nd(new lslgeneric::OctTree<pcl::PointXYZ>());
00070     //lslgeneric::NDTMap<pcl::PointXYZ> nd(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.2));
00071     lslgeneric::NDTMap<pcl::PointXYZ> nd(new lslgeneric::CellVector<pcl::PointXYZ>());
00072 
00073     // TODO - this are the values from the Freiburg 1 camera.
00074     double fx = 517.3;
00075     double fy = 516.5;
00076     double cx = 318.6;
00077     double cy = 255.3;
00078     std::vector<double> dist(5);
00079     dist[0] = 0.2624;
00080     dist[1] = -0.9531;
00081     dist[2] = -0.0054;
00082     dist[3] = 0.0026;
00083     dist[4] = 1.1633;
00084     double ds = 1.035; // Depth scaling factor.
00085     bool isFloat = (depth_img.depth() == CV_32F);
00086     double scale = 1; //0.0002;
00087     if(!isFloat)
00088     {
00089         scale = 0.002; //raw scale conversion
00090     }
00091 
00092     lslgeneric::DepthCamera<pcl::PointXYZ> cameraParams (fx,fy,cx,cy,dist,ds*scale,isFloat);
00093 //   pcl::PointCloud<pcl::PointXYZ> pc;
00094 //   cameraParams.convertDepthImageToPointCloud(depth_img, pc);
00095 
00096     std::vector<cv::KeyPoint> kpts;
00097     cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create("SURF");
00098     detector->detect(color_img_loc, kpts);
00099 
00100     ROS_INFO("found %d keypoints",kpts.size());
00101     size_t support = 7;
00102     double maxVar = 0.3;
00103 //    nd.loadDepthImage(depth_img, cameraParams);
00104     //nd.loadDepthImageFeatures(depth_img, kpts, support, maxVar, cameraParams);
00105     nd.loadDepthImageFeatures(depth_img, kpts, support, maxVar, cameraParams,true);
00106     nd.computeNDTCells();
00107 
00108     lslgeneric::CellVector<pcl::PointXYZ> *cv = dynamic_cast<lslgeneric::CellVector<pcl::PointXYZ>* > (nd.getMyIndex());
00109     if(cv!=NULL)
00110     {
00111         cv->cleanCellsAboveSize(maxVar);
00112     }
00113 
00114     ROS_INFO("Loaded point cloud");
00115     char fname[50];
00116     snprintf(fname,49,"ndt_map%05d.wrl",ctr2);
00117     nd.writeToVRML(fname);
00118     /*    snprintf(fname,49,"cloud%05d.wrl",ctr2);
00119         lslgeneric::writeToVRML(fname,pc);
00120         snprintf(fname,49,"depth%05d.png",ctr2);
00121         cv::imwrite(fname,depth_img);
00122         snprintf(fname,49,"depth_raw%05d.png",ctr2);
00123         cv::imwrite(fname,depth_img_raw);
00124     */
00125     ctr2++;
00126     //std::exit(0);
00127 }
00128 
00129 void callColorImg(const sensor_msgs::ImageConstPtr &msg)
00130 {
00131 
00132     //sensor_msgs::CvBridge _imBridge;
00133     mutex.lock();
00134     fresh = true;
00135     //cv::Mat tmp_mat = cv::Mat(_imBridge.imgMsgToCv(msg, "mono8"));
00136     cv::Mat tmp_mat(cv_bridge::toCvCopy(msg, "mono8")->image);
00137     tmp_mat.copyTo(color_img);
00138     mutex.unlock();
00139 
00140 }
00141 
00142 int
00143 main (int argc, char** argv)
00144 {
00145 
00146     ros::init(argc, argv, "ndt_builder");
00147     ros::NodeHandle n;
00148     ros::Subscriber sub1 = n.subscribe("points2_in", 10, ndtCallback);
00149     ros::Subscriber sub2 = n.subscribe("depth_image", 10, ndtCallbackDepthImg);
00150     ros::Subscriber sub3 = n.subscribe("color_image", 10, callColorImg);
00151     ros::spin();
00152 
00153     return (0);
00154 }
00155 
00156 
00157 


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:31:57