Merge branch 'master' of gitsvn-nt.oru.se:software/ndt_core

ndt_generic/include/ndt_generic/pcl_utils.h

@@ -15,6 +15,9 @@
 #include <pcl/PCLPointCloud2.h>
 #include <pcl_ros/transforms.h>
 #include "laser_geometry/laser_geometry.h"
+#include "queue"
+#include "ndt_generic/utils.h"
+#include "pcl/io/pcd_io.h"
 namespace ndt_generic {
 
 template <class PointT>
@@ -67,6 +70,29 @@ void filter_height_angle(pcl::PointCloud<PointT> &cloud,
 
 template <class PointT>
 void AddVariance(pcl::PointCloud<PointT> &cloud, double varz=0.05);
-} // namespace
+
+
+template <class PointT>
+class PointCloudQueue
+{
+public:
+
+  PointCloudQueue(size_t size=10) {max_size_=size;}
+
+  void Push(const pcl::PointCloud<PointT> &cloud);
+
+  void GetCloud(pcl::PointCloud<PointT> &cloud);
+
+  void Save( const std::string &name_prefix="cloud_");
+
+private:
+  size_t max_size_;
+  std::vector<pcl::PointCloud<PointT> > clouds;
+  unsigned int cloud_set_=0;
+
+};
+
+  } // namespace
+
 #include "ndt_generic/pcl_utils_impl.h"
 

ndt_generic/include/ndt_generic/pcl_utils_impl.h

@@ -140,6 +140,26 @@ template <class PointT> void AddVariance(pcl::PointCloud<PointT> &cloud, double
   for(int i=0; i<cloud.points.size(); i++)
     cloud.points[i].z+= varz*(((double)rand())/(double)RAND_MAX-0.5);
 }
+template <class PointT> void PointCloudQueue<PointT>::Push(const pcl::PointCloud<PointT> &cloud){
+  if(clouds.size()==max_size_){
+    //std::cout<<"queue size="<<max_size_<<" popping last element"<<std::endl;
+    clouds.erase(clouds.begin());
+  }
 
+  clouds.push_back(cloud);
+}
+template <class PointT> void PointCloudQueue<PointT>::GetCloud(pcl::PointCloud<PointT> &cloud){
+  cloud.clear();
+  for(int i=0 ; i<clouds.size() ; i++){
+    cloud+=clouds[i];
+  }
+  //std::cout<<"Got "<<cloud.size() <<" points"<<std::endl;
+}
+template <class PointT> void PointCloudQueue<PointT>::Save( const std::string &name_prefix){
+  static pcl::PCDWriter w;
+  pcl::PointCloud<PointT> cloud_save;
+  GetCloud(cloud_save);
+  w.writeBinary(name_prefix+ndt_generic::toString(++cloud_set_)+".pcd", cloud_save);
+}
 }
 #endif // PCL_UTILS_IMPL_H

ndt_generic/src/sensors_utils.cpp

@@ -49,7 +49,7 @@ bool GetSensorPose(const std::string &dataset,  Eigen::Vector3d & transl,  Eigen
     transl[2]=-0.957;
     euler[0]=0.807*M_PI/180;
     euler[1]=0.166*M_PI/180;
-    euler[2]=0;//-90.703*M_PI/180;//-1.625
+    euler[2]=-90.703*M_PI/180;//-1.625
     found_sensor_pose=true;
   }
   else if(dataset.compare("coop-2013")==0){

ndt_map/include/ndt_map/ndt_map.h

@@ -161,6 +161,7 @@ public:
     //si was allocated outside the NDT class and should be deallocated outside
     isFirstLoad_=!dealloc;//////////////////////////////////////////////////////////////////////////////false; Henrik - was false, but why?
 
+
     NDTCell *ptCell = new NDTCell();
     index_->setCellType(ptCell);
     delete ptCell;
@@ -190,7 +191,7 @@ public:
   {
     isFirstLoad_=false;
     
-   std::cout << " EIGEN " << EIGEN_DEFAULT_ALIGN_BYTES << " " << EIGEN_MALLOC_ALREADY_ALIGNED << std::endl;
+   //std::cout << " EIGEN " << EIGEN_DEFAULT_ALIGN_BYTES << " " << EIGEN_MALLOC_ALREADY_ALIGNED << std::endl;
 
 //    exit(0);
     NDTCell *ptCell = new NDTCell();

ndt_map/src/ndt_map.cpp

@@ -2592,7 +2592,10 @@ return ss.str();
 double L2_Score(NDTMap *target, std::vector<NDTCell*> source){
 
   double score=1;
-  if(source.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
+  if(source.size()==0){
+    fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
+    return 0;
+  }
 
   for(int n=0;n<source.size();n++){
     Eigen::Vector3d m = source[n]->getMean();

ndt_rviz/src/ndt_display.cpp

@@ -16,7 +16,6 @@
 namespace perception_oru {
 
 NDTDisplay::NDTDisplay() {
-  ROS_ERROR("BUILDING OBJECT");
   color_property_ = new rviz::ColorProperty(
       "Color", QColor(204, 51, 204), "Color to draw the acceleration arrows.",
       this, SLOT(updateColorAndAlpha()));