flirtlib: Regression.cpp Source File

00001 #include "Regression.h"
00002 
00003 LineParameters computeNormals(const std::vector<Point2D>& _points, const std::vector<double>& _weights){
00004     if (_weights.size()!=_points.size()) return computeNormals(_points);
00005     
00006     LineParameters result;
00007     
00008     double sumWeights = 0;
00009     for(unsigned int i = 0; i < _weights.size(); i++) sumWeights += _weights[i];
00010     
00011     double meanWeightX = 0, meanWeightY = 0;
00012     for(unsigned int i = 0; i < _weights.size(); i++) {
00013         meanWeightX += 1.0/(_weights[i]*_weights[i])* _points[i].x;
00014         meanWeightY += 1.0/(_weights[i]*_weights[i])* _points[i].y;
00015     }
00016     meanWeightX = meanWeightX / sumWeights;
00017     meanWeightY = meanWeightY / sumWeights;
00018     
00019     // alpha
00020     double nominator = 0, denominator = 0;
00021     for(unsigned int i = 0; i < _weights.size(); i++) {
00022         nominator += 1.0/(_weights[i]*_weights[i])*(_points[i].x - meanWeightX)*(_points[i].y - meanWeightY);
00023         denominator += 1.0/(_weights[i]*_weights[i])*((_points[i].y - meanWeightY)*(_points[i].y - meanWeightY) - 
00024             (_points[i].x - meanWeightX)*(_points[i].x - meanWeightX));
00025     }
00026     
00027     result.alpha = 0.5 * atan2(-2*nominator,denominator);
00028 
00029     // rho
00030     result.rho = meanWeightX * cos(result.alpha) + meanWeightY * sin(result.alpha);
00031     
00032     // normalize to get the solution facing the robot
00033     if(result.rho < 0){
00034         result.alpha += M_PI;
00035         result.rho = -result.rho;
00036     }
00037     
00038     return result;  
00039 }
00040 
00041 LineParameters computeNormals(const std::vector<Point2D>& _points) {
00042     return computeNormals(_points, std::vector<double>(_points.size(),1.0));
00043 }