ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
plot_prediction.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Robert Markowski
13  ********************************************************************************/
14 #pragma once
15 
17 #include <plotlablib/utility.h>
19 #include "prediction_config.h"
20 #include "plot_shape.h"
21 #include <unordered_set>
22 
23 namespace adore
24 {
25 
26 namespace PLOT
27 {
28 
29 inline void plotCylinderTree(adore::mad::OccupancyCylinderTree tree, double t, double t_max, PLOT::PredictionConfig::prediction_config config, std::string prefix, DLR_TS::PlotLab::AFigureStub* figure, std::unordered_set<std::string> &plot_tags)
30 {
31  unsigned int i=0;
32  unsigned int stripCount=1;
33  std::string incomplete_format = "LineWidth=1.0;LineColor=";
34  int cylinder_count = tree.getLevel(0).size();
35  double V1[500];
36  double V2[500];
37  double V3[500];
38 
39  std::ostringstream tag;
40  tag << prefix << stripCount;
41 
42  // reminder: CircleStrip has a special data layout:
43  // given a circle with center x,y,z and radius r and color rgba (1 float per channel)
44  // v1[0] = x v2[0] = y v3[0] = z
45  // v1[1] = r v2[1] = rg (2 floats) v3[1] = ba (2 floats)
46  // then the pattern repeats
47 
48  for(const auto& element:tree.getLevel(0))
49  {
50  const auto& cylinder = element.second;
51 
53  double ti = adore::mad::bound(0.0,cylinder.t0_ - t,t_max);
54  V1[i*2] = cylinder.x_;
55  V1[i*2+1] = std::max(0.1,cylinder.rxy_-0.1);
56  V2[i*2] = cylinder.y_;
57  rg.fvalue[0] = config.getShade(0,ti,t_max,0.0);
58  rg.fvalue[1] = config.getShade(1,ti,t_max,0.0);
59  V2[i*2 + 1] = rg.dvalue;
60  V3[i*2] = 1.0 + std::max(cylinder.t0_-t,0.0);
61  ba.fvalue[0] = config.getShade(2,ti,t_max,0.0);
62  ba.fvalue[1] = 1.0f;
63  V3[i*2 + 1] = ba.dvalue;
64  i++;
65 
66  ti = adore::mad::bound(0.0,cylinder.t1_ - t,t_max);
67  V1[i*2] = cylinder.x_;
68  V1[i*2+1] = std::max(0.1,cylinder.rxy_);
69  V2[i*2] = cylinder.y_;
70  rg.fvalue[0] = config.getShade(0,ti,t_max,0.0);
71  rg.fvalue[1] = config.getShade(1,ti,t_max,0.0);
72  V2[i*2 + 1] = rg.dvalue;
73  V3[i*2] = 1.0 + std::max(cylinder.t1_-t,0.0);
74  ba.fvalue[0] = config.getShade(2,ti,t_max,0.0);
75  ba.fvalue[1] = 1.0f;
76  V3[i*2 + 1] = ba.dvalue;
77  i++;
78 
79  if (i > 127)
80  {
81  // std::cout << " warning: CylinderTree strip with too many cylinders: " << tree.getLevel(0).size() << std::endl;
82  // break;
83  plot_tags.insert(tag.str());
84  figure->circlestrip(tag.str(),V1,V2,V3,i*2,"");
85  tag.clear();
86  stripCount++;
87  tag << prefix << stripCount;
88  i = 0;
89  }
90  }
91  if (i > 0)
92  {
93  // DLR_TS::PlotLab::TwoFloatsPacked drg, dba;
94  // drg.dvalue = V2[1];
95  // dba.dvalue = V3[1];
96  // std::cout << " #DEBUG plot_prediction.h - 1st circle" << std::endl;
97  // std::cout << " x " << V1[0] << " \ty " << V2[0] << " z " << V3[0] << std::endl;
98  // std::cout << " r " << V1[1] << " \tr " << drg.fvalue[0] << " \tg " << drg.fvalue[1] << " \tb " << dba.fvalue[0] << " \ta " << dba.fvalue[1] << std::endl;
99 
100  // drg.dvalue = V2[i*2-1];
101  // dba.dvalue = V3[i*2-1];
102  // std::cout << " #DEBUG plot_prediction.h - last circle" << std::endl;
103  // std::cout << " x " << V1[i*2-2] << " \ty " << V2[i*2-2] << " z " << V3[i*2-2] << std::endl;
104  // std::cout << " r " << V1[i*2-1] << " \tr " << drg.fvalue[0] << " \tg " << drg.fvalue[1] << " \tb " << dba.fvalue[0] << " \ta " << dba.fvalue[1] << std::endl;
105  // std::cout << " #DEBUG plot_prediction.h striplength " << tree.getLevel(0).size() << " i " << i << std::endl;
106  plot_tags.insert(tag.str());
107  figure->circlestrip(tag.str(),V1,V2,V3,i*2,"");
108  }
109 
110 }
111 
112 inline void plotCylinderTree_onlyMean(adore::mad::OccupancyCylinderTree tree, double dz, PLOT::PredictionConfig::prediction_config config, std::string prefix, DLR_TS::PlotLab::AFigureStub* figure, std::unordered_set<std::string> &plot_tags)
113 {
114  unsigned int i=0;
115  unsigned int stripCount=1;
116  std::ostringstream format;
117  format<< "LineWidth=3.0;LineColor="<<config.r_.start_<<","<<config.g_.start_<<","<<config.b_.start_;
118  int cylinder_count = tree.getLevel(0).size();
119  static const int max_size = 128;
120  static const int arrow_head_count = 3;
121  double V1[500];
122  double V2[500];
123  double V3[500];
124  static const double psi = 150.0/180.0*M_PI;//arrow head angle
125  static const double cpsi = std::cos(psi);
126  static const double spsi = std::sin(psi);
127 
128 
129  std::ostringstream tag;
130  tag << prefix << stripCount;
131 
132  for(const auto& element:tree.getLevel(0))
133  {
134  const auto& cylinder = element.second;
135  V1[i] = cylinder.x_;
136  V2[i] = cylinder.y_;
137  V3[i] = 0.5*(cylinder.z0_+cylinder.z1_) + dz;
138  i++;
139 
140 
141  if (i >= max_size-arrow_head_count)
142  {
143  const double x1 = V1[i-1];
144  const double y1 = V2[i-1];
145  const double x0 = V1[i-2];
146  const double y0 = V2[i-2];
147  double li = 1.0/std::sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));
148  const double ex = (x1-x0)*li;
149  const double ey = (y1-y0)*li;
150  V1[i] = V1[i-1] + ex * cpsi - ey * spsi;
151  V2[i] = V2[i-1] + ex * spsi + ey * cpsi;
152  V3[i] = V3[i-1];
153  V1[i+1] = V1[i-1];
154  V2[i+1] = V2[i-1];
155  V3[i+1] = V3[i-1];
156  V1[i+2] = V1[i-1] + ex * cpsi + ey * spsi;
157  V2[i+2] = V2[i-1] - ex * spsi + ey * cpsi;
158  V3[i+2] = V3[i-1];
159  plot_tags.insert(tag.str());
160  figure->plot(tag.str(),V1,V2,V3,i+3,format.str());
161  tag.clear();
162  stripCount++;
163  tag << prefix << stripCount;
164  i = 0;
165  }
166  }
167  if (i > 1)
168  {
169  const double x1 = V1[i-1];
170  const double y1 = V2[i-1];
171  const double x0 = V1[i-2];
172  const double y0 = V2[i-2];
173  double li = 1.0/std::sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));
174  const double ex = (x1-x0)*li;
175  const double ey = (y1-y0)*li;
176  V1[i] = V1[i-1] + ex * cpsi - ey * spsi;
177  V2[i] = V2[i-1] + ex * spsi + ey * cpsi;
178  V3[i] = V3[i-1];
179  V1[i+1] = V1[i-1];
180  V2[i+1] = V2[i-1];
181  V3[i+1] = V3[i-1];
182  V1[i+2] = V1[i-1] + ex * cpsi + ey * spsi;
183  V2[i+2] = V2[i-1] - ex * spsi + ey * cpsi;
184  V3[i+2] = V3[i-1];
185  plot_tags.insert(tag.str());
186  figure->plot(tag.str(),V1,V2,V3,i+3,format.str());
187  }
188 
189 }
190 
191 inline void plotPredictionSet(adore::env::OccupancyCylinderPredictionSet set, double t, double t_max, PLOT::PredictionConfig::prediction_config config, std::string subtopic, DLR_TS::PlotLab::AFigureStub* figure, std::unordered_set<std::string> &plot_tags)
192 {
193  // unsigned int totalPredictionstripsCount = 0;
194  for(const auto& prediction : set)
195  {
196  std::stringstream ss;
197  ss<<subtopic<<"/"<<prediction.trackingID_<<"/"<<prediction.branchID_<<"/";
198  plotCylinderTree(prediction.occupancy_, t, t_max, config, ss.str(), figure,plot_tags);
199  // plot_tags.insert(ss.str());
200  // totalPredictionstripsCount++;
201  }
202  // std::cout << " #debug plot_prediction.h total count of prediction strips: " << totalPredictionstripsCount << std::endl;
203 }
204 
205 
206 inline void plotPredictionSetMinimal(adore::env::OccupancyCylinderPredictionSet set, double dz, PLOT::PredictionConfig::prediction_config config, std::string subtopic, DLR_TS::PlotLab::AFigureStub* figure, std::unordered_set<std::string> &plot_tags)
207 {
208  for(const auto& prediction : set)
209  {
210  std::stringstream ss;
211  ss<<subtopic<<"/"<<prediction.trackingID_<<"/"<<prediction.branchID_<<"/";
212  plotCylinderTree_onlyMean(prediction.occupancy_, dz, config, ss.str(), figure,plot_tags);
213  }
214 }
215 
216 // inline void unplotPrediction(std::string prefix, int trackingID, int branchID, DLR_TS::PlotLab::AFigureStub* figure)
217 // {
218 // std::stringstream ss;
219 // ss << prefix << "/" << trackingID << "/" << branchID << "/";
220 // figure->erase_similar(ss.str());
221 // }
222 
223 // inline void unplotPredictionSet(std::string prefix, adore::env::OccupancyCylinderPredictionSet old_set, adore::env::OccupancyCylinderPredictionSet new_set, DLR_TS::PlotLab::AFigureStub* figure)
224 // {
225 // for(auto itPred=old_set.begin(); itPred!=old_set.end(); itPred++)
226 // {
227 // auto prediction = *itPred;
228 // bool outdated = true;
229 // for(auto itNew=new_set.begin(); itNew!=new_set.end(); itNew++)
230 // {
231 // if(prediction.trackingID_ == itNew->trackingID_ && prediction.branchID_ == itNew->branchID_)
232 // {
233 // outdated = false;
234 // continue;
235 // }
236 // }
237 // if(outdated)
238 // {
239 // std::stringstream ss;
240 // ss << prefix << "/" << prediction.trackingID_ << "/" << prediction.branchID_ << "/";
241 // figure->erase_similar(ss.str());
242 // }
243 // }
244 // }
245 
246 }
247 
248 }
#define M_PI
Definition: arraymatrixtools.h:24
Definition: afigurestub.h:24
virtual void plot(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options)=0
virtual void circlestrip(std::string hashtag, double *X, double *Y, double *R, int size, std::string options)=0
Definition: occupancycylinder.h:84
VolumeVector & getLevel(int i)
Definition: vectorbasedvolumetree.h:60
void plotCylinderTree(adore::mad::OccupancyCylinderTree tree, double t, double t_max, PLOT::PredictionConfig::prediction_config config, std::string prefix, DLR_TS::PlotLab::AFigureStub *figure, std::unordered_set< std::string > &plot_tags)
Definition: plot_prediction.h:29
void plotCylinderTree_onlyMean(adore::mad::OccupancyCylinderTree tree, double dz, PLOT::PredictionConfig::prediction_config config, std::string prefix, DLR_TS::PlotLab::AFigureStub *figure, std::unordered_set< std::string > &plot_tags)
Definition: plot_prediction.h:112
void plotPredictionSet(adore::env::OccupancyCylinderPredictionSet set, double t, double t_max, PLOT::PredictionConfig::prediction_config config, std::string subtopic, DLR_TS::PlotLab::AFigureStub *figure, std::unordered_set< std::string > &plot_tags)
Definition: plot_prediction.h:191
void plotPredictionSetMinimal(adore::env::OccupancyCylinderPredictionSet set, double dz, PLOT::PredictionConfig::prediction_config config, std::string subtopic, DLR_TS::PlotLab::AFigureStub *figure, std::unordered_set< std::string > &plot_tags)
Definition: plot_prediction.h:206
std::vector< OccupancyCylinderPrediction > OccupancyCylinderPredictionSet
Definition: occupancycylinderprediction.h:40
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
void set(T *data, T value, int size)
Definition: adoremath.h:39
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
y0
Definition: adore_set_goal.py:26
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
Definition: areaofeffectconverter.h:20
double start_
Definition: prediction_config.h:30
Definition: prediction_config.h:27
color_range b_
Definition: prediction_config.h:46
double getShade(int rgb_index, double x, double x_max, double x_min=0)
Definition: prediction_config.h:52
color_range g_
Definition: prediction_config.h:46
color_range r_
Definition: prediction_config.h:46
Definition: utility.h:22
double dvalue
Definition: utility.h:23
float fvalue[2]
Definition: utility.h:24