int main() { int D = 0; int N = 0; /* int[] arr1 = [0.1666, 0.1666, 0.1666, 0.1666, 0.1666, 0.1666]; */ int in[] = {1,2,3,4,5,6}; int out[] = {0,0,0,0,0,0,0,0,0,0,0}; convolve1D(in, out, 6, in, 6); int i; for(i=0; i < 11; i++) { printf("out[%d] = %d\n",i, out[i]); } while(scanf("%D %D", &D, &H)) { } }
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]; } }