void NormalDetector::computeSignal(const LaserReading& reading, std::vector<double>& signal, std::vector<unsigned int>& maxRangeMapping) const{
// const std::vector<Point2D>& points = reading.getCartesian();
std::vector<double> ranges;
ranges.reserve(reading.getRho().size());
maxRangeMapping.reserve(reading.getRho().size());
for(unsigned int i = 0; i < reading.getRho().size(); i++){
if(reading.getRho()[i] < reading.getMaxRange()){
ranges.push_back(reading.getRho()[i]);
maxRangeMapping.push_back(i);
} else if (m_useMaxRange){
ranges.push_back(reading.getMaxRange());
maxRangeMapping.push_back(i);
}
}
int offsetRange = floor((int)m_filterBank[0].size()/2.0);
const std::vector<double>& rangeData = convolve1D(ranges, m_filterBank[0], -offsetRange);
const std::vector<double>& phiData = reading.getPhi();
std::vector<Point2D> points(rangeData.size());
for(unsigned int i = 0; i < rangeData.size(); i++){
if(rangeData[i]<reading.getMaxRange()){
points[i].x = cos(phiData[maxRangeMapping[i]])*rangeData[i];
points[i].y = sin(phiData[maxRangeMapping[i]])*rangeData[i];
} else {
points[i].x = cos(phiData[maxRangeMapping[i]])*reading.getMaxRange();
points[i].y = sin(phiData[maxRangeMapping[i]])*reading.getMaxRange();
}
}
signal.resize(points.size());
unsigned int offset = floor((double)m_windowSize * 0.5);
std::vector<Point2D>::const_iterator first = points.begin();
std::vector<Point2D>::const_iterator last = first + m_windowSize;
double oldangle = 0;
for(unsigned int i = offset; i < signal.size() - offset; i++){
LineParameters param = computeNormals(std::vector<Point2D>(first,last));
signal[i] = normAngle(param.alpha, oldangle - M_PI);
oldangle = signal[i];
first++; last++;
}
for(unsigned int i = 0; i < offset; i++){
signal[i] = signal[offset];
}
for(unsigned int i = signal.size() - offset; i < signal.size(); i++){
signal[i] = signal[signal.size() - offset - 1];
}
}
