Commit 1c1729c4 authored by Tomasz Kucner's avatar Tomasz Kucner
Browse files

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

parents 3c1be695 859eb9c9
......@@ -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"
......@@ -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
......@@ -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){
......
......@@ -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();
......
This diff is collapsed.
......@@ -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();
......
......@@ -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()));
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment