rangereading.cpp
Go to the documentation of this file.
00001 #include <limits>
00002 #include <iostream>
00003 #include <assert.h>
00004 #include <sys/types.h>
00005 #include <gmapping/utils/gvalues.h>
00006 #include <gmapping/sensor/sensor_range/rangereading.h>
00007 
00008 namespace GMapping{
00009 
00010 using namespace std;
00011 
00012 RangeReading::RangeReading(const RangeSensor* rs, double time):
00013         SensorReading(rs,time){}
00014 
00015 RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time):
00016         SensorReading(rs,time){
00017         assert(n_beams==rs->beams().size());
00018         resize(n_beams);
00019         for (unsigned int i=0; i<size(); i++)
00020                 (*this)[i]=d[i];
00021 }
00022 
00023 RangeReading::~RangeReading(){
00024 //      cerr << __PRETTY_FUNCTION__ << ": CAZZZZZZZZZZZZZZZZZZZZOOOOOOOOOOO" << endl;
00025 }
00026 
00027 unsigned int RangeReading::rawView(double* v, double density) const{
00028         if (density==0){
00029                 for (unsigned int i=0; i<size(); i++)
00030                         v[i]=(*this)[i];
00031         } else {
00032                 Point lastPoint(0,0);
00033                 uint suppressed=0;
00034                 for (unsigned int i=0; i<size(); i++){
00035                         const RangeSensor* rs=dynamic_cast<const RangeSensor*>(getSensor());
00036                         assert(rs);
00037                         Point lp(
00038                                 cos(rs->beams()[i].pose.theta)*(*this)[i],
00039                                 sin(rs->beams()[i].pose.theta)*(*this)[i]);
00040                         Point dp=lastPoint-lp;
00041                         double distance=sqrt(dp*dp);
00042                         if (distance<density){
00043                           //                            v[i]=MAXDOUBLE;
00044                                 v[i]=std::numeric_limits<double>::max();
00045                                 suppressed++;
00046                         }
00047                         else{
00048                                 lastPoint=lp;
00049                                 v[i]=(*this)[i];
00050                         }
00051                         //std::cerr<< __PRETTY_FUNCTION__ << std::endl;
00052                         //std::cerr<< "suppressed " << suppressed <<"/"<<size() << std::endl;
00053                 }
00054         }
00055         //      return size();
00056         return static_cast<unsigned int>(size());
00057 
00058 };
00059 
00060 unsigned int RangeReading::activeBeams(double density) const{
00061         if (density==0.)
00062                 return size();
00063                 int ab=0;
00064         Point lastPoint(0,0);
00065         uint suppressed=0;
00066         for (unsigned int i=0; i<size(); i++){
00067                 const RangeSensor* rs=dynamic_cast<const RangeSensor*>(getSensor());
00068                 assert(rs);
00069                 Point lp(
00070                         cos(rs->beams()[i].pose.theta)*(*this)[i],
00071                         sin(rs->beams()[i].pose.theta)*(*this)[i]);
00072                 Point dp=lastPoint-lp;
00073                 double distance=sqrt(dp*dp);
00074                 if (distance<density){
00075                         suppressed++;
00076                 }
00077                 else{
00078                         lastPoint=lp;
00079                         ab++;
00080                 }
00081                 //std::cerr<< __PRETTY_FUNCTION__ << std::endl;
00082                 //std::cerr<< "suppressed " << suppressed <<"/"<<size() << std::endl;
00083         }
00084         return ab;
00085 }
00086 
00087 std::vector<Point> RangeReading::cartesianForm(double maxRange) const{
00088         const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>(getSensor());
00089         assert(rangeSensor && rangeSensor->beams().size());
00090         //      uint m_beams=rangeSensor->beams().size();
00091         uint m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
00092         std::vector<Point> cartesianPoints(m_beams);
00093         double px,py,ps,pc;
00094         px=rangeSensor->getPose().x;
00095         py=rangeSensor->getPose().y;
00096         ps=sin(rangeSensor->getPose().theta);
00097         pc=cos(rangeSensor->getPose().theta);
00098         for (unsigned int i=0; i<m_beams; i++){
00099                 const double& rho=(*this)[i];
00100                 const double& s=rangeSensor->beams()[i].s;
00101                 const double& c=rangeSensor->beams()[i].c;
00102                 if (rho>=maxRange){
00103                         cartesianPoints[i]=Point(0,0);
00104                 } else {
00105                         Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho);
00106                         cartesianPoints[i].x=px+pc*p.x-ps*p.y;
00107                         cartesianPoints[i].y=py+ps*p.x+pc*p.y;
00108                 }
00109         }
00110         return cartesianPoints;
00111 }
00112 
00113 };
00114 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21