187 lines
3.9 KiB
Text
187 lines
3.9 KiB
Text
/*
|
|
* pointfilter implementation
|
|
*
|
|
* Copyright (C) Dorit Borrmann, Jan Elseberg
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
#include "slam6d/pointfilter.h"
|
|
|
|
using std::string;
|
|
using std::map;
|
|
#include <sstream>
|
|
using std::stringstream;
|
|
#include <stdexcept>
|
|
using std::runtime_error;
|
|
|
|
#include <iostream>
|
|
using std::cout;
|
|
using std::endl;
|
|
|
|
|
|
|
|
map<string, Checker* (*)(const string&)>* PointFilter::factory = new map<string, Checker* (*)(const string&)>;
|
|
|
|
|
|
|
|
PointFilter::PointFilter() :
|
|
m_changed(true), m_checker(0)
|
|
{
|
|
}
|
|
|
|
PointFilter::PointFilter(const std::string& params) :
|
|
m_changed(true), m_checker(0)
|
|
{
|
|
size_t start = 0, end = string::npos;
|
|
while((end = params.find(' ', start)) != string::npos) {
|
|
// extract the word (start-end+1) without the space (-1)
|
|
string key(params.substr(start, start-end));
|
|
end++;
|
|
// get the second word position
|
|
start = params.find(' ', end);
|
|
// insert
|
|
m_params[key] = params.substr(end, (start-end));
|
|
// advance to the character after space
|
|
if(start != string::npos)
|
|
start++;
|
|
else
|
|
break;
|
|
}
|
|
}
|
|
|
|
PointFilter::~PointFilter()
|
|
{
|
|
if(m_checker)
|
|
delete m_checker;
|
|
}
|
|
|
|
PointFilter& PointFilter::setRange(double maxDist, double minDist)
|
|
{
|
|
m_changed = true;
|
|
stringstream s_max; s_max << maxDist;
|
|
stringstream s_min; s_min << minDist;
|
|
m_params["rangemax"] = s_max.str();
|
|
m_params["rangemin"] = s_min.str();
|
|
return *this;
|
|
}
|
|
|
|
PointFilter& PointFilter::setHeight(double top, double bottom)
|
|
{
|
|
m_changed = true;
|
|
stringstream s_top; s_top << top;
|
|
stringstream s_bottom; s_bottom << bottom;
|
|
m_params["heighttop"] = s_top.str();
|
|
m_params["heightbottom"] = s_bottom.str();
|
|
return *this;
|
|
}
|
|
|
|
std::string PointFilter::getParams()
|
|
{
|
|
stringstream s;
|
|
for(map<string, string>::iterator it = m_params.begin(); it != m_params.end(); ++it) {
|
|
s << (*it).first << " " << (*it).second << " ";
|
|
}
|
|
return s.str();
|
|
}
|
|
|
|
void PointFilter::createCheckers()
|
|
{
|
|
// delete the outdated ones
|
|
if(m_checker) {
|
|
delete m_checker;
|
|
m_checker = 0;
|
|
}
|
|
|
|
// create new ones
|
|
Checker** current = &m_checker;
|
|
for(map<string, string>::iterator it = m_params.begin(); it != m_params.end(); ++it) {
|
|
*current = (*factory)[it->first](it->second);
|
|
// if a Checker has been successfully created advance to its pointer in the chain
|
|
if(*current) {
|
|
current = &((*current)->m_next);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
Checker::Checker() :
|
|
m_next(0)
|
|
{
|
|
}
|
|
|
|
Checker::~Checker()
|
|
{
|
|
if(m_next)
|
|
delete m_next;
|
|
}
|
|
|
|
|
|
|
|
// create factory instaces for key string to factory function mapping
|
|
namespace {
|
|
CheckerFactory<CheckerRangeMax> max("rangemax");
|
|
CheckerFactory<CheckerRangeMin> min("rangemin");
|
|
CheckerFactory<CheckerHeightTop> top("heighttop");
|
|
CheckerFactory<CheckerHeightBottom> bottom("heightbottom");
|
|
}
|
|
|
|
|
|
|
|
CheckerRangeMax::CheckerRangeMax(const std::string& value) {
|
|
stringstream s(value);
|
|
s >> m_max;
|
|
// default value: no check
|
|
if(m_max <= 0.0) throw runtime_error("No range filter needed.");
|
|
m_max *= m_max;
|
|
}
|
|
|
|
bool CheckerRangeMax::test(double* point) {
|
|
if(point[0]*point[0] + point[1]*point[1] + point[2]*point[2] < m_max)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
CheckerRangeMin::CheckerRangeMin(const std::string& value) {
|
|
stringstream s(value);
|
|
s >> m_min;
|
|
// default value: no check
|
|
if(m_min <= 0.0) throw runtime_error("No range filter needed.");
|
|
m_min *= m_min;
|
|
}
|
|
|
|
bool CheckerRangeMin::test(double* point) {
|
|
if(point[0]*point[0] + point[1]*point[1] + point[2]*point[2] > m_min)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
CheckerHeightTop::CheckerHeightTop(const std::string& value) {
|
|
stringstream s(value);
|
|
s >> m_top;
|
|
}
|
|
|
|
bool CheckerHeightTop::test(double* point) {
|
|
if(point[1] < m_top)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
CheckerHeightBottom::CheckerHeightBottom(const std::string& value) {
|
|
stringstream s(value);
|
|
s >> m_bottom;
|
|
}
|
|
|
|
bool CheckerHeightBottom::test(double* point) {
|
|
if(point[1] > m_bottom)
|
|
return true;
|
|
return false;
|
|
}
|