inflation.cpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Includes
6 *****************************************************************************/
7 
8 #include <iostream>
9 #include "../../../include/cost_map_core/operators/inflation.hpp"
10 
11 /*****************************************************************************
12 ** Namespaces
13 *****************************************************************************/
14 
15 namespace cost_map {
16 
17 /*****************************************************************************
18 ** Implementation
19 *****************************************************************************/
20 
21 void Inflate::operator()(const std::string layer_source,
22  const std::string layer_destination,
23  const float& inflation_radius,
24  const InflationComputer& inflation_computer,
26  ) {
27  //unsigned char* master_array = cost_map...
28 
29  unsigned int size_x = cost_map.getSize().x();
30  unsigned int size_y = cost_map.getSize().y();
31  unsigned int size = size_x*size_y;
32  const cost_map::Matrix& data_source = cost_map.get(layer_source);
33  cost_map.add(layer_destination, data_source);
34 
35  // Rebuild internals
36  seen_.resize(size_x, size_y);
37  seen_.setConstant(false);
38  unsigned int new_cell_inflation_radius = static_cast<unsigned int>(std::max(0.0, std::ceil(inflation_radius / cost_map.getResolution())));
39  if (new_cell_inflation_radius != cell_inflation_radius_ ) {
40  cell_inflation_radius_ = new_cell_inflation_radius;
41  computeCaches(cost_map.getResolution(), inflation_computer);
42  }
43 
44  // eigen is default column storage, so iterate over the rows most quickly
45  // if we want to make it robust, check for data_source.IsRowMajor
46  for (unsigned int j = 0, number_of_rows = data_source.rows(), number_of_columns = data_source.cols(); j < number_of_columns; ++j) {
47  for (unsigned int i = 0; i < number_of_rows; ++i) {
48  unsigned char cost = data_source(i, j);
49  if (cost == LETHAL_OBSTACLE) {
50  unsigned int index = j*number_of_rows + i;
51  enqueue(cost_map.get(layer_source), cost_map.get(layer_destination), i, j, i, j);
52  }
53  }
54  }
55  while (!inflation_queue_.empty())
56  {
57  // get the highest priority cell and pop it off the priority queue
58  const CellData& current_cell = inflation_queue_.top();
59 
60  unsigned int mx = current_cell.x_;
61  unsigned int my = current_cell.y_;
62  unsigned int sx = current_cell.src_x_;
63  unsigned int sy = current_cell.src_y_;
64 
65  // pop once we have our cell info
66  inflation_queue_.pop();
67 
68  // attempt to put the neighbors of the current cell onto the queue
69  if (mx > 0) {
70  enqueue(cost_map.get(layer_source), cost_map.get(layer_destination), mx - 1, my, sx, sy);
71  }
72  if (my > 0) {
73  enqueue(cost_map.get(layer_source), cost_map.get(layer_destination), mx, my - 1, sx, sy);
74  }
75  if (mx < size_x - 1) {
76  enqueue(cost_map.get(layer_source), cost_map.get(layer_destination), mx + 1, my, sx, sy);
77  }
78  if (my < size_y - 1) {
79  enqueue(cost_map.get(layer_source), cost_map.get(layer_destination), mx, my + 1, sx, sy);
80  }
81  }
82 }
83 
84 void Inflate::enqueue(const cost_map::Matrix& data_source,
85  cost_map::Matrix& data_destination,
86  unsigned int mx, unsigned int my,
87  unsigned int src_x, unsigned int src_y
88  )
89 {
90  // set the cost of the cell being inserted
91  if (!seen_(mx, my))
92  {
93  // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
94  double distance = distanceLookup(mx, my, src_x, src_y);
95 
96  // we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
97  if (distance > cell_inflation_radius_) {
98  return;
99  }
100 
101  // assign the cost associated with the distance from an obstacle to the cell
102  unsigned char cost = costLookup(mx, my, src_x, src_y);
103  unsigned char old_cost = data_source(mx, my);
104 
105  if (old_cost == NO_INFORMATION && cost >= INSCRIBED_OBSTACLE)
106  data_destination(mx, my) = cost;
107  else
108  data_destination(mx, my) = std::max(old_cost, cost);
109 
110  // push the cell data onto the queue and mark
111  seen_(mx, my) = true;
112  CellData data(distance, mx, my, src_x, src_y);
113  inflation_queue_.push(data);
114  }
115 }
116 
117 double Inflate::distanceLookup(int mx, int my, int src_x, int src_y)
118 {
119  unsigned int dx = abs(mx - src_x);
120  unsigned int dy = abs(my - src_y);
121  return cached_distances_(dx, dy);
122 }
123 
124 unsigned char Inflate::costLookup(int mx, int my, int src_x, int src_y)
125 {
126  unsigned int dx = abs(mx - src_x);
127  unsigned int dy = abs(my - src_y);
128  return cached_costs_(dx, dy);
129 }
130 
131 void Inflate::computeCaches(const float& resolution, const InflationComputer& compute_cost)
132 {
135 
136  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) {
137  for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) {
138  cached_distances_(i,j) = std::hypot(i, j);
139  }
140  }
141 
142  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) {
143  for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) {
144  cached_costs_(i, j) = compute_cost(resolution*cached_distances_(i, j));
145  }
146  }
147 }
148 
149 /*****************************************************************************
150 ** Inflation Computers
151 *****************************************************************************/
152 
154  const float& inscribed_radius,
155  const float& weight)
156 : inscribed_radius_(inscribed_radius)
157 , weight_(weight)
158 {
159 }
160 
161 unsigned char ROSInflationComputer::operator()(const float &distance) const {
162  unsigned char cost = 0;
163  if (distance == 0.0)
164  cost = LETHAL_OBSTACLE;
165  else if (distance <= inscribed_radius_)
166  cost = INSCRIBED_OBSTACLE;
167  else
168  {
169  // make sure cost falls off by Euclidean distance
170  double factor = std::exp(-1.0 * weight_ * (distance - inscribed_radius_));
171  cost = (unsigned char)((INSCRIBED_OBSTACLE - 1) * factor);
172  }
173  return cost;
174 }
175 
176 /*****************************************************************************
177 ** Deflation
178 *****************************************************************************/
179 
180 Deflate::Deflate(const bool& do_not_strip_inscribed_region)
181 : do_not_strip_inscribed_region(do_not_strip_inscribed_region)
182 {
183 }
184 
185 void Deflate::operator()(const std::string layer_source,
186  const std::string layer_destination,
188  )
189 {
190  // make a call on the data, just to check that the layer is there
191  // will throw std::out_of_range if not
192  cost_map::Matrix data_source = cost_map.get(layer_source);
193  // add a layer filled with NO_INFORMATION
194  cost_map.add(layer_destination);
195  cost_map::Matrix& data_destination = cost_map.get(layer_destination);
196 
198 
199  // eigen is by default column major - iterate with the row index changing fastest
200  for (unsigned int j = 0, number_of_rows = data_source.rows(), number_of_columns = data_source.cols(); j < number_of_columns; ++j) {
201  for (unsigned int i = 0; i < number_of_rows; ++i) {
202  unsigned char cost = data_source(i, j);
203  data_destination(i, j) = (cost >= cost_threshold ) ? cost : cost_map::FREE_SPACE;
204  }
205  }
206 }
207 
208 
209 /*****************************************************************************
210  ** Trailers
211  *****************************************************************************/
212 
213 } // namespace cost_map
unsigned char costLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed costs.
Definition: inflation.cpp:124
Function which can compute costs for the inflation layer.
Definition: inflation.hpp:35
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: common.hpp:30
const unsigned char NO_INFORMATION
Definition: common.cpp:21
cost_map::Matrix cached_costs_
Definition: inflation.hpp:162
unsigned int cell_inflation_radius_
Definition: inflation.hpp:164
const Size & getSize() const
double getResolution() const
void add(const std::string &layer, const DataType value=NO_INFORMATION)
double distanceLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed distances.
Definition: inflation.cpp:117
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic > seen_
Definition: inflation.hpp:160
virtual unsigned char operator()(const float &distance) const
Given a distance, compute a cost.
Definition: inflation.cpp:161
const unsigned char LETHAL_OBSTACLE
Definition: common.cpp:22
Deflate(const bool &do_not_strip_inscribed_region=false)
Definition: inflation.cpp:180
ROSInflationComputer(const float &inscribed_radius, const float &weight)
Definition: inflation.cpp:153
const unsigned char INSCRIBED_OBSTACLE
Definition: common.cpp:23
const Matrix & get(const std::string &layer) const
void operator()(const std::string layer_source, const std::string layer_destination, CostMap &cost_map)
Deflate...
Definition: inflation.cpp:185
const unsigned char FREE_SPACE
Definition: common.cpp:24
void computeCaches(const float &resolution, const InflationComputer &compute_cost)
Definition: inflation.cpp:131
std::priority_queue< CellData > inflation_queue_
Definition: inflation.hpp:163
bool do_not_strip_inscribed_region
Definition: inflation.hpp:192
void enqueue(const cost_map::Matrix &data_source, cost_map::Matrix &data_destination, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y)
Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation...
Definition: inflation.cpp:84
void operator()(const std::string layer_source, const std::string layer_destination, const float &inflation_radius, const InflationComputer &inflation_computer, CostMap &cost_map)
Inflate...
Definition: inflation.cpp:21
Eigen::MatrixXf cached_distances_
Definition: inflation.hpp:161


cost_map_core
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:41