21 #include <unordered_set>
32 unsigned int stripCount=1;
33 std::string incomplete_format =
"LineWidth=1.0;LineColor=";
34 int cylinder_count = tree.
getLevel(0).size();
39 std::ostringstream tag;
40 tag << prefix << stripCount;
48 for(
const auto& element:tree.
getLevel(0))
50 const auto& cylinder = element.second;
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_;
60 V3[i*2] = 1.0 +
std::max(cylinder.t0_-t,0.0);
67 V1[i*2] = cylinder.x_;
68 V1[i*2+1] =
std::max(0.1,cylinder.rxy_);
69 V2[i*2] = cylinder.y_;
73 V3[i*2] = 1.0 +
std::max(cylinder.t1_-t,0.0);
83 plot_tags.insert(tag.str());
87 tag << prefix << stripCount;
106 plot_tags.insert(tag.str());
115 unsigned int stripCount=1;
116 std::ostringstream format;
118 int cylinder_count = tree.
getLevel(0).size();
119 static const int max_size = 128;
120 static const int arrow_head_count = 3;
124 static const double psi = 150.0/180.0*
M_PI;
125 static const double cpsi =
std::cos(psi);
126 static const double spsi =
std::sin(psi);
129 std::ostringstream tag;
130 tag << prefix << stripCount;
132 for(
const auto& element:tree.
getLevel(0))
134 const auto& cylinder = element.second;
137 V3[i] = 0.5*(cylinder.z0_+cylinder.z1_) + dz;
141 if (i >= max_size-arrow_head_count)
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];
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;
156 V1[i+2] = V1[i-1] + ex * cpsi + ey * spsi;
157 V2[i+2] = V2[i-1] - ex * spsi + ey * cpsi;
159 plot_tags.insert(tag.str());
160 figure->
plot(tag.str(),V1,V2,V3,i+3,format.str());
163 tag << prefix << stripCount;
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];
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;
182 V1[i+2] = V1[i-1] + ex * cpsi + ey * spsi;
183 V2[i+2] = V2[i-1] - ex * spsi + ey * cpsi;
185 plot_tags.insert(tag.str());
186 figure->
plot(tag.str(),V1,V2,V3,i+3,format.str());
194 for(
const auto& prediction :
set)
196 std::stringstream ss;
197 ss<<subtopic<<
"/"<<prediction.trackingID_<<
"/"<<prediction.branchID_<<
"/";
198 plotCylinderTree(prediction.occupancy_, t, t_max, config, ss.str(), figure,plot_tags);
208 for(
const auto& prediction :
set)
210 std::stringstream ss;
211 ss<<subtopic<<
"/"<<prediction.trackingID_<<
"/"<<prediction.branchID_<<
"/";
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
double dvalue
Definition: utility.h:23
float fvalue[2]
Definition: utility.h:24