//* Line extraction from the robot sensors - 9/24/2010 2:09AM *

#include "nav_robot2lines.h" void NAV_FindLine(const ArPose& RP, const std::vector<ArPoseWithTime>* ECHOS, double anglefrom, double angleto, NAV_Line &line) { NAV_Quadrants Q(RP); // need to classify point positions around the robot // line unsigned int line_n = 0; double line_sx = 0.0; double line_sxx = 0.0; double line_sy = 0.0; double line_syy = 0.0; double line_sxy = 0.0; for (std::vector<ArPoseWithTime>::const_iterator it=ECHOS->begin(); it!=ECHOS->end(); ++it) { double x=it->getX(), y=it->getY(); double angle = Q.findAngleTo(*it); if (anglefrom<=angle && angle<=angleto) { line_n++; line_sx += x; line_sxx += x*x; line_sy += y; line_syy += y*y; line_sxy += x*y; } } if (line_n>3) { double num = line_sxy - line_sx*line_sy/line_n; double den = line_sxx - line_sx*line_sx/line_n; line.setABC( num, -den, line_sy*den/line_n-num*line_sx/line_n ); line.normalize(); } else { line.reset(); } } void NAV_FindCurrentLine(const ArPose& RP, ArRangeDevice& S, double anglefrom, double angleto, NAV_Line &line) { S.lockDevice(); // if forget to lock the exported data may be corrupt const std::vector<ArPoseWithTime>* ECHOS = S.getCurrentBufferAsVector(); S.unlockDevice(); // if forget to lock the exported data may be corrupt NAV_FindLine(RP, ECHOS, anglefrom, angleto, line); } void NAV_FindCumulativeLine(const ArPose& RP, ArRangeDevice& S, double anglefrom, double angleto, NAV_Line &line) { S.lockDevice(); // if forget to lock the exported data may be corrupt const std::vector<ArPoseWithTime>* ECHOS = S.getCumulativeBufferAsVector(); S.unlockDevice(); // if forget to lock the exported data may be corrupt NAV_FindLine(RP, ECHOS, anglefrom, angleto, line); } void NAV_FindLines(const ArPose& RP, const std::vector<ArPoseWithTime>* ECHOS, NAV_Line* left, NAV_Line* right, NAV_Line* ahead, NAV_Line* back, NAV_Line* frontleft, NAV_Line* frontright ) { NAV_Quadrants Q(RP); // need to classify point positions around the robot // line left unsigned int lln = 0; double llsx = 0.0; double llsxx = 0.0; double llsy = 0.0; double llsyy = 0.0; double llsxy = 0.0; // line right unsigned int lrn = 0; double lrsx = 0.0; double lrsxx = 0.0; double lrsy = 0.0; double lrsyy = 0.0; double lrsxy = 0.0; // line ahead unsigned int lan = 0; double lasx = 0.0; double lasxx = 0.0; double lasy = 0.0; double lasyy = 0.0; double lasxy = 0.0; // line back unsigned int lbn = 0; double lbsx = 0.0; double lbsxx = 0.0; double lbsy = 0.0; double lbsyy = 0.0; double lbsxy = 0.0; // line front left unsigned int lfln = 0; double lflsx = 0.0; double lflsxx = 0.0; double lflsy = 0.0; double lflsyy = 0.0; double lflsxy = 0.0; // line front right unsigned int lfrn = 0; double lfrsx = 0.0; double lfrsxx = 0.0; double lfrsy = 0.0; double lfrsyy = 0.0; double lfrsxy = 0.0; for (std::vector<ArPoseWithTime>::const_iterator it=ECHOS->begin(); it!=ECHOS->end(); ++it) { double x=it->getX(), y=it->getY(); if (Q.isInFront(x,y)) { if (ahead!=NULL) { lan++; lasx += x; lasxx += x*x; lasy += y; lasyy += y*y; lasxy += x*y; } if (Q.isToLeft(x,y)) { if (frontleft!=NULL) { lfln++; lflsx += x; lflsxx += x*x; lflsy += y; lflsyy += y*y; lflsxy += x*y; } } else { if (frontright!=NULL) { lfrn++; lfrsx += x; lfrsxx += x*x; lfrsy += y; lfrsyy += y*y; lfrsxy += x*y; } } } else { if (back!=NULL) { lbn++; lbsx += x; lbsxx += x*x; lbsy += y; lbsyy += y*y; lbsxy += x*y; } } if (Q.isToLeft(x,y)) { if (left!=NULL) { lln++; llsx += x; llsxx += x*x; llsy += y; llsyy += y*y; llsxy += x*y; } } else { if (right!=NULL) { lrn++; lrsx += x; lrsxx += x*x; lrsy += y; lrsyy += y*y; lrsxy += x*y; } } } if (ahead!=NULL) if (lan>3) { double num = lasxy - lasx*lasy/lan; double den = lasxx - lasx*lasx/lan; ahead->setABC( num, -den, lasy*den/lan-num*lasx/lan ); ahead->normalize(); } else { ahead->reset(); } if (back!=NULL) if (lbn>3) { double num = lbsxy - lbsx*lbsy/lbn; double den = lbsxx - lbsx*lbsx/lbn; back->setABC( num, -den, lbsy*den/lbn-num*lbsx/lbn ); back->normalize(); } else { back->reset(); } if (left!=NULL) if (lln>3) { double num = llsxy - llsx*llsy/lln; double den = llsxx - llsx*llsx/lln; left->setABC( num, -den, llsy*den/lln-num*llsx/lln ); left->normalize(); } else { left->reset(); } if (right!=NULL) if (lrn>3) { double num = lrsxy - lrsx*lrsy/lrn; double den = lrsxx - lrsx*lrsx/lrn; right->setABC( num, -den, lrsy*den/lrn-num*lrsx/lrn ); right->normalize(); } else { right->reset(); } if (frontleft!=NULL) if (lfln>3) { double num = lflsxy - lflsx*lflsy/lfln; double den = lflsxx - lflsx*lflsx/lfln; frontleft->setABC( num, -den, lflsy*den/lfln-num*lflsx/lfln ); frontleft->normalize(); } else { frontleft->reset(); } if (frontright!=NULL) if (lrn>3) { double num = lfrsxy - lfrsx*lfrsy/lfrn; double den = lfrsxx - lfrsx*lfrsx/lfrn; frontright->setABC( num, -den, lfrsy*den/lfrn-num*lfrsx/lfrn ); frontright->normalize(); } else { frontright->reset(); } } void NAV_FindCurrentLines(const ArPose& RP, ArRangeDevice& S, NAV_Line* left, NAV_Line* right, NAV_Line* ahead, NAV_Line* back, NAV_Line* frontleft, NAV_Line* frontright ) { S.lockDevice(); // if forget to lock the exported data may be corrupt const std::vector<ArPoseWithTime>* ECHOS = S.getCurrentBufferAsVector(); S.unlockDevice(); // if forget to lock the exported data may be corrupt NAV_FindLines(RP, ECHOS, left, right, ahead, back, frontleft, frontright); } void NAV_FindCumulativeLines(const ArPose& RP, ArRangeDevice& S, NAV_Line* left, NAV_Line* right, NAV_Line* ahead, NAV_Line* back, NAV_Line* frontleft, NAV_Line* frontright ) { S.lockDevice(); // if forget to lock the exported data may be corrupt const std::vector<ArPoseWithTime>* ECHOS = S.getCumulativeBufferAsVector(); S.unlockDevice(); // if forget to lock the exported data may be corrupt NAV_FindLines(RP, ECHOS, left, right, ahead, back, frontleft, frontright); }