rangereading.cpp
Go to the documentation of this file.
1 #include <limits>
2 #include <iostream>
3 #include <assert.h>
4 #include <sys/types.h>
7 
8 namespace GMapping{
9 
10 using namespace std;
11 
12 RangeReading::RangeReading(const RangeSensor* rs, double time):
13  SensorReading(rs,time){}
14 
15 RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time):
16  SensorReading(rs,time){
17  assert(n_beams==rs->beams().size());
18  resize(n_beams);
19  for (unsigned int i=0; i<size(); i++)
20  (*this)[i]=d[i];
21 }
22 
24 // cerr << __func__ << ": CAZZZZZZZZZZZZZZZZZZZZOOOOOOOOOOO" << endl;
25 }
26 
27 unsigned int RangeReading::rawView(double* v, double density) const{
28  if (density==0){
29  for (unsigned int i=0; i<size(); i++)
30  v[i]=(*this)[i];
31  } else {
32  Point lastPoint(0,0);
33  uint suppressed=0;
34  for (unsigned int i=0; i<size(); i++){
35  const RangeSensor* rs=dynamic_cast<const RangeSensor*>(getSensor());
36  assert(rs);
37  Point lp(
38  cos(rs->beams()[i].pose.theta)*(*this)[i],
39  sin(rs->beams()[i].pose.theta)*(*this)[i]);
40  Point dp=lastPoint-lp;
41  double distance=sqrt(dp*dp);
42  if (distance<density){
43  // v[i]=MAXDOUBLE;
45  suppressed++;
46  }
47  else{
48  lastPoint=lp;
49  v[i]=(*this)[i];
50  }
51  //std::cerr<< __func__ << std::endl;
52  //std::cerr<< "suppressed " << suppressed <<"/"<<size() << std::endl;
53  }
54  }
55  // return size();
56  return static_cast<unsigned int>(size());
57 
58 };
59 
60 unsigned int RangeReading::activeBeams(double density) const{
61  if (density==0.)
62  return size();
63  int ab=0;
64  Point lastPoint(0,0);
65  uint suppressed=0;
66  for (unsigned int i=0; i<size(); i++){
67  const RangeSensor* rs=dynamic_cast<const RangeSensor*>(getSensor());
68  assert(rs);
69  Point lp(
70  cos(rs->beams()[i].pose.theta)*(*this)[i],
71  sin(rs->beams()[i].pose.theta)*(*this)[i]);
72  Point dp=lastPoint-lp;
73  double distance=sqrt(dp*dp);
74  if (distance<density){
75  suppressed++;
76  }
77  else{
78  lastPoint=lp;
79  ab++;
80  }
81  //std::cerr<< __func__ << std::endl;
82  //std::cerr<< "suppressed " << suppressed <<"/"<<size() << std::endl;
83  }
84  return ab;
85 }
86 
87 std::vector<Point> RangeReading::cartesianForm(double maxRange) const{
88  const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>(getSensor());
89  assert(rangeSensor && rangeSensor->beams().size());
90  // uint m_beams=rangeSensor->beams().size();
91  uint m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
92  std::vector<Point> cartesianPoints(m_beams);
93  double px,py,ps,pc;
94  px=rangeSensor->getPose().x;
95  py=rangeSensor->getPose().y;
96  ps=sin(rangeSensor->getPose().theta);
97  pc=cos(rangeSensor->getPose().theta);
98  for (unsigned int i=0; i<m_beams; i++){
99  const double& rho=(*this)[i];
100  const double& s=rangeSensor->beams()[i].s;
101  const double& c=rangeSensor->beams()[i].c;
102  if (rho>=maxRange){
103  cartesianPoints[i]=Point(0,0);
104  } else {
105  Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho);
106  cartesianPoints[i].x=px+pc*p.x-ps*p.y;
107  cartesianPoints[i].y=py+ps*p.x+pc*p.y;
108  }
109  }
110  return cartesianPoints;
111 }
112 
113 };
114 
point< double > Point
Definition: point.h:202
const std::vector< Beam > & beams() const
Definition: rangesensor.h:24
const Sensor * getSensor() const
Definition: sensoreading.h:13
RangeReading(const RangeSensor *rs, double time=0)
std::vector< Point > cartesianForm(double maxRange=1e6) const
unsigned int activeBeams(double density=0.) const
unsigned int rawView(double *v, double density=0.) const
OrientedPoint getPose() const
Definition: rangesensor.h:26
unsigned int c
Definition: gfs2stream.cpp:41
double max(double a, double b)
Definition: gfs2img.cpp:22


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20