├── .gitignore
├── README.md
├── kitti_evaluation
    ├── evaluate_object_3d_offline.cpp
    ├── evaluate_object_3d_offline_waymo.cpp
    └── hack_kitti_evaluation.h
├── mains
    ├── pickle_to_kitti_format.py
    └── write_JIoU.py
└── utils
    ├── __init__.py
    ├── calibration.py
    ├── kitti_utils.py
    ├── metric_plots.py
    ├── metric_utils.py
    ├── paul_geometry.py
    ├── probability_utils.py
    └── simple_stats.py


/.gitignore:
--------------------------------------------------------------------------------
1 | mains/results/
2 | *.m
3 | __pycache__
4 | matlab_scripts/
5 | debug/
6 | IROS/


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | 
 2 | # Inferring Spatial Uncertainty in Object Detection
 3 | 
 4 | A teaser version of the code for the paper [**Labels Are Not Perfect: Inferring Spatial Uncertainty in Object Detection**](https://arxiv.org/pdf/2012.12195.pdf).
 5 | 
 6 | ## Usage
 7 | ### C++ evaluation first
 8 | It contains a cpp file used for kitti offline evaluation, compile it with
 9 | 
10 | `g++ -o evaluate_object_3d_offline  evaluate_object_3d_offline.cpp -lboost_system -lboost_filesystem`
11 | 
12 | and use it same as the original kitti evaluation
13 | 
14 | `./evaluate_object_3d_offline ~/Kitti/object/training/label_2/ [your-path-to-predictions-in-KITTI-format]`
15 | 
16 | It will generate files hack files at the result folder
17 | 
18 | ### Python visualization after evaluation
19 | 
20 | Use the python file `write_JIOU.py` to print results. Change the paths in the file before usage.
21 | 
22 | There are several implicit parameters:
23 | * If "waymo" exists in the prediction data path, it will read the data in a format different from KITTI.
24 | * If "unc" exists in the prediction data path, it will try to read the probabilistic prediction in the format of the paper [**Hujie20**](https://arxiv.org/pdf/2006.12015.pdf)
25 | * If "uncertainty"  exists in the prediction data path, it will try to read the probabilistic prediction in the format of ProbPIXOR proposed in paper [**Di19**](https://arxiv.org/pdf/1909.12358.pdf)
26 | 
27 | ## Hints
28 | The IO of probabilistic prediction result is a disaster. The data format are not unified and vary from paper to paper. We have two kinds of  
29 | 
30 | ## LICENSE
31 | ### BSD License – Berkeley
32 | 
33 | Copyright (c) 2020 [Zining Wang](https://github.com/ZiningWang)
34 | 
35 | All rights reserved.
36 | 
37 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
38 | 
39 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
40 | 
41 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
42 | 
43 | * Neither the name of the <organization> nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
44 | 
45 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
46 | Please be sure to add the following as part of the copyright notice: "THIS SOFTWARE AND/OR DATA WAS DEPOSITED IN THE BAIR OPEN RESEARCH COMMONS REPOSITORY ON 12/05/2020."  Enter the date the software was released for "12/05/2020."
47 | 


--------------------------------------------------------------------------------
/kitti_evaluation/evaluate_object_3d_offline.cpp:
--------------------------------------------------------------------------------
   1 | #include <iostream>
   2 | #include <algorithm>
   3 | #include <stdio.h>
   4 | #include <math.h>
   5 | #include <vector>
   6 | #include <numeric>
   7 | #include <strings.h>
   8 | #include <assert.h>
   9 | 
  10 | #include <dirent.h>
  11 | 
  12 | #include <boost/numeric/ublas/matrix.hpp>
  13 | #include <boost/numeric/ublas/io.hpp>
  14 | 
  15 | #include <boost/geometry.hpp>
  16 | #include <boost/geometry/geometries/point_xy.hpp>
  17 | #include <boost/geometry/geometries/polygon.hpp>
  18 | #include <boost/geometry/geometries/adapted/c_array.hpp>
  19 | 
  20 | #include "mail.h"
  21 | #include "hack_kitti_evaluation.h"
  22 | 
  23 | BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
  24 | 
  25 | typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > Polygon;
  26 | 
  27 | 
  28 | using namespace std;
  29 | 
  30 | /*=======================================================================
  31 | STATIC EVALUATION PARAMETERS
  32 | =======================================================================*/
  33 | 
  34 | // holds the number of test images on the server
  35 | const int32_t N_TESTIMAGES = 7518;
  36 | //const int32_t N_TESTIMAGES = 7480;
  37 | 
  38 | // easy, moderate and hard evaluation level
  39 | enum DIFFICULTY{EASY=0, MODERATE=1, HARD=2};
  40 | 
  41 | // evaluation metrics: image, ground or 3D
  42 | enum METRIC{IMAGE=0, GROUND=1, BOX3D=2};
  43 | 
  44 | // evaluation parameter
  45 | const int32_t MIN_HEIGHT[3]     = {40, 25, 25};     // minimum height for evaluated groundtruth/detections
  46 | const int32_t MAX_OCCLUSION[3]  = {0, 1, 2};        // maximum occlusion level of the groundtruth used for evaluation
  47 | const double  MAX_TRUNCATION[3] = {0.15, 0.3, 0.5}; // maximum truncation level of the groundtruth used for evaluation
  48 | 
  49 | // evaluated object classes
  50 | enum CLASSES{CAR=0, PEDESTRIAN=1, CYCLIST=2};
  51 | const int NUM_CLASS = 3;
  52 | 
  53 | // parameters varying per class
  54 | vector<string> CLASS_NAMES;
  55 | // the minimum overlap required for 2D evaluation on the image/ground plane and 3D evaluation
  56 | const double MIN_OVERLAP[3][3] = {{0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}};
  57 | 
  58 | // no. of recall steps that should be evaluated (discretized)
  59 | const double N_SAMPLE_PTS = 41;
  60 | 
  61 | // initialize class names
  62 | void initGlobals () {
  63 |   CLASS_NAMES.push_back("car");
  64 |   CLASS_NAMES.push_back("pedestrian");
  65 |   CLASS_NAMES.push_back("cyclist");
  66 | }
  67 | 
  68 | /*=======================================================================
  69 | DATA TYPES FOR EVALUATION
  70 | =======================================================================*/
  71 | 
  72 | // holding data needed for precision-recall and precision-aos
  73 | struct tPrData {
  74 |   vector<double> v;           // detection score for computing score thresholds
  75 |   double         similarity;  // orientation similarity
  76 |   double         similarity_ground; // ground orientation similarity
  77 |   int32_t        tp;          // true positives
  78 |   int32_t        fp;          // false positives
  79 |   int32_t        fn;          // false negatives
  80 |   tPrData () :
  81 |     similarity(0), tp(0), fp(0), fn(0), similarity_ground(0) {}
  82 | };
  83 | 
  84 | // holding bounding boxes for ground truth and detections
  85 | struct tBox {
  86 |   string  type;     // object type as car, pedestrian or cyclist,...
  87 |   double   x1;      // left corner
  88 |   double   y1;      // top corner
  89 |   double   x2;      // right corner
  90 |   double   y2;      // bottom corner
  91 |   double   alpha;   // image orientation
  92 |   tBox (string type, double x1,double y1,double x2,double y2,double alpha) :
  93 |     type(type),x1(x1),y1(y1),x2(x2),y2(y2),alpha(alpha) {}
  94 | };
  95 | 
  96 | // holding ground truth data
  97 | struct tGroundtruth {
  98 |   tBox    box;        // object type, box, orientation
  99 |   double  truncation; // truncation 0..1
 100 |   int32_t occlusion;  // occlusion 0,1,2 (non, partly, fully)
 101 |   double ry;
 102 |   double  t1, t2, t3;
 103 |   double h, w, l;
 104 |   tGroundtruth () :
 105 |     box(tBox("invalild",-1,-1,-1,-1,-10)),truncation(-1),occlusion(-1) {}
 106 |   tGroundtruth (tBox box,double truncation,int32_t occlusion) :
 107 |     box(box),truncation(truncation),occlusion(occlusion) {}
 108 |   tGroundtruth (string type,double x1,double y1,double x2,double y2,double alpha,double truncation,int32_t occlusion) :
 109 |     box(tBox(type,x1,y1,x2,y2,alpha)),truncation(truncation),occlusion(occlusion) {}
 110 | };
 111 | 
 112 | // holding detection data
 113 | struct tDetection {
 114 |   tBox    box;    // object type, box, orientation
 115 |   double  thresh; // detection score
 116 |   double  ry;
 117 |   double  t1, t2, t3;
 118 |   double  h, w, l;
 119 |   tDetection ():
 120 |     box(tBox("invalid",-1,-1,-1,-1,-10)),thresh(-1000) {}
 121 |   tDetection (tBox box,double thresh) :
 122 |     box(box),thresh(thresh) {}
 123 |   tDetection (string type,double x1,double y1,double x2,double y2,double alpha,double thresh) :
 124 |     box(tBox(type,x1,y1,x2,y2,alpha)),thresh(thresh) {}
 125 | };
 126 | 
 127 | 
 128 | /*=======================================================================
 129 | FUNCTIONS TO LOAD DETECTION AND GROUND TRUTH DATA ONCE, SAVE RESULTS
 130 | =======================================================================*/
 131 | vector<int32_t> indices;
 132 | 
 133 | vector<tDetection> loadDetections(string file_name, bool &compute_aos,
 134 |         vector<bool> &eval_image, vector<bool> &eval_ground,
 135 |         vector<bool> &eval_3d, bool &success) {
 136 | 
 137 |   // holds all detections (ignored detections are indicated by an index vector
 138 |   vector<tDetection> detections;
 139 |   FILE *fp = fopen(file_name.c_str(),"r");
 140 |   if (!fp) {
 141 |     success = false;
 142 |     return detections;
 143 |   }
 144 |   while (!feof(fp)) {
 145 |     tDetection d;
 146 |     double trash;
 147 |     char str[255];
 148 |     if (fscanf(fp, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
 149 |                    str, &trash, &trash, &d.box.alpha, &d.box.x1, &d.box.y1,
 150 |                    &d.box.x2, &d.box.y2, &d.h, &d.w, &d.l, &d.t1, &d.t2, &d.t3,
 151 |                    &d.ry, &d.thresh)==16) {
 152 | 
 153 |         // d.thresh = 1;
 154 |       d.box.type = str;
 155 |       detections.push_back(d);
 156 | 
 157 |       // orientation=-10 is invalid, AOS is not evaluated if at least one orientation is invalid
 158 |       if(d.box.alpha == -10)
 159 |         compute_aos = false;
 160 | 
 161 |       // a class is only evaluated if it is detected at least once
 162 |       for (int c = 0; c < NUM_CLASS; c++) {
 163 |         if (!strcasecmp(d.box.type.c_str(), CLASS_NAMES[c].c_str())) {
 164 |           if (!eval_image[c] && d.box.x1 >= 0)
 165 |             eval_image[c] = true;
 166 |           if (!eval_ground[c] && d.t1 != -1000 && d.t3 != -1000 && d.w > 0 && d.l > 0)
 167 |             eval_ground[c] = true;
 168 |           if (!eval_3d[c] && d.t1 != -1000 && d.t2 != -1000 && d.t3 != -1000 && d.h > 0 && d.w > 0 && d.l > 0)
 169 |             eval_3d[c] = true;
 170 |           break;
 171 |         }
 172 |       }
 173 |     }
 174 |   }
 175 |   fclose(fp);
 176 |   success = true;
 177 |   return detections;
 178 | }
 179 | 
 180 | vector<tGroundtruth> loadGroundtruth(string file_name,bool &success) {
 181 | 
 182 |   // holds all ground truth (ignored ground truth is indicated by an index vector
 183 |   vector<tGroundtruth> groundtruth;
 184 |   FILE *fp = fopen(file_name.c_str(),"r");
 185 |   if (!fp) {
 186 |     success = false;
 187 |     return groundtruth;
 188 |   }
 189 |   while (!feof(fp)) {
 190 |     tGroundtruth g;
 191 |     char str[255];
 192 |     if (fscanf(fp, "%s %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
 193 |                    str, &g.truncation, &g.occlusion, &g.box.alpha,
 194 |                    &g.box.x1,   &g.box.y1,     &g.box.x2,    &g.box.y2,
 195 |                    &g.h,      &g.w,        &g.l,       &g.t1,
 196 |                    &g.t2,      &g.t3,        &g.ry )==15) {
 197 |       g.box.type = str;
 198 |       groundtruth.push_back(g);
 199 |     }
 200 |   }
 201 |   fclose(fp);
 202 |   success = true;
 203 |   return groundtruth;
 204 | }
 205 | 
 206 | void saveStats (const vector<double> &precision, const vector<double> &aos, FILE *filep_det, FILE *fp_ori) {
 207 | 
 208 |   // save precision to file
 209 |   if(precision.empty())
 210 |     return;
 211 |   for (int32_t i=0; i<precision.size(); i++)
 212 |     fprintf(filep_det,"%f ",precision[i]);
 213 |   fprintf(filep_det,"\n");
 214 | 
 215 |   // save orientation similarity, only if there were no invalid orientation entries in submission (alpha=-10)
 216 |   if(aos.empty())
 217 |     return;
 218 |   for (int32_t i=0; i<aos.size(); i++)
 219 |     fprintf(fp_ori,"%f ",aos[i]);
 220 |   fprintf(fp_ori,"\n");
 221 | }
 222 | 
 223 | /*=======================================================================
 224 | EVALUATION HELPER FUNCTIONS
 225 | =======================================================================*/
 226 | 
 227 | // criterion defines whether the overlap is computed with respect to both areas (ground truth and detection)
 228 | // or with respect to box a or b (detection and "dontcare" areas)
 229 | inline double imageBoxOverlap(tBox a, tBox b, int32_t criterion=-1){
 230 | 
 231 |   // overlap is invalid in the beginning
 232 |   double o = -1;
 233 | 
 234 |   // get overlapping area
 235 |   double x1 = max(a.x1, b.x1);
 236 |   double y1 = max(a.y1, b.y1);
 237 |   double x2 = min(a.x2, b.x2);
 238 |   double y2 = min(a.y2, b.y2);
 239 | 
 240 |   // compute width and height of overlapping area
 241 |   double w = x2-x1;
 242 |   double h = y2-y1;
 243 | 
 244 |   // set invalid entries to 0 overlap
 245 |   if(w<=0 || h<=0)
 246 |     return 0;
 247 | 
 248 |   // get overlapping areas
 249 |   double inter = w*h;
 250 |   double a_area = (a.x2-a.x1) * (a.y2-a.y1);
 251 |   double b_area = (b.x2-b.x1) * (b.y2-b.y1);
 252 | 
 253 |   // intersection over union overlap depending on users choice
 254 |   if(criterion==-1)     // union
 255 |     o = inter / (a_area+b_area-inter);
 256 |   else if(criterion==0) // bbox_a
 257 |     o = inter / a_area;
 258 |   else if(criterion==1) // bbox_b
 259 |     o = inter / b_area;
 260 | 
 261 |   // overlap
 262 |   return o;
 263 | }
 264 | 
 265 | inline double imageBoxOverlap(tDetection a, tGroundtruth b, int32_t criterion=-1){
 266 |   return imageBoxOverlap(a.box, b.box, criterion);
 267 | }
 268 | 
 269 | // compute polygon of an oriented bounding box
 270 | template <typename T>
 271 | Polygon toPolygon(const T& g) {
 272 |     using namespace boost::numeric::ublas;
 273 |     using namespace boost::geometry;
 274 |     matrix<double> mref(2, 2);
 275 |     mref(0, 0) = cos(g.ry); mref(0, 1) = sin(g.ry);
 276 |     mref(1, 0) = -sin(g.ry); mref(1, 1) = cos(g.ry);
 277 | 
 278 |     static int count = 0;
 279 |     matrix<double> corners(2, 4);
 280 |     double data[] = {g.l / 2, g.l / 2, -g.l / 2, -g.l / 2,
 281 |                      g.w / 2, -g.w / 2, -g.w / 2, g.w / 2};
 282 |     std::copy(data, data + 8, corners.data().begin());
 283 |     matrix<double> gc = prod(mref, corners);
 284 |     for (int i = 0; i < 4; ++i) {
 285 |         gc(0, i) += g.t1;
 286 |         gc(1, i) += g.t3;
 287 |     }
 288 | 
 289 |     double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
 290 |     Polygon poly;
 291 |     append(poly, points);
 292 |     return poly;
 293 | }
 294 | 
 295 | // measure overlap between bird's eye view bounding boxes, parametrized by (ry, l, w, tx, tz)
 296 | inline double groundBoxOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1) {
 297 |     using namespace boost::geometry;
 298 |     Polygon gp = toPolygon(g);
 299 |     Polygon dp = toPolygon(d);
 300 | 
 301 |     std::vector<Polygon> in, un;
 302 |     intersection(gp, dp, in);
 303 |     union_(gp, dp, un);
 304 | 
 305 |     double inter_area = in.empty() ? 0 : area(in.front());
 306 |     double union_area = area(un.front());
 307 |     double o;
 308 |     if(criterion==-1)     // union
 309 |         o = inter_area / union_area;
 310 |     else if(criterion==0) // bbox_a
 311 |         o = inter_area / area(dp);
 312 |     else if(criterion==1) // bbox_b
 313 |         o = inter_area / area(gp);
 314 | 
 315 |     return o;
 316 | }
 317 | 
 318 | // measure overlap between 3D bounding boxes, parametrized by (ry, h, w, l, tx, ty, tz)
 319 | inline double box3DOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1) {
 320 |     using namespace boost::geometry;
 321 |     Polygon gp = toPolygon(g);
 322 |     Polygon dp = toPolygon(d);
 323 | 
 324 |     std::vector<Polygon> in, un;
 325 |     intersection(gp, dp, in);
 326 |     union_(gp, dp, un);
 327 | 
 328 |     double ymax = min(d.t2, g.t2);
 329 |     double ymin = max(d.t2 - d.h, g.t2 - g.h);
 330 | 
 331 |     double inter_area = in.empty() ? 0 : area(in.front());
 332 |     double inter_vol = inter_area * max(0.0, ymax - ymin);
 333 | 
 334 |     double det_vol = d.h * d.l * d.w;
 335 |     double gt_vol = g.h * g.l * g.w;
 336 | 
 337 |     double o;
 338 |     if(criterion==-1)     // union
 339 |         o = inter_vol / (det_vol + gt_vol - inter_vol);
 340 |     else if(criterion==0) // bbox_a
 341 |         o = inter_vol / det_vol;
 342 |     else if(criterion==1) // bbox_b
 343 |         o = inter_vol / gt_vol;
 344 | 
 345 |     return o;
 346 | }
 347 | 
 348 | vector<double> getThresholds(vector<double> &v, double n_groundtruth){
 349 | 
 350 |   // holds scores needed to compute N_SAMPLE_PTS recall values
 351 |   vector<double> t;
 352 | 
 353 |   // sort scores in descending order
 354 |   // (highest score is assumed to give best/most confident detections)
 355 |   sort(v.begin(), v.end(), greater<double>());
 356 | 
 357 |   // get scores for linearly spaced recall
 358 |   double current_recall = 0;
 359 |   for(int32_t i=0; i<v.size(); i++){
 360 | 
 361 |     // check if right-hand-side recall with respect to current recall is close than left-hand-side one
 362 |     // in this case, skip the current detection score
 363 |     double l_recall, r_recall, recall;
 364 |     l_recall = (double)(i+1)/n_groundtruth;
 365 |     if(i<(v.size()-1))
 366 |       r_recall = (double)(i+2)/n_groundtruth;
 367 |     else
 368 |       r_recall = l_recall;
 369 | 
 370 |     if( (r_recall-current_recall) < (current_recall-l_recall) && i<(v.size()-1))
 371 |       continue;
 372 | 
 373 |     // left recall is the best approximation, so use this and goto next recall step for approximation
 374 |     recall = l_recall;
 375 | 
 376 |     // the next recall step was reached
 377 |     t.push_back(v[i]);
 378 |     current_recall += 1.0/(N_SAMPLE_PTS-1.0);
 379 |   }
 380 |   return t;
 381 | }
 382 | 
 383 | void cleanData(CLASSES current_class, const vector<tGroundtruth> &gt, const vector<tDetection> &det, vector<int32_t> &ignored_gt, vector<tGroundtruth> &dc, vector<int32_t> &ignored_det, int32_t &n_gt, DIFFICULTY difficulty){
 384 | 
 385 |   // extract ground truth bounding boxes for current evaluation class
 386 |   for(int32_t i=0;i<gt.size(); i++){
 387 | 
 388 |     // only bounding boxes with a minimum height are used for evaluation
 389 |     double height = gt[i].box.y2 - gt[i].box.y1;
 390 | 
 391 |     // neighboring classes are ignored ("van" for "car" and "person_sitting" for "pedestrian")
 392 |     // (lower/upper cases are ignored)
 393 |     int32_t valid_class;
 394 | 
 395 |     // all classes without a neighboring class
 396 |     if(!strcasecmp(gt[i].box.type.c_str(), CLASS_NAMES[current_class].c_str()))
 397 |       valid_class = 1;
 398 | 
 399 |     // classes with a neighboring class
 400 |     else if(!strcasecmp(CLASS_NAMES[current_class].c_str(), "Pedestrian") && !strcasecmp("Person_sitting", gt[i].box.type.c_str()))
 401 |       valid_class = 0;
 402 |     else if(!strcasecmp(CLASS_NAMES[current_class].c_str(), "Car") && !strcasecmp("Van", gt[i].box.type.c_str()))
 403 |       valid_class = 0;
 404 | 
 405 |     // classes not used for evaluation
 406 |     else
 407 |       valid_class = -1;
 408 | 
 409 |     // ground truth is ignored, if occlusion, truncation exceeds the difficulty or ground truth is too small
 410 |     // (doesn't count as FN nor TP, although detections may be assigned)
 411 |     bool ignore = false;
 412 |     if(gt[i].occlusion>MAX_OCCLUSION[difficulty] || gt[i].truncation>MAX_TRUNCATION[difficulty] || height<=MIN_HEIGHT[difficulty])
 413 |       ignore = true;
 414 | 
 415 |     // set ignored vector for ground truth
 416 |     // current class and not ignored (total no. of ground truth is detected for recall denominator)
 417 |     if(valid_class==1 && !ignore){
 418 |       ignored_gt.push_back(0);
 419 |       n_gt++;
 420 |     }
 421 | 
 422 |     // neighboring class, or current class but ignored
 423 |     else if(valid_class==0 || (ignore && valid_class==1))
 424 |       ignored_gt.push_back(1);
 425 | 
 426 |     // all other classes which are FN in the evaluation
 427 |     else
 428 |       ignored_gt.push_back(-1);
 429 |   }
 430 | 
 431 |   // extract dontcare areas
 432 |   for(int32_t i=0;i<gt.size(); i++)
 433 |     if(!strcasecmp("DontCare", gt[i].box.type.c_str()))
 434 |       dc.push_back(gt[i]);
 435 | 
 436 |   // extract detections bounding boxes of the current class
 437 |   for(int32_t i=0;i<det.size(); i++){
 438 | 
 439 |     // neighboring classes are not evaluated
 440 |     int32_t valid_class;
 441 |     if(!strcasecmp(det[i].box.type.c_str(), CLASS_NAMES[current_class].c_str()))
 442 |       valid_class = 1;
 443 |     else
 444 |       valid_class = -1;
 445 | 
 446 |     int32_t height = fabs(det[i].box.y1 - det[i].box.y2);
 447 | 
 448 |     // set ignored vector for detections
 449 |     if(height<MIN_HEIGHT[difficulty])
 450 |       ignored_det.push_back(1);
 451 |     else if(valid_class==1)
 452 |       ignored_det.push_back(0);
 453 |     else
 454 |       ignored_det.push_back(-1);
 455 |   }
 456 | }
 457 | 
 458 | tPrData computeStatistics(CLASSES current_class, const vector<tGroundtruth> &gt,
 459 |         const vector<tDetection> &det, const vector<tGroundtruth> &dc,
 460 |         const vector<int32_t> &ignored_gt, const vector<int32_t>  &ignored_det,
 461 |         vector<int32_t> &detected_gt, vector<int32_t>  &accepted_det, vector<bool> &fp_det, vector<double> &overlap_det,
 462 |         bool compute_fp, double (*boxoverlap)(tDetection, tGroundtruth, int32_t),
 463 |         METRIC metric, bool compute_aos=false, bool compute_aos_3d=false, double thresh=0, bool debug=false){
 464 | 
 465 |   tPrData stat = tPrData();
 466 |   const double NO_DETECTION = -10000000;
 467 |   vector<double> delta;            // holds angular difference for TPs (needed for AOS evaluation)
 468 |   vector<double> delta_ground;     // holds angular difference for TPs in BEV or 3D
 469 |   vector<bool> assigned_detection; // holds wether a detection was assigned to a valid or ignored ground truth
 470 |   assigned_detection.assign(det.size(), false);
 471 |   vector<bool> ignored_threshold;
 472 |   ignored_threshold.assign(det.size(), false); // holds detections with a threshold lower than thresh if FP are computed
 473 | 
 474 |   // detections with a low score are ignored for computing precision (needs FP)
 475 |   if(compute_fp)
 476 |     for(int32_t i=0; i<det.size(); i++)
 477 |       if(det[i].thresh<thresh)
 478 |         ignored_threshold[i] = true;
 479 | 
 480 |   // evaluate all ground truth boxes
 481 |   for(int32_t i=0; i<gt.size(); i++){
 482 | 
 483 |     // this ground truth is not of the current or a neighboring class and therefore ignored
 484 |     if(ignored_gt[i]==-1)
 485 |       continue;
 486 | 
 487 |     /*=======================================================================
 488 |     find candidates (overlap with ground truth > 0.5) (logical len(det))
 489 |     =======================================================================*/
 490 |     int32_t det_idx          = -1;
 491 |     double valid_detection = NO_DETECTION;
 492 |     double max_overlap     = 0;
 493 |     double max_overlap_WZN = 0;
 494 |     int32_t j_WZN = -1;
 495 | 
 496 |     // search for a possible detection
 497 |     bool assigned_ignored_det = false;
 498 |     for(int32_t j=0; j<det.size(); j++){
 499 | 
 500 |       // detections not of the current class, already assigned or with a low threshold are ignored
 501 |       if(ignored_det[j]==-1)
 502 |         continue;
 503 |       if(assigned_detection[j])
 504 |         continue;
 505 |       if(ignored_threshold[j])
 506 |         continue;
 507 | 
 508 |       // find the maximum score for the candidates and get idx of respective detection
 509 |       double overlap = boxoverlap(det[j], gt[i], -1);
 510 | 
 511 |       // for computing recall thresholds, the candidate with highest score is considered
 512 |       if(!compute_fp && overlap>MIN_OVERLAP[metric][current_class] && det[j].thresh>valid_detection){
 513 |         det_idx         = j;
 514 |         valid_detection = det[j].thresh;
 515 |       }
 516 | 
 517 |       // for computing pr curve values, the candidate with the greatest overlap is considered
 518 |       // if the greatest overlap is an ignored detection (min_height), the overlapping detection is used
 519 |       else if(compute_fp && overlap>MIN_OVERLAP[metric][current_class] && (overlap>max_overlap || assigned_ignored_det) && ignored_det[j]==0){
 520 |         max_overlap     = overlap;
 521 |         det_idx         = j;
 522 |         valid_detection = 1;
 523 |         assigned_ignored_det = false;
 524 |       }
 525 |       else if(compute_fp && overlap>MIN_OVERLAP[metric][current_class] && valid_detection==NO_DETECTION && ignored_det[j]==1){
 526 |         det_idx              = j;
 527 |         valid_detection      = 1;
 528 |         assigned_ignored_det = true;
 529 |       }
 530 |       //WZN
 531 |       if (overlap > overlap_det[j]){
 532 |         max_overlap_WZN = overlap;
 533 |         j_WZN = j;
 534 |         overlap_det[j] = overlap;
 535 |         accepted_det[j] = i;
 536 |       }
 537 |     }
 538 | 
 539 | 
 540 | 
 541 |     /*=======================================================================
 542 |     compute TP, FP and FN
 543 |     =======================================================================*/
 544 | 
 545 |     // nothing was assigned to this valid ground truth
 546 |     if(valid_detection==NO_DETECTION && ignored_gt[i]==0) {
 547 |       //WZN
 548 |       detected_gt[i] = -10; //considered as false negative 
 549 |       stat.fn++;
 550 |     }
 551 | 
 552 |     // only evaluate valid ground truth <=> detection assignments (considering difficulty level)
 553 |     else if(valid_detection!=NO_DETECTION && (ignored_gt[i]==1 || ignored_det[det_idx]==1)){
 554 |       //WZN
 555 |       if (detected_gt[i]==-10) {detected_gt[i] = -1;} //previously fn. Now neither tp or fn, just because detection is wierd
 556 | 
 557 |       assigned_detection[det_idx] = true;
 558 |     }
 559 | 
 560 |     // found a valid true positive
 561 |     else if(valid_detection!=NO_DETECTION){
 562 |       // WZN
 563 |       detected_gt[i] = det_idx;
 564 | 
 565 |       // write highest score to threshold vector
 566 |       stat.tp++;
 567 |       stat.v.push_back(det[det_idx].thresh);
 568 | 
 569 |       // compute angular difference of detection and ground truth if valid detection orientation was provided
 570 |       if(compute_aos)
 571 |         delta.push_back(gt[i].box.alpha - det[det_idx].box.alpha);
 572 | 
 573 |       // compute heading difference of detection and ground truth if valid detection orientation was provided
 574 |       if(compute_aos_3d)
 575 |         delta_ground.push_back(abs(gt[i].ry - det[det_idx].ry));
 576 | 
 577 |       // clean up
 578 |       assigned_detection[det_idx] = true;
 579 |     }
 580 |     /*WZN DEBUG
 581 |     if (detected_gt[i]==-10 && thresh>0 && thresh<0.4) {
 582 |       cout << "DEBUG WZN: fn found with overlap: " << max_overlap_WZN << ", thre=" << thresh << ", gt=" << i << ", det=" << j_WZN 
 583 |        << ", det_idx=" << det_idx << ", valid=" << valid_detection  << ", ig_gt=" << ignored_gt[i] << endl;
 584 |     }*/
 585 |   }
 586 | 
 587 |   // if FP are requested, consider stuff area
 588 |   if(compute_fp) {
 589 | 
 590 |     // count fp
 591 |     for (int32_t i = 0; i < det.size(); i++) {
 592 | 
 593 |       // count false positives if required (height smaller than required is ignored (ignored_det==1)
 594 |       if (!(assigned_detection[i] || ignored_det[i] == -1 || ignored_det[i] == 1 || ignored_threshold[i])) {
 595 |         //WZN 
 596 |         fp_det[i] = true; //considered as false positive
 597 | 
 598 |         stat.fp++;
 599 |       }
 600 |     }
 601 | 
 602 |     // do not consider detections overlapping with stuff area
 603 |     int32_t nstuff = 0;
 604 |     for (int32_t i = 0; i < dc.size(); i++) {
 605 |       for (int32_t j = 0; j < det.size(); j++) {
 606 | 
 607 |         // detections not of the current class, already assigned, with a low threshold or a low minimum height are ignored
 608 |         if (assigned_detection[j])
 609 |           continue;
 610 |         if (ignored_det[j] == -1 || ignored_det[j] == 1)
 611 |           continue;
 612 |         if (ignored_threshold[j])
 613 |           continue;
 614 | 
 615 |         // compute overlap and assign to stuff area, if overlap exceeds class specific value
 616 |         double overlap = boxoverlap(det[j], dc[i], 0);
 617 |         if (overlap > MIN_OVERLAP[metric][current_class]) {
 618 |           assigned_detection[j] = true;
 619 |           //WZN
 620 |           accepted_det[j] = -1;
 621 | 
 622 |           nstuff++;
 623 |         }
 624 |       }
 625 |     }
 626 | 
 627 |     
 628 | 
 629 |     // FP = no. of all not to ground truth assigned detections - detections assigned to stuff areas
 630 |     stat.fp -= nstuff;
 631 | 
 632 |     // if all orientation values are valid, the AOS is computed
 633 | 
 634 |     if(compute_aos){
 635 |       vector<double> tmp;
 636 |       // FP have a similarity of 0, for all TP compute AOS
 637 |       tmp.assign(stat.fp, 0);
 638 |       for(int32_t i=0; i<delta.size(); i++)
 639 |         tmp.push_back((1.0+cos(delta[i]))/2.0);
 640 |       // be sure, that all orientation deltas are computed
 641 |       assert(tmp.size()==stat.fp+stat.tp);
 642 |       assert(delta.size()==stat.tp);
 643 | 
 644 | 
 645 |       // get the mean orientation similarity for this image
 646 |       if(stat.tp>0 || stat.fp>0)
 647 |         stat.similarity = accumulate(tmp.begin(), tmp.end(), 0.0);
 648 | 
 649 |       // there was neither a FP nor a TP, so the similarity is ignored in the evaluation
 650 |       else
 651 |         stat.similarity = -1;
 652 |     }
 653 | 
 654 |     if (compute_aos_3d) {
 655 |         vector<double> tmp_ground;
 656 |         tmp_ground.assign(stat.fp, 0);
 657 |         for (int32_t i = 0; i < delta_ground.size(); i++)
 658 |             tmp_ground.push_back((1.0 + cos(delta_ground[i])) / 2.0);
 659 | 
 660 |         assert(tmp_ground.size()==stat.fp+stat.tp);
 661 |         assert(delta_ground.size()==stat.tp);
 662 | 
 663 |         if(stat.tp>0 || stat.fp>0)
 664 |             stat.similarity_ground = accumulate(tmp_ground.begin(), tmp_ground.end(), 0.0);
 665 | 
 666 |         // there was neither a FP nor a TP, so the similarity is ignored in the evaluation
 667 |         else
 668 |             stat.similarity_ground = -1;
 669 |     }
 670 |   }
 671 | 
 672 | 
 673 |   return stat;
 674 | }
 675 | 
 676 | /*=======================================================================
 677 | EVALUATE CLASS-WISE
 678 | =======================================================================*/
 679 | 
 680 | bool eval_class (FILE *filep_det, FILE *fp_ori,CLASSES current_class,
 681 |                  const vector< vector<tGroundtruth> > &groundtruth,
 682 |                  const vector< vector<tDetection> > &detections, 
 683 |                  vector<vector<int32_t> > &detected_gt, vector<vector<int32_t> > &accepted_det, vector<vector<bool> > &fp_det, vector<vector<double> > &overlap_det,
 684 |                  bool compute_aos, bool compute_aos_ground,
 685 |                  double (*boxoverlap)(tDetection, tGroundtruth, int32_t),
 686 |                  vector<double> &precision, vector<double> &aos, vector<double> &aos_ground,
 687 |                  DIFFICULTY difficulty, METRIC metric) {
 688 | 
 689 |   assert(groundtruth.size() == detections.size());
 690 | 
 691 |   // init
 692 |   int32_t n_gt=0;                                     // total no. of gt (denominator of recall)
 693 |   vector<double> v, thresholds;                       // detection scores, evaluated for recall discretization
 694 |   vector< vector<int32_t> > ignored_gt, ignored_det;  // index of ignored gt detection for current class/difficulty
 695 |   vector< vector<tGroundtruth> > dontcare;            // index of dontcare areas, included in ground truth
 696 |   
 697 | 
 698 |   // for all test images do
 699 |   for (int32_t i=0; i<groundtruth.size(); i++){
 700 | 
 701 |     // holds ignored ground truth, ignored detections and dontcare areas for current frame
 702 |     vector<int32_t> i_gt, i_det;
 703 |     vector<tGroundtruth> dc;
 704 | 
 705 |     // only evaluate objects of current class and ignore occluded, truncated objects
 706 |     cleanData(current_class, groundtruth[i], detections[i], i_gt, dc, i_det, n_gt, difficulty);
 707 |     ignored_gt.push_back(i_gt);
 708 |     ignored_det.push_back(i_det);
 709 |     dontcare.push_back(dc);
 710 |     // compute statistics to get recall values
 711 |     tPrData pr_tmp = tPrData();
 712 |     pr_tmp = computeStatistics(current_class, groundtruth[i], detections[i], dc, i_gt, i_det, detected_gt[i], accepted_det[i], fp_det[i], overlap_det[i], false, boxoverlap, metric, false, false);
 713 | 
 714 | 
 715 |     // add detection scores to vector over all images
 716 |     for(int32_t j=0; j<pr_tmp.v.size(); j++)
 717 |       v.push_back(pr_tmp.v[j]);
 718 |   }
 719 | 
 720 | 
 721 |   // get scores that must be evaluated for recall discretization
 722 |   thresholds = getThresholds(v, n_gt);
 723 | 
 724 |   // compute TP,FP,FN for relevant scores
 725 |   vector<tPrData> pr;
 726 |   pr.assign(thresholds.size(),tPrData());
 727 |   for (int32_t i=0; i<groundtruth.size(); i++){
 728 | 
 729 |     // for all scores/recall thresholds do:
 730 |     for(int32_t t=0; t<thresholds.size(); t++){
 731 |       tPrData tmp = tPrData();
 732 |       tmp = computeStatistics(current_class, groundtruth[i], detections[i], dontcare[i],
 733 |                               ignored_gt[i], ignored_det[i], detected_gt[i], accepted_det[i], fp_det[i], overlap_det[i],
 734 |                               true, boxoverlap, metric,
 735 |                               compute_aos, compute_aos_ground, thresholds[t], t==38);
 736 | 
 737 |       // add no. of TP, FP, FN, AOS for current frame to total evaluation for current threshold
 738 |       pr[t].tp += tmp.tp;
 739 |       pr[t].fp += tmp.fp;
 740 |       pr[t].fn += tmp.fn;
 741 | 
 742 |       /*//WZN DEBUG:
 743 |       if (i>830 && i<870&& difficulty==HARD)  {cout << "i = " << i << endl;}
 744 |       if (i==850 && difficulty==HARD){
 745 |         cout << "DEBUG: thresold=" << thresholds[t]  << ", t =" << t << "/" << thresholds.size() <<", detected_gt=" ;
 746 |         for (int iBug=0; iBug<detected_gt[i].size(); iBug++){
 747 |           cout << detected_gt[i][iBug] << " ";
 748 |         }
 749 |         cout << endl;
 750 |         cout << "DEBUG: "<<"ignored_gt=" ;
 751 |         for (int iBug=0; iBug<detected_gt[i].size(); iBug++){
 752 |           cout << ignored_gt[i][iBug] << " ";
 753 |         }
 754 |         cout << endl;
 755 |         cout << "DEBUG: "<<"ignored_det=" ;
 756 |         for (int iBug=0; iBug<ignored_det[i].size(); iBug++){
 757 |           cout << ignored_det[i][iBug] << " ";
 758 |         }
 759 |         cout << endl;
 760 |         cout << "DEBUG: tp,fp,fn: " << tmp.tp << tmp.fp << tmp.fn << endl;
 761 |       }*/
 762 | 
 763 |       //cout << "DEBUG: t, fp: " << t << ", " << pr[t].fp << endl; 
 764 | 
 765 |       if(tmp.similarity!=-1)
 766 |         pr[t].similarity += tmp.similarity;
 767 | 
 768 |       if(tmp.similarity_ground!=-1)
 769 |         pr[t].similarity_ground += tmp.similarity_ground;
 770 |     }
 771 |   }
 772 | 
 773 | 
 774 |   //cout << "DEBUG: stat: " << "thresold: " << thresholds[30] << ", tp fn fp: " << pr[30].tp << ", " <<pr[30].fn << ", " << pr[30].fp << endl;
 775 | 
 776 |   // compute recall, precision and AOS
 777 |   vector<double> recall;
 778 |   precision.assign(N_SAMPLE_PTS, 0);
 779 |   if(compute_aos)
 780 |     aos.assign(N_SAMPLE_PTS, 0);
 781 | 
 782 |   if(compute_aos_ground)
 783 |     aos_ground.assign(N_SAMPLE_PTS, 0);
 784 | 
 785 |   double r=0;
 786 |   for (int32_t i=0; i<thresholds.size(); i++){
 787 |     r = pr[i].tp/(double)(pr[i].tp + pr[i].fn);
 788 |     recall.push_back(r);
 789 |     precision[i] = pr[i].tp/(double)(pr[i].tp + pr[i].fp);
 790 | 
 791 | 
 792 | 
 793 |     if(compute_aos)
 794 |       aos[i] = pr[i].similarity/(double)(pr[i].tp + pr[i].fp);
 795 | 
 796 |     if(compute_aos_ground)
 797 |       aos_ground[i] = pr[i].similarity_ground/(double)(pr[i].tp + pr[i].fp);
 798 |   }
 799 | 
 800 |   // filter precision and AOS using max_{i..end}(precision)
 801 |   for (int32_t i=0; i<thresholds.size(); i++){
 802 |     precision[i] = *max_element(precision.begin()+i, precision.end());
 803 |     if(compute_aos)
 804 |       aos[i] = *max_element(aos.begin()+i, aos.end());
 805 |     if(compute_aos_ground)
 806 |       aos_ground[i] = *max_element(aos_ground.begin()+i, aos_ground.end());
 807 |   }
 808 | 
 809 |   // save statisics and finish with success
 810 |   saveStats(precision, aos, filep_det, fp_ori);
 811 |     return true;
 812 | }
 813 | 
 814 | void printAp(string file_name, vector<double> vals[]){
 815 | 
 816 |   float sum[3] = {0, 0, 0};
 817 |   for (int v = 0; v < 3; ++v)
 818 |     for (int i = 0; i < vals[v].size(); i = i + 4)
 819 |       sum[v] += vals[v][i];
 820 |   printf("%s AP: %f %f %f\n", file_name.c_str(), sum[0] / 11 * 100, sum[1] / 11 * 100, sum[2] / 11 * 100);
 821 | 
 822 | }
 823 | void saveAndPlotPlots(string dir_name,string file_name,string obj_type,vector<double> vals[],bool is_aos){
 824 | 
 825 |   char command[1024];
 826 | 
 827 |   // save plot data to file
 828 |   FILE *fp = fopen((dir_name + "/" + file_name + ".txt").c_str(),"w");
 829 |   // printf("save %s\n", (dir_name + "/" + file_name + ".txt").c_str());
 830 | 
 831 |   for (int32_t i=0; i<(int)N_SAMPLE_PTS; i++)
 832 |     fprintf(fp,"%f %f %f %f\n",(double)i/(N_SAMPLE_PTS-1.0),vals[0][i],vals[1][i],vals[2][i]);
 833 |   fclose(fp);
 834 | 
 835 |   float sum[3] = {0, 0, 0};
 836 |   for (int v = 0; v < 3; ++v)
 837 |       for (int i = 0; i < vals[v].size(); i = i + 4)
 838 |           sum[v] += vals[v][i];
 839 |   printf("%s AP: %f %f %f\n", file_name.c_str(), sum[0] / 11 * 100, sum[1] / 11 * 100, sum[2] / 11 * 100);
 840 | 
 841 | 
 842 |   // create png + eps
 843 |   for (int32_t j=0; j<2; j++) {
 844 | 
 845 |     // open file
 846 |     FILE *fp = fopen((dir_name + "/" + file_name + ".gp").c_str(),"w");
 847 | 
 848 |     // save gnuplot instructions
 849 |     if (j==0) {
 850 |       fprintf(fp,"set term png size 450,315 font \"Helvetica\" 11\n");
 851 |       fprintf(fp,"set output \"%s.png\"\n",file_name.c_str());
 852 |     } else {
 853 |       fprintf(fp,"set term postscript eps enhanced color font \"Helvetica\" 20\n");
 854 |       fprintf(fp,"set output \"%s.eps\"\n",file_name.c_str());
 855 |     }
 856 | 
 857 |     // set labels and ranges
 858 |     fprintf(fp,"set size ratio 0.7\n");
 859 |     fprintf(fp,"set xrange [0:1]\n");
 860 |     fprintf(fp,"set yrange [0:1]\n");
 861 |     fprintf(fp,"set xlabel \"Recall\"\n");
 862 |     if (!is_aos) fprintf(fp,"set ylabel \"Precision\"\n");
 863 |     else         fprintf(fp,"set ylabel \"Orientation Similarity\"\n");
 864 |     obj_type[0] = toupper(obj_type[0]);
 865 |     fprintf(fp,"set title \"%s\"\n",obj_type.c_str());
 866 | 
 867 |     // line width
 868 |     int32_t   lw = 5;
 869 |     if (j==0) lw = 3;
 870 | 
 871 |     // plot error curve
 872 |     fprintf(fp,"plot ");
 873 |     fprintf(fp,"\"%s.txt\" using 1:2 title 'Easy' with lines ls 1 lw %d,",file_name.c_str(),lw);
 874 |     fprintf(fp,"\"%s.txt\" using 1:3 title 'Moderate' with lines ls 2 lw %d,",file_name.c_str(),lw);
 875 |     fprintf(fp,"\"%s.txt\" using 1:4 title 'Hard' with lines ls 3 lw %d",file_name.c_str(),lw);
 876 | 
 877 |     // close file
 878 |     fclose(fp);
 879 | 
 880 |     // run gnuplot => create png + eps
 881 |     sprintf(command,"cd %s; gnuplot %s",dir_name.c_str(),(file_name + ".gp").c_str());
 882 |     system(command);
 883 |   }
 884 | 
 885 |   // create pdf and crop
 886 |   //sprintf(command,"cd %s; ps2pdf %s.eps %s_large.pdf",dir_name.c_str(),file_name.c_str(),file_name.c_str());
 887 |   //system(command);
 888 |   //sprintf(command,"cd %s; pdfcrop %s_large.pdf %s.pdf",dir_name.c_str(),file_name.c_str(),file_name.c_str());
 889 |   //system(command);
 890 |   //sprintf(command,"cd %s; rm %s_large.pdf",dir_name.c_str(),file_name.c_str());
 891 |   //system(command);
 892 | }
 893 | 
 894 | vector<int32_t> getEvalIndices(const string& result_dir) {
 895 | 
 896 |     DIR* dir;
 897 |     dirent* entity;
 898 |     dir = opendir(result_dir.c_str());
 899 |     if (dir) {
 900 |         while (entity = readdir(dir)) {
 901 |             string path(entity->d_name);
 902 |             int32_t len = path.size();
 903 |             if (len < 10) continue;
 904 |             int32_t index = atoi(path.substr(len - 10, 10).c_str());
 905 |             indices.push_back(index);
 906 |         }
 907 |     }
 908 |     return indices;
 909 | }
 910 | 
 911 | bool eval(string gt_dir, string result_dir, Mail* mail){
 912 | 
 913 |   // set some global parameters
 914 |   initGlobals();
 915 | 
 916 |   // ground truth and result directories
 917 |   // string gt_dir         = "data/object/label_2";
 918 |   // string result_dir     = "results/" + result_sha;
 919 |   string plot_dir       = result_dir + "/plot";
 920 | 
 921 |   // create output directories
 922 |   system(("mkdir " + plot_dir).c_str());
 923 | 
 924 |   // hold detections and ground truth in memory
 925 |   vector< vector<tGroundtruth> > groundtruth;
 926 |   vector< vector<tDetection> >   detections;
 927 | 
 928 |   // holds wether orientation similarity shall be computed (might be set to false while loading detections)
 929 |   // and which labels where provided by this submission
 930 |   bool compute_aos=true;
 931 |   bool compute_aos_ground=false;
 932 |   vector<bool> eval_image(NUM_CLASS, false);
 933 |   vector<bool> eval_ground(NUM_CLASS, false);
 934 |   vector<bool> eval_3d(NUM_CLASS, false);
 935 | 
 936 |   // for all images read groundtruth and detections
 937 |   //mail->msg("Loading detections...");
 938 |   std::vector<int32_t> indices = getEvalIndices(result_dir + "/data/");
 939 |   //printf("number of files for evaluation: %d\n", (int)indices.size());
 940 | 
 941 |   for (int32_t i=0; i<indices.size(); i++) {
 942 | 
 943 |     // file name
 944 |     char file_name[256];
 945 |     sprintf(file_name,"%06d.txt",indices.at(i));
 946 |     //cout << file_name << endl;
 947 | 
 948 |     // read ground truth and result poses
 949 |     bool gt_success,det_success;
 950 |     vector<tGroundtruth> gt   = loadGroundtruth(gt_dir + "/" + file_name,gt_success);
 951 |     vector<tDetection>   det  = loadDetections(result_dir + "/data/" + file_name,
 952 |             compute_aos, eval_image, eval_ground, eval_3d, det_success);
 953 |     groundtruth.push_back(gt);
 954 |     detections.push_back(det);
 955 | 
 956 |     // check for errors
 957 |     if (!gt_success) {
 958 |       mail->msg("ERROR: Couldn't read: %s of ground truth. Please write me an email!", file_name);
 959 |       return false;
 960 |     }
 961 |     if (!det_success) {
 962 |       mail->msg("ERROR: Couldn't read: %s", file_name);
 963 |       return false;
 964 |     }
 965 |   }
 966 |   mail->msg("  done.");
 967 | 
 968 |   //WZN
 969 |   vector<vector<vector<int32_t> > > detected_gt_2D(3), detected_gt_3D(3), detected_gt_G(3);
 970 |   vector<vector<vector<int32_t> > > accepted_det_2D(3), accepted_det_3D(3), accepted_det_G(3);
 971 |   vector<vector<vector<double> > > overlap_det_2D(3), overlap_det_3D(3), overlap_det_G(3);
 972 |   vector<vector<vector<bool> > > fp_det_2D(3), fp_det_3D(3), fp_det_G(3);
 973 |   for (int difficulty=0; difficulty<3; difficulty++){
 974 |     for (int32_t i=0; i<indices.size(); i++) {
 975 |       if (indices[i]==6493) {cout << "DEBUG: 6493: " << i << endl;}
 976 |       int n_gt = groundtruth[i].size();
 977 |       int n_det = detections[i].size();
 978 |       detected_gt_2D[difficulty].push_back(vector<int32_t>(n_gt,-1));
 979 |       detected_gt_3D[difficulty].push_back(vector<int32_t>(n_gt,-1));
 980 |       detected_gt_G[difficulty].push_back(vector<int32_t>(n_gt,-1));
 981 |       accepted_det_2D[difficulty].push_back(vector<int32_t>(n_det,-1));
 982 |       accepted_det_3D[difficulty].push_back(vector<int32_t>(n_det,-1));
 983 |       accepted_det_G[difficulty].push_back(vector<int32_t>(n_det,-1));
 984 |       overlap_det_2D[difficulty].push_back(vector<double>(n_det,0));
 985 |       overlap_det_3D[difficulty].push_back(vector<double>(n_det,0));
 986 |       overlap_det_G[difficulty].push_back(vector<double>(n_det,0));
 987 |       fp_det_2D[difficulty].push_back(vector<bool>(n_det,false));
 988 |       fp_det_3D[difficulty].push_back(vector<bool>(n_det,false));
 989 |       fp_det_G[difficulty].push_back(vector<bool>(n_det,false));
 990 |     }
 991 |   }
 992 |     
 993 | 
 994 |   // holds pointers for result files
 995 |   FILE *filep_det=0, *fp_ori=0;
 996 | 
 997 | 
 998 |   // don't evaluate AOS for birdview boxes and 3D boxes
 999 |   compute_aos = false;
1000 |   compute_aos_ground = true;
1001 |   // eval bird's eye view bounding boxes
1002 |   for (int c = 0; c < NUM_CLASS; c++) {
1003 |     CLASSES cls = (CLASSES)c;
1004 |     if (eval_ground[c]) {
1005 |       filep_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection_ground.txt").c_str(), "w");
1006 |       vector<double> precision[3], aos[3], aos_ground[3];
1007 |       if(   !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_G[EASY], accepted_det_G[EASY], fp_det_G[EASY], overlap_det_G[EASY], compute_aos,compute_aos_ground, groundBoxOverlap, precision[0], aos[0],aos_ground[0], EASY, GROUND)
1008 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_G[MODERATE], accepted_det_G[MODERATE], fp_det_G[MODERATE], overlap_det_G[MODERATE], compute_aos,compute_aos_ground, groundBoxOverlap, precision[1], aos[1],aos_ground[1], MODERATE, GROUND)
1009 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_G[HARD], accepted_det_G[HARD], fp_det_G[HARD], overlap_det_G[HARD], compute_aos,compute_aos_ground, groundBoxOverlap, precision[2], aos[2],aos_ground[2], HARD, GROUND)) {
1010 |         mail->msg("%s evaluation failed.", CLASS_NAMES[c].c_str());
1011 |         return false;
1012 |       }
1013 |       fclose(filep_det);
1014 |       saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection_BEV",CLASS_NAMES[c], precision, 0);
1015 |       if(compute_aos_ground)
1016 |         printAp(CLASS_NAMES[c] + "_heading_BEV",aos_ground);
1017 |     }
1018 |   }
1019 | 
1020 |   // eval image 2D bounding boxes
1021 |   for (int c = 0; c < NUM_CLASS; c++) {
1022 |     CLASSES cls = (CLASSES)c;
1023 |     if (eval_image[c]) {
1024 |       filep_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection.txt").c_str(), "w");
1025 |       if(compute_aos)
1026 |         fp_ori = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_orientation.txt").c_str(),"w");
1027 |       vector<double> precision[3], aos[3], aos_ground[3];
1028 |       if(   !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_2D[EASY], accepted_det_2D[EASY], fp_det_2D[EASY], overlap_det_2D[EASY], compute_aos,compute_aos_ground, imageBoxOverlap, precision[0], aos[0],aos_ground[0], EASY, IMAGE)
1029 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_2D[MODERATE], accepted_det_2D[MODERATE], fp_det_2D[MODERATE], overlap_det_2D[MODERATE], compute_aos,compute_aos_ground, imageBoxOverlap, precision[1], aos[1],aos_ground[0], MODERATE, IMAGE)
1030 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_2D[HARD], accepted_det_2D[HARD], fp_det_2D[HARD], overlap_det_2D[HARD], compute_aos,compute_aos_ground, imageBoxOverlap, precision[2], aos[2],aos_ground[0], HARD, IMAGE)) {
1031 |         mail->msg("%s evaluation failed.", CLASS_NAMES[c].c_str());
1032 |         return false;
1033 |       }
1034 |       fclose(filep_det);
1035 |       saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection", CLASS_NAMES[c], precision, 0);
1036 |       if(compute_aos){
1037 |         saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_orientation", CLASS_NAMES[c], aos, 1);
1038 |         fclose(fp_ori);
1039 |       }
1040 |     }
1041 |   }
1042 | 
1043 | 
1044 | 
1045 |   // eval 3D bounding boxes
1046 |   for (int c = 0; c < NUM_CLASS; c++) {
1047 |     CLASSES cls = (CLASSES)c;
1048 |     if (eval_3d[c]) {
1049 |       filep_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection_ground.txt").c_str(), "w");
1050 |       vector<double> precision[3], aos[3], aos_ground[3];
1051 |       if(   !eval_class(filep_det, fp_ori,cls, groundtruth, detections, detected_gt_3D[EASY], accepted_det_3D[EASY], fp_det_3D[EASY], overlap_det_3D[EASY], compute_aos,compute_aos_ground, box3DOverlap, precision[0], aos[0],aos_ground[0], EASY, BOX3D)
1052 |          || !eval_class(filep_det, fp_ori,cls, groundtruth, detections, detected_gt_3D[MODERATE], accepted_det_3D[MODERATE], fp_det_3D[MODERATE], overlap_det_3D[MODERATE], compute_aos,compute_aos_ground, box3DOverlap, precision[1], aos[1],aos_ground[1], MODERATE, BOX3D)
1053 |          || !eval_class(filep_det, fp_ori,cls, groundtruth, detections, detected_gt_3D[HARD], accepted_det_3D[HARD], fp_det_3D[HARD], overlap_det_3D[HARD], compute_aos,compute_aos_ground, box3DOverlap, precision[2], aos[2],aos_ground[2], HARD, BOX3D)) {
1054 |         mail->msg("%s evaluation failed.", CLASS_NAMES[c].c_str());
1055 |         return false;
1056 |       }
1057 |       fclose(filep_det);
1058 |       saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection_3D",CLASS_NAMES[c], precision, 0);
1059 |       if(compute_aos_ground)
1060 |         printAp(CLASS_NAMES[c] + "_heading_3D",aos_ground);
1061 |     }
1062 |   }
1063 | 
1064 | 
1065 |   // WZN: write to file
1066 |   cout << "Indices: " << indices[0] << " " << indices[1] << " " << indices[2] << " " << indices[3] << " " << indices[4] << " " << indices[5] << endl;
1067 |   cout << "DEBUG: " << detected_gt_3D.size() << ", " << detected_gt_G.size() << ", " << detected_gt_2D.size() << endl;
1068 |   cout << "DEBUG: " << detected_gt_3D[0].size() << ", " << detected_gt_G[0].size() << ", " << detected_gt_2D[0].size() << endl;
1069 |   write_detection_file(result_dir, indices, accepted_det_3D[EASY], accepted_det_G[EASY], accepted_det_2D[EASY], fp_det_3D[EASY], fp_det_G[EASY], fp_det_2D[EASY], overlap_det_3D[EASY], overlap_det_G[EASY], overlap_det_2D[EASY], "EASY");
1070 |   write_gt_file(result_dir,indices, detected_gt_3D[EASY], detected_gt_G[EASY], detected_gt_2D[EASY], "EASY");
1071 |   write_detection_file(result_dir,indices, accepted_det_3D[MODERATE], accepted_det_G[MODERATE], accepted_det_2D[MODERATE], fp_det_3D[MODERATE], fp_det_G[MODERATE], fp_det_2D[MODERATE], overlap_det_3D[MODERATE], overlap_det_G[MODERATE], overlap_det_2D[MODERATE], "MODERATE");
1072 |   write_gt_file(result_dir,indices, detected_gt_3D[MODERATE], detected_gt_G[MODERATE], detected_gt_2D[MODERATE], "MODERATE");
1073 |   write_detection_file(result_dir,indices, accepted_det_3D[HARD], accepted_det_G[HARD], accepted_det_2D[HARD], fp_det_3D[HARD], fp_det_G[HARD], fp_det_2D[HARD], overlap_det_3D[HARD], overlap_det_G[HARD], overlap_det_2D[HARD], "HARD");
1074 |   write_gt_file(result_dir,indices, detected_gt_3D[HARD], detected_gt_G[HARD], detected_gt_2D[HARD], "HARD");
1075 | 
1076 |   // success
1077 |   return true;
1078 | }
1079 | 
1080 | int32_t main (int32_t argc,char *argv[]) {
1081 | 
1082 |   // we need 2 or 4 arguments!
1083 |   if (argc!=3) {
1084 |     cout << "Usage: ./eval_detection_3d_offline gt_dir result_dir" << endl;
1085 |     return 1;
1086 |   }
1087 | 
1088 |   // read arguments
1089 |   string gt_dir = argv[1];
1090 |   string result_dir = argv[2];
1091 | 
1092 |   // init notification mail
1093 |   Mail *mail;
1094 |   mail = new Mail();
1095 |   //mail->msg("Thank you for participating in our evaluation!");
1096 | 
1097 |   // run evaluation
1098 |   if (eval(gt_dir, result_dir, mail)) {
1099 |     //mail->msg("Your evaluation results are available at:");
1100 |     //mail->msg(result_dir.c_str());
1101 |   } else {
1102 |     system(("rm -r " + result_dir + "/plot").c_str());
1103 |     mail->msg("An error occured while processing your results.");
1104 |   }
1105 | 
1106 |   // send mail and exit
1107 |   delete mail;
1108 | 
1109 |   return 0;
1110 | }
1111 | 


--------------------------------------------------------------------------------
/kitti_evaluation/evaluate_object_3d_offline_waymo.cpp:
--------------------------------------------------------------------------------
   1 | #include <iostream>
   2 | #include <algorithm>
   3 | #include <stdio.h>
   4 | #include <math.h>
   5 | #include <vector>
   6 | #include <numeric>
   7 | #include <strings.h>
   8 | #include <assert.h>
   9 | 
  10 | #include <dirent.h>
  11 | 
  12 | #include <boost/numeric/ublas/matrix.hpp>
  13 | #include <boost/numeric/ublas/io.hpp>
  14 | 
  15 | #include <boost/geometry.hpp>
  16 | #include <boost/geometry/geometries/point_xy.hpp>
  17 | #include <boost/geometry/geometries/polygon.hpp>
  18 | #include <boost/geometry/geometries/adapted/c_array.hpp>
  19 | 
  20 | #include "mail.h"
  21 | #include "hack_kitti_evaluation.h"
  22 | 
  23 | BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
  24 | 
  25 | typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > Polygon;
  26 | 
  27 | 
  28 | using namespace std;
  29 | 
  30 | /*=======================================================================
  31 | STATIC EVALUATION PARAMETERS
  32 | =======================================================================*/
  33 | 
  34 | // holds the number of test images on the server
  35 | const int32_t N_TESTIMAGES = 7518;
  36 | //const int32_t N_TESTIMAGES = 7480;
  37 | 
  38 | // easy, moderate and hard evaluation level
  39 | enum DIFFICULTY{EASY=0, MODERATE=1, HARD=2};
  40 | 
  41 | // evaluation metrics: image, ground or 3D
  42 | enum METRIC{IMAGE=0, GROUND=1, BOX3D=2};
  43 | 
  44 | // evaluation parameter
  45 | const int32_t MIN_HEIGHT[3]     = {40, 25, 25};     // minimum height for evaluated groundtruth/detections
  46 | const double MAX_DEPTH[3] = {30.0, 50.0, 70.0};      // maximum depth
  47 | //const int32_t MAX_OCCLUSION[3]  = {0, 1, 2};        // maximum occlusion level of the groundtruth used for evaluation
  48 | //const double  MAX_TRUNCATION[3] = {0.15, 0.3, 0.5}; // maximum truncation level of the groundtruth used for evaluation
  49 | const double MAX_H[3] = {2.5, 2.5, 2.5};
  50 | const double MAX_L[3] = {6.5, 6.5, 6.5};
  51 | const double MIN_L[3] = {3.0, 3.0, 3.0};
  52 | 
  53 | // evaluated object classes
  54 | enum CLASSES{CAR=0, PEDESTRIAN=1, CYCLIST=2};
  55 | const int NUM_CLASS = 3;
  56 | 
  57 | // parameters varying per class
  58 | vector<string> CLASS_NAMES;
  59 | // the minimum overlap required for 2D evaluation on the image/ground plane and 3D evaluation
  60 | const double MIN_OVERLAP[3][3] = {{0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}};
  61 | 
  62 | // no. of recall steps that should be evaluated (discretized)
  63 | const double N_SAMPLE_PTS = 41;
  64 | 
  65 | // initialize class names
  66 | void initGlobals () {
  67 |   CLASS_NAMES.push_back("vehicle");
  68 |   CLASS_NAMES.push_back("pedestrian");
  69 |   CLASS_NAMES.push_back("cyclist");
  70 | }
  71 | 
  72 | /*=======================================================================
  73 | DATA TYPES FOR EVALUATION
  74 | =======================================================================*/
  75 | 
  76 | // holding data needed for precision-recall and precision-aos
  77 | struct tPrData {
  78 |   vector<double> v;           // detection score for computing score thresholds
  79 |   double         similarity;  // orientation similarity
  80 |   double         similarity_ground; // ground orientation similarity
  81 |   int32_t        tp;          // true positives
  82 |   int32_t        fp;          // false positives
  83 |   int32_t        fn;          // false negatives
  84 |   tPrData () :
  85 |     similarity(0), tp(0), fp(0), fn(0), similarity_ground(0) {}
  86 | };
  87 | 
  88 | // holding bounding boxes for ground truth and detections
  89 | struct tBox {
  90 |   string  type;     // object type as car, pedestrian or cyclist,...
  91 |   double   x1;      // left corner
  92 |   double   y1;      // top corner
  93 |   double   x2;      // right corner
  94 |   double   y2;      // bottom corner
  95 |   double   alpha;   // image orientation
  96 |   tBox (string type, double x1,double y1,double x2,double y2,double alpha) :
  97 |     type(type),x1(x1),y1(y1),x2(x2),y2(y2),alpha(alpha) {}
  98 | };
  99 | 
 100 | // holding ground truth data
 101 | struct tGroundtruth {
 102 |   tBox    box;        // object type, box, orientation
 103 |   double  truncation; // truncation 0..1
 104 |   int32_t occlusion;  // occlusion 0,1,2 (non, partly, fully)
 105 |   double ry;
 106 |   double  t1, t2, t3;
 107 |   double h, w, l;
 108 |   tGroundtruth () :
 109 |     box(tBox("invalild",-1,-1,-1,-1,-10)),truncation(-1),occlusion(-1) {}
 110 |   tGroundtruth (tBox box,double truncation,int32_t occlusion) :
 111 |     box(box),truncation(truncation),occlusion(occlusion) {}
 112 |   tGroundtruth (string type,double x1,double y1,double x2,double y2,double alpha,double truncation,int32_t occlusion) :
 113 |     box(tBox(type,x1,y1,x2,y2,alpha)),truncation(truncation),occlusion(occlusion) {}
 114 | };
 115 | 
 116 | // holding detection data
 117 | struct tDetection {
 118 |   tBox    box;    // object type, box, orientation
 119 |   double  thresh; // detection score
 120 |   double  ry;
 121 |   double  t1, t2, t3;
 122 |   double  h, w, l;
 123 |   tDetection ():
 124 |     box(tBox("invalid",-1,-1,-1,-1,-10)),thresh(-1000) {}
 125 |   tDetection (tBox box,double thresh) :
 126 |     box(box),thresh(thresh) {}
 127 |   tDetection (string type,double x1,double y1,double x2,double y2,double alpha,double thresh) :
 128 |     box(tBox(type,x1,y1,x2,y2,alpha)),thresh(thresh) {}
 129 | };
 130 | 
 131 | 
 132 | /*=======================================================================
 133 | FUNCTIONS TO LOAD DETECTION AND GROUND TRUTH DATA ONCE, SAVE RESULTS
 134 | =======================================================================*/
 135 | vector<int32_t> indices;
 136 | 
 137 | vector<tDetection> loadDetections(string file_name, bool &compute_aos,
 138 |         vector<bool> &eval_image, vector<bool> &eval_ground,
 139 |         vector<bool> &eval_3d, bool &success) {
 140 | 
 141 |   // holds all detections (ignored detections are indicated by an index vector
 142 |   vector<tDetection> detections;
 143 |   FILE *fp = fopen(file_name.c_str(),"r");
 144 |   if (!fp) {
 145 |     success = false;
 146 |     return detections;
 147 |   }
 148 |   while (!feof(fp)) {
 149 |     tDetection d;
 150 |     double trash;
 151 |     char str[255];
 152 |     if (fscanf(fp, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
 153 |                    str, &trash, &trash, &d.box.alpha, &d.box.x1, &d.box.y1,
 154 |                    &d.box.x2, &d.box.y2, &d.h, &d.w, &d.l, &d.t3, &d.t1, &d.t2,
 155 |                    &d.ry, &d.thresh)==16) {
 156 | 
 157 |         // d.thresh = 1;
 158 |       d.box.type = str;
 159 |       detections.push_back(d);
 160 | 
 161 |       // orientation=-10 is invalid, AOS is not evaluated if at least one orientation is invalid
 162 |       if(d.box.alpha == -10)
 163 |         compute_aos = false;
 164 | 
 165 |       // a class is only evaluated if it is detected at least once
 166 |       for (int c = 0; c < NUM_CLASS; c++) {
 167 |         if (!strcasecmp(d.box.type.c_str(), CLASS_NAMES[c].c_str())) {
 168 |           if (!eval_image[c] && d.box.x1 >= 0)
 169 |             eval_image[c] = true;
 170 |           if (!eval_ground[c] && d.t1 != -1000 && d.t3 != -1000 && d.w > 0 && d.l > 0)
 171 |             eval_ground[c] = true;
 172 |           if (!eval_3d[c] && d.t1 != -1000 && d.t2 != -1000 && d.t3 != -1000 && d.h > 0 && d.w > 0 && d.l > 0)
 173 |             eval_3d[c] = true;
 174 |           break;
 175 |         }
 176 |       }
 177 |     }
 178 |   }
 179 |   fclose(fp);
 180 |   success = true;
 181 |   return detections;
 182 | }
 183 | 
 184 | vector<tGroundtruth> loadGroundtruth(string file_name,bool &success) {
 185 | 
 186 |   // holds all ground truth (ignored ground truth is indicated by an index vector
 187 |   vector<tGroundtruth> groundtruth;
 188 |   FILE *fp = fopen(file_name.c_str(),"r");
 189 |   if (!fp) {
 190 |     success = false;
 191 |     return groundtruth;
 192 |   }
 193 |   while (!feof(fp)) {
 194 |     tGroundtruth g;
 195 |     char str[255];
 196 |     double temp;
 197 |     if (fscanf(fp, "%s %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
 198 |                    str, &g.truncation, &g.occlusion, &g.box.alpha,
 199 |                    &g.box.x1,   &g.box.y1,     &g.box.x2,    &g.box.y2,
 200 |                    &g.h,      &g.w,        &g.l,       &g.t3,
 201 |                    &g.t1,      &g.t2,        &g.ry,   &temp )==16) {
 202 |       g.box.type = str;
 203 |       groundtruth.push_back(g);
 204 |     }
 205 |   }
 206 |   fclose(fp);
 207 |   success = true;
 208 |   return groundtruth;
 209 | }
 210 | 
 211 | void saveStats (const vector<double> &precision, const vector<double> &aos, FILE *filep_det, FILE *fp_ori) {
 212 | 
 213 |   // save precision to file
 214 |   if(precision.empty())
 215 |     return;
 216 |   for (int32_t i=0; i<precision.size(); i++)
 217 |     fprintf(filep_det,"%f ",precision[i]);
 218 |   fprintf(filep_det,"\n");
 219 | 
 220 |   // save orientation similarity, only if there were no invalid orientation entries in submission (alpha=-10)
 221 |   if(aos.empty())
 222 |     return;
 223 |   for (int32_t i=0; i<aos.size(); i++)
 224 |     fprintf(fp_ori,"%f ",aos[i]);
 225 |   fprintf(fp_ori,"\n");
 226 | }
 227 | 
 228 | /*=======================================================================
 229 | EVALUATION HELPER FUNCTIONS
 230 | =======================================================================*/
 231 | 
 232 | // criterion defines whether the overlap is computed with respect to both areas (ground truth and detection)
 233 | // or with respect to box a or b (detection and "dontcare" areas)
 234 | inline double imageBoxOverlap(tBox a, tBox b, int32_t criterion=-1){
 235 | 
 236 |   // overlap is invalid in the beginning
 237 |   double o = -1;
 238 | 
 239 |   // get overlapping area
 240 |   double x1 = max(a.x1, b.x1);
 241 |   double y1 = max(a.y1, b.y1);
 242 |   double x2 = min(a.x2, b.x2);
 243 |   double y2 = min(a.y2, b.y2);
 244 | 
 245 |   // compute width and height of overlapping area
 246 |   double w = x2-x1;
 247 |   double h = y2-y1;
 248 | 
 249 |   // set invalid entries to 0 overlap
 250 |   if(w<=0 || h<=0)
 251 |     return 0;
 252 | 
 253 |   // get overlapping areas
 254 |   double inter = w*h;
 255 |   double a_area = (a.x2-a.x1) * (a.y2-a.y1);
 256 |   double b_area = (b.x2-b.x1) * (b.y2-b.y1);
 257 | 
 258 |   // intersection over union overlap depending on users choice
 259 |   if(criterion==-1)     // union
 260 |     o = inter / (a_area+b_area-inter);
 261 |   else if(criterion==0) // bbox_a
 262 |     o = inter / a_area;
 263 |   else if(criterion==1) // bbox_b
 264 |     o = inter / b_area;
 265 | 
 266 |   // overlap
 267 |   return o;
 268 | }
 269 | 
 270 | inline double imageBoxOverlap(tDetection a, tGroundtruth b, int32_t criterion=-1){
 271 |   return imageBoxOverlap(a.box, b.box, criterion);
 272 | }
 273 | 
 274 | // compute polygon of an oriented bounding box
 275 | template <typename T>
 276 | Polygon toPolygon(const T& g) {
 277 |     using namespace boost::numeric::ublas;
 278 |     using namespace boost::geometry;
 279 |     matrix<double> mref(2, 2);
 280 |     mref(0, 0) = cos(g.ry); mref(0, 1) = sin(g.ry);
 281 |     mref(1, 0) = -sin(g.ry); mref(1, 1) = cos(g.ry);
 282 | 
 283 |     static int count = 0;
 284 |     matrix<double> corners(2, 4);
 285 |     double data[] = {g.l / 2, g.l / 2, -g.l / 2, -g.l / 2,
 286 |                      g.w / 2, -g.w / 2, -g.w / 2, g.w / 2};
 287 |     std::copy(data, data + 8, corners.data().begin());
 288 |     matrix<double> gc = prod(mref, corners);
 289 |     for (int i = 0; i < 4; ++i) {
 290 |         gc(0, i) += g.t1;
 291 |         gc(1, i) += g.t3;
 292 |     }
 293 | 
 294 |     double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
 295 |     Polygon poly;
 296 |     append(poly, points);
 297 |     return poly;
 298 | }
 299 | 
 300 | // measure overlap between bird's eye view bounding boxes, parametrized by (ry, l, w, tx, tz)
 301 | inline double groundBoxOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1) {
 302 |     using namespace boost::geometry;
 303 |     Polygon gp = toPolygon(g);
 304 |     Polygon dp = toPolygon(d);
 305 | 
 306 |     std::vector<Polygon> in, un;
 307 |     intersection(gp, dp, in);
 308 |     union_(gp, dp, un);
 309 | 
 310 |     double inter_area = in.empty() ? 0 : area(in.front());
 311 |     double union_area = area(un.front());
 312 |     double o;
 313 |     if(criterion==-1)     // union
 314 |         o = inter_area / union_area;
 315 |     else if(criterion==0) // bbox_a
 316 |         o = inter_area / area(dp);
 317 |     else if(criterion==1) // bbox_b
 318 |         o = inter_area / area(gp);
 319 | 
 320 |     return o;
 321 | }
 322 | 
 323 | // measure overlap between 3D bounding boxes, parametrized by (ry, h, w, l, tx, ty, tz)
 324 | inline double box3DOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1) {
 325 |     using namespace boost::geometry;
 326 |     Polygon gp = toPolygon(g);
 327 |     Polygon dp = toPolygon(d);
 328 | 
 329 |     std::vector<Polygon> in, un;
 330 |     intersection(gp, dp, in);
 331 |     union_(gp, dp, un);
 332 | 
 333 |     double ymax = min(d.t2, g.t2);
 334 |     double ymin = max(d.t2 - d.h, g.t2 - g.h);
 335 | 
 336 |     double inter_area = in.empty() ? 0 : area(in.front());
 337 |     double inter_vol = inter_area * max(0.0, ymax - ymin);
 338 | 
 339 |     double det_vol = d.h * d.l * d.w;
 340 |     double gt_vol = g.h * g.l * g.w;
 341 | 
 342 |     double o;
 343 |     if(criterion==-1)     // union
 344 |         o = inter_vol / (det_vol + gt_vol - inter_vol);
 345 |     else if(criterion==0) // bbox_a
 346 |         o = inter_vol / det_vol;
 347 |     else if(criterion==1) // bbox_b
 348 |         o = inter_vol / gt_vol;
 349 | 
 350 |     return o;
 351 | }
 352 | 
 353 | vector<double> getThresholds(vector<double> &v, double n_groundtruth){
 354 | 
 355 |   // holds scores needed to compute N_SAMPLE_PTS recall values
 356 |   vector<double> t;
 357 | 
 358 |   // sort scores in descending order
 359 |   // (highest score is assumed to give best/most confident detections)
 360 |   sort(v.begin(), v.end(), greater<double>());
 361 | 
 362 |   // get scores for linearly spaced recall
 363 |   double current_recall = 0;
 364 |   for(int32_t i=0; i<v.size(); i++){
 365 | 
 366 |     // check if right-hand-side recall with respect to current recall is close than left-hand-side one
 367 |     // in this case, skip the current detection score
 368 |     double l_recall, r_recall, recall;
 369 |     l_recall = (double)(i+1)/n_groundtruth;
 370 |     if(i<(v.size()-1))
 371 |       r_recall = (double)(i+2)/n_groundtruth;
 372 |     else
 373 |       r_recall = l_recall;
 374 | 
 375 |     if( (r_recall-current_recall) < (current_recall-l_recall) && i<(v.size()-1))
 376 |       continue;
 377 | 
 378 |     // left recall is the best approximation, so use this and goto next recall step for approximation
 379 |     recall = l_recall;
 380 | 
 381 |     // the next recall step was reached
 382 |     t.push_back(v[i]);
 383 |     current_recall += 1.0/(N_SAMPLE_PTS-1.0);
 384 |   }
 385 |   return t;
 386 | }
 387 | 
 388 | void cleanData(CLASSES current_class, const vector<tGroundtruth> &gt, const vector<tDetection> &det, vector<int32_t> &ignored_gt, vector<tGroundtruth> &dc, vector<int32_t> &ignored_det, int32_t &n_gt, DIFFICULTY difficulty){
 389 |   int n_ignored_gt=0;
 390 |   // extract ground truth bounding boxes for current evaluation class
 391 |   for(int32_t i=0;i<gt.size(); i++){
 392 | 
 393 |     // only bounding boxes with a minimum height are used for evaluation
 394 |     double height = gt[i].box.y2 - gt[i].box.y1;
 395 | 
 396 |     // neighboring classes are ignored ("van" for "car" and "person_sitting" for "pedestrian")
 397 |     // (lower/upper cases are ignored)
 398 |     int32_t valid_class;
 399 | 
 400 |     // all classes without a neighboring class
 401 |     if(!strcasecmp(gt[i].box.type.c_str(), CLASS_NAMES[current_class].c_str()))
 402 |       valid_class = 1;
 403 | 
 404 |     // classes with a neighboring class
 405 |     else if(!strcasecmp(CLASS_NAMES[current_class].c_str(), "Pedestrian") && !strcasecmp("Person_sitting", gt[i].box.type.c_str()))
 406 |       valid_class = 0;
 407 |     else if(!strcasecmp(CLASS_NAMES[current_class].c_str(), "Car") && !strcasecmp("Van", gt[i].box.type.c_str()))
 408 |       valid_class = 0;
 409 | 
 410 |     // classes not used for evaluation
 411 |     else
 412 |       valid_class = -1;
 413 | 
 414 |     // ground truth is ignored, if occlusion, truncation exceeds the difficulty or ground truth is too small
 415 |     // (doesn't count as FN nor TP, although detections may be assigned)
 416 |     bool ignore = false;
 417 |     if(gt[i].t1<-40.0 || gt[i].t1>40.0 || gt[i].t3>MAX_DEPTH[difficulty] || gt[i].h>=MAX_H[difficulty] || gt[i].l>=MAX_L[difficulty] || gt[i].l<=MIN_L[difficulty]){
 418 |       ignore = true;
 419 |       n_ignored_gt++;
 420 |     }
 421 |     //WZN: for ignore debug
 422 |     //cout << "DEBUG: class: " << gt[i].box.type.c_str() << ", valid_class: " << valid_class << ", ignore: " << ignore << ", height: " << height << endl;
 423 |     // set ignored vector for ground truth
 424 |     // current class and not ignored (total no. of ground truth is detected for recall denominator)
 425 |     if(valid_class==1 && !ignore){
 426 |       ignored_gt.push_back(0);
 427 |       n_gt++;
 428 |     }
 429 |     // neighboring class, or current class but ignored
 430 |     else if(valid_class==0 || (ignore && valid_class==1))
 431 |       ignored_gt.push_back(1);
 432 | 
 433 |     // all other classes which are FN in the evaluation
 434 |     else
 435 |       ignored_gt.push_back(-1);
 436 |   }
 437 | 
 438 |   // extract dontcare areas
 439 |   for(int32_t i=0;i<gt.size(); i++)
 440 |     if(!strcasecmp("DontCare", gt[i].box.type.c_str()))
 441 |       dc.push_back(gt[i]);
 442 | 
 443 |   // extract detections bounding boxes of the current class
 444 |   for(int32_t i=0;i<det.size(); i++){
 445 | 
 446 |     // neighboring classes are not evaluated
 447 |     int32_t valid_class;
 448 |     if(!strcasecmp(det[i].box.type.c_str(), CLASS_NAMES[current_class].c_str()))
 449 |       valid_class = 1;
 450 |     else
 451 |       valid_class = -1;
 452 | 
 453 |     int32_t height = fabs(det[i].box.y1 - det[i].box.y2);
 454 | 
 455 |     // set ignored vector for detections
 456 |     if(det[i].t3>MAX_DEPTH[difficulty])
 457 |       ignored_det.push_back(1);
 458 |     else if(valid_class==1)
 459 |       ignored_det.push_back(0);
 460 |     else
 461 |       ignored_det.push_back(-1);
 462 |   }
 463 |   //cout<<"num of gt: " << n_gt << ", # of ignored: " << n_ignored_gt << endl;
 464 | }
 465 | 
 466 | tPrData computeStatistics(CLASSES current_class, const vector<tGroundtruth> &gt,
 467 |         const vector<tDetection> &det, const vector<tGroundtruth> &dc,
 468 |         const vector<int32_t> &ignored_gt, const vector<int32_t>  &ignored_det,
 469 |         vector<int32_t> &detected_gt, vector<int32_t>  &accepted_det, vector<bool> &fp_det, vector<double> &overlap_det,
 470 |         bool compute_fp, double (*boxoverlap)(tDetection, tGroundtruth, int32_t),
 471 |         METRIC metric, bool compute_aos=false, bool compute_aos_3d=false, double thresh=0, bool debug=false){
 472 | 
 473 |   tPrData stat = tPrData();
 474 |   const double NO_DETECTION = -10000000;
 475 |   vector<double> delta;            // holds angular difference for TPs (needed for AOS evaluation)
 476 |   vector<double> delta_ground;     // holds angular difference for TPs in BEV or 3D
 477 |   vector<bool> assigned_detection; // holds wether a detection was assigned to a valid or ignored ground truth
 478 |   assigned_detection.assign(det.size(), false);
 479 |   vector<bool> ignored_threshold;
 480 |   ignored_threshold.assign(det.size(), false); // holds detections with a threshold lower than thresh if FP are computed
 481 | 
 482 |   // detections with a low score are ignored for computing precision (needs FP)
 483 |   if(compute_fp)
 484 |     for(int32_t i=0; i<det.size(); i++)
 485 |       if(det[i].thresh<thresh)
 486 |         ignored_threshold[i] = true;
 487 | 
 488 |   // evaluate all ground truth boxes
 489 |   for(int32_t i=0; i<gt.size(); i++){
 490 | 
 491 |     // this ground truth is not of the current or a neighboring class and therefore ignored
 492 |     if(ignored_gt[i]==-1)
 493 |       continue;
 494 | 
 495 |     /*=======================================================================
 496 |     find candidates (overlap with ground truth > 0.5) (logical len(det))
 497 |     =======================================================================*/
 498 |     int32_t det_idx          = -1;
 499 |     double valid_detection = NO_DETECTION;
 500 |     double max_overlap     = 0;
 501 |     double max_overlap_WZN = 0;
 502 |     int32_t j_WZN = -1;
 503 | 
 504 |     // search for a possible detection
 505 |     bool assigned_ignored_det = false;
 506 |     for(int32_t j=0; j<det.size(); j++){
 507 | 
 508 |       // detections not of the current class, already assigned or with a low threshold are ignored
 509 |       if(ignored_det[j]==-1)
 510 |         continue;
 511 |       if(assigned_detection[j])
 512 |         continue;
 513 |       if(ignored_threshold[j])
 514 |         continue;
 515 | 
 516 |       // find the maximum score for the candidates and get idx of respective detection
 517 |       double overlap = boxoverlap(det[j], gt[i], -1);
 518 | 
 519 |       // for computing recall thresholds, the candidate with highest score is considered
 520 |       if(!compute_fp && overlap>MIN_OVERLAP[metric][current_class] && det[j].thresh>valid_detection){
 521 |         det_idx         = j;
 522 |         valid_detection = det[j].thresh;
 523 |       }
 524 | 
 525 |       // for computing pr curve values, the candidate with the greatest overlap is considered
 526 |       // if the greatest overlap is an ignored detection (min_height), the overlapping detection is used
 527 |       else if(compute_fp && overlap>MIN_OVERLAP[metric][current_class] && (overlap>max_overlap || assigned_ignored_det) && ignored_det[j]==0){
 528 |         max_overlap     = overlap;
 529 |         det_idx         = j;
 530 |         valid_detection = 1;
 531 |         assigned_ignored_det = false;
 532 |       }
 533 |       else if(compute_fp && overlap>MIN_OVERLAP[metric][current_class] && valid_detection==NO_DETECTION && ignored_det[j]==1){
 534 |         det_idx              = j;
 535 |         valid_detection      = 1;
 536 |         assigned_ignored_det = true;
 537 |       }
 538 |       //WZN
 539 |       if (overlap > overlap_det[j]){
 540 |         max_overlap_WZN = overlap;
 541 |         j_WZN = j;
 542 |         overlap_det[j] = overlap;
 543 |         accepted_det[j] = i;
 544 |       }
 545 |     }
 546 | 
 547 | 
 548 | 
 549 |     /*=======================================================================
 550 |     compute TP, FP and FN
 551 |     =======================================================================*/
 552 | 
 553 |     // nothing was assigned to this valid ground truth
 554 |     if(valid_detection==NO_DETECTION && ignored_gt[i]==0) {
 555 |       //WZN
 556 |       detected_gt[i] = -10; //considered as false negative 
 557 |       stat.fn++;
 558 |     }
 559 | 
 560 |     // only evaluate valid ground truth <=> detection assignments (considering difficulty level)
 561 |     else if(valid_detection!=NO_DETECTION && (ignored_gt[i]==1 || ignored_det[det_idx]==1)){
 562 |       //WZN
 563 |       if (detected_gt[i]==-10) {detected_gt[i] = -1;} //previously fn. Now neither tp or fn, just because detection is wierd
 564 | 
 565 |       assigned_detection[det_idx] = true;
 566 |     }
 567 | 
 568 |     // found a valid true positive
 569 |     else if(valid_detection!=NO_DETECTION){
 570 |       // WZN
 571 |       detected_gt[i] = det_idx;
 572 | 
 573 |       // write highest score to threshold vector
 574 |       stat.tp++;
 575 |       stat.v.push_back(det[det_idx].thresh);
 576 | 
 577 |       // compute angular difference of detection and ground truth if valid detection orientation was provided
 578 |       if(compute_aos)
 579 |         delta.push_back(gt[i].box.alpha - det[det_idx].box.alpha);
 580 | 
 581 |       // compute heading difference of detection and ground truth if valid detection orientation was provided
 582 |       if(compute_aos_3d)
 583 |         delta_ground.push_back(abs(gt[i].ry - det[det_idx].ry));
 584 | 
 585 |       // clean up
 586 |       assigned_detection[det_idx] = true;
 587 |     }
 588 |     /*WZN DEBUG
 589 |     if (detected_gt[i]==-10 && thresh>0 && thresh<0.4) {
 590 |       cout << "DEBUG WZN: fn found with overlap: " << max_overlap_WZN << ", thre=" << thresh << ", gt=" << i << ", det=" << j_WZN 
 591 |        << ", det_idx=" << det_idx << ", valid=" << valid_detection  << ", ig_gt=" << ignored_gt[i] << endl;
 592 |     }*/
 593 |   }
 594 | 
 595 |   // if FP are requested, consider stuff area
 596 |   if(compute_fp) {
 597 | 
 598 |     // count fp
 599 |     for (int32_t i = 0; i < det.size(); i++) {
 600 | 
 601 |       // count false positives if required (height smaller than required is ignored (ignored_det==1)
 602 |       if (!(assigned_detection[i] || ignored_det[i] == -1 || ignored_det[i] == 1 || ignored_threshold[i])) {
 603 |         //WZN 
 604 |         fp_det[i] = true; //considered as false positive
 605 | 
 606 |         stat.fp++;
 607 |       }
 608 |     }
 609 | 
 610 |     // do not consider detections overlapping with stuff area
 611 |     int32_t nstuff = 0;
 612 |     for (int32_t i = 0; i < dc.size(); i++) {
 613 |       for (int32_t j = 0; j < det.size(); j++) {
 614 | 
 615 |         // detections not of the current class, already assigned, with a low threshold or a low minimum height are ignored
 616 |         if (assigned_detection[j])
 617 |           continue;
 618 |         if (ignored_det[j] == -1 || ignored_det[j] == 1)
 619 |           continue;
 620 |         if (ignored_threshold[j])
 621 |           continue;
 622 | 
 623 |         // compute overlap and assign to stuff area, if overlap exceeds class specific value
 624 |         double overlap = boxoverlap(det[j], dc[i], 0);
 625 |         if (overlap > MIN_OVERLAP[metric][current_class]) {
 626 |           assigned_detection[j] = true;
 627 |           //WZN
 628 |           accepted_det[j] = -1;
 629 | 
 630 |           nstuff++;
 631 |         }
 632 |       }
 633 |     }
 634 | 
 635 |     
 636 | 
 637 |     // FP = no. of all not to ground truth assigned detections - detections assigned to stuff areas
 638 |     stat.fp -= nstuff;
 639 | 
 640 |     // if all orientation values are valid, the AOS is computed
 641 | 
 642 |     if(compute_aos){
 643 |       vector<double> tmp;
 644 |       // FP have a similarity of 0, for all TP compute AOS
 645 |       tmp.assign(stat.fp, 0);
 646 |       for(int32_t i=0; i<delta.size(); i++)
 647 |         tmp.push_back((1.0+cos(delta[i]))/2.0);
 648 |       // be sure, that all orientation deltas are computed
 649 |       assert(tmp.size()==stat.fp+stat.tp);
 650 |       assert(delta.size()==stat.tp);
 651 | 
 652 | 
 653 |       // get the mean orientation similarity for this image
 654 |       if(stat.tp>0 || stat.fp>0)
 655 |         stat.similarity = accumulate(tmp.begin(), tmp.end(), 0.0);
 656 | 
 657 |       // there was neither a FP nor a TP, so the similarity is ignored in the evaluation
 658 |       else
 659 |         stat.similarity = -1;
 660 |     }
 661 | 
 662 |     if (compute_aos_3d) {
 663 |         vector<double> tmp_ground;
 664 |         tmp_ground.assign(stat.fp, 0);
 665 |         for (int32_t i = 0; i < delta_ground.size(); i++)
 666 |             tmp_ground.push_back((1.0 + cos(delta_ground[i])) / 2.0);
 667 | 
 668 |         assert(tmp_ground.size()==stat.fp+stat.tp);
 669 |         assert(delta_ground.size()==stat.tp);
 670 | 
 671 |         if(stat.tp>0 || stat.fp>0)
 672 |             stat.similarity_ground = accumulate(tmp_ground.begin(), tmp_ground.end(), 0.0);
 673 | 
 674 |         // there was neither a FP nor a TP, so the similarity is ignored in the evaluation
 675 |         else
 676 |             stat.similarity_ground = -1;
 677 |     }
 678 |   }
 679 | 
 680 | 
 681 |   return stat;
 682 | }
 683 | 
 684 | /*=======================================================================
 685 | EVALUATE CLASS-WISE
 686 | =======================================================================*/
 687 | 
 688 | bool eval_class (FILE *filep_det, FILE *fp_ori,CLASSES current_class,
 689 |                  const vector< vector<tGroundtruth> > &groundtruth,
 690 |                  const vector< vector<tDetection> > &detections,
 691 |                  vector<vector<int32_t> > &detected_gt, vector<vector<int32_t> > &accepted_det, vector<vector<bool> > &fp_det, vector<vector<double> > &overlap_det,
 692 |                  bool compute_aos, bool compute_aos_ground,
 693 |                  double (*boxoverlap)(tDetection, tGroundtruth, int32_t),
 694 |                  vector<double> &precision, vector<double> &aos, vector<double> &aos_ground,
 695 |                  DIFFICULTY difficulty, METRIC metric) {
 696 | 
 697 |   assert(groundtruth.size() == detections.size());
 698 | 
 699 |   // init
 700 |   int32_t n_gt=0;                                     // total no. of gt (denominator of recall)
 701 |   vector<double> v, thresholds;                       // detection scores, evaluated for recall discretization
 702 |   vector< vector<int32_t> > ignored_gt, ignored_det;  // index of ignored gt detection for current class/difficulty
 703 |   vector< vector<tGroundtruth> > dontcare;            // index of dontcare areas, included in ground truth
 704 |   
 705 | 
 706 |   // for all test images do
 707 |   for (int32_t i=0; i<groundtruth.size(); i++){
 708 | 
 709 |     // holds ignored ground truth, ignored detections and dontcare areas for current frame
 710 |     vector<int32_t> i_gt, i_det;
 711 |     vector<tGroundtruth> dc;
 712 | 
 713 |     // only evaluate objects of current class and ignore occluded, truncated objects
 714 |     cleanData(current_class, groundtruth[i], detections[i], i_gt, dc, i_det, n_gt, difficulty);
 715 |     ignored_gt.push_back(i_gt);
 716 |     ignored_det.push_back(i_det);
 717 |     dontcare.push_back(dc);
 718 |     // compute statistics to get recall values
 719 |     tPrData pr_tmp = tPrData();
 720 |     pr_tmp = computeStatistics(current_class, groundtruth[i], detections[i], dc, i_gt, i_det, detected_gt[i], accepted_det[i], fp_det[i], overlap_det[i], false, boxoverlap, metric, false, false);
 721 | 
 722 | 
 723 |     // add detection scores to vector over all images
 724 |     for(int32_t j=0; j<pr_tmp.v.size(); j++)
 725 |       v.push_back(pr_tmp.v[j]);
 726 |   }
 727 | 
 728 | 
 729 |   // get scores that must be evaluated for recall discretization
 730 |   thresholds = getThresholds(v, n_gt);
 731 | 
 732 |   // compute TP,FP,FN for relevant scores
 733 |   vector<tPrData> pr;
 734 |   pr.assign(thresholds.size(),tPrData());
 735 |   for (int32_t i=0; i<groundtruth.size(); i++){
 736 | 
 737 |     // for all scores/recall thresholds do:
 738 |     for(int32_t t=0; t<thresholds.size(); t++){
 739 |       tPrData tmp = tPrData();
 740 |       tmp = computeStatistics(current_class, groundtruth[i], detections[i], dontcare[i],
 741 |                               ignored_gt[i], ignored_det[i], detected_gt[i], accepted_det[i], fp_det[i], overlap_det[i],
 742 |                               true, boxoverlap, metric,
 743 |                               compute_aos, compute_aos_ground, thresholds[t], t==38);
 744 | 
 745 |       // add no. of TP, FP, FN, AOS for current frame to total evaluation for current threshold
 746 |       pr[t].tp += tmp.tp;
 747 |       pr[t].fp += tmp.fp;
 748 |       pr[t].fn += tmp.fn;
 749 | 
 750 |       /*//WZN DEBUG:
 751 |       if (i>830 && i<870&& difficulty==HARD)  {cout << "i = " << i << endl;}
 752 |       if (i==850 && difficulty==HARD){
 753 |         cout << "DEBUG: thresold=" << thresholds[t]  << ", t =" << t << "/" << thresholds.size() <<", detected_gt=" ;
 754 |         for (int iBug=0; iBug<detected_gt[i].size(); iBug++){
 755 |           cout << detected_gt[i][iBug] << " ";
 756 |         }
 757 |         cout << endl;
 758 |         cout << "DEBUG: "<<"ignored_gt=" ;
 759 |         for (int iBug=0; iBug<detected_gt[i].size(); iBug++){
 760 |           cout << ignored_gt[i][iBug] << " ";
 761 |         }
 762 |         cout << endl;
 763 |         cout << "DEBUG: "<<"ignored_det=" ;
 764 |         for (int iBug=0; iBug<ignored_det[i].size(); iBug++){
 765 |           cout << ignored_det[i][iBug] << " ";
 766 |         }
 767 |         cout << endl;
 768 |         cout << "DEBUG: tp,fp,fn: " << tmp.tp << tmp.fp << tmp.fn << endl;
 769 |       }*/
 770 | 
 771 |       //cout << "DEBUG: t, fp: " << t << ", " << pr[t].fp << endl; 
 772 | 
 773 |       if(tmp.similarity!=-1)
 774 |         pr[t].similarity += tmp.similarity;
 775 | 
 776 |       if(tmp.similarity_ground!=-1)
 777 |         pr[t].similarity_ground += tmp.similarity_ground;
 778 |     }
 779 |   }
 780 | 
 781 | 
 782 |   //cout << "DEBUG: stat: " << "thresold: " << thresholds[30] << ", tp fn fp: " << pr[30].tp << ", " <<pr[30].fn << ", " << pr[30].fp << endl;
 783 | 
 784 |   // compute recall, precision and AOS
 785 |   vector<double> recall;
 786 |   precision.assign(N_SAMPLE_PTS, 0);
 787 |   if(compute_aos)
 788 |     aos.assign(N_SAMPLE_PTS, 0);
 789 | 
 790 |   if(compute_aos_ground)
 791 |     aos_ground.assign(N_SAMPLE_PTS, 0);
 792 | 
 793 |   double r=0;
 794 |   for (int32_t i=0; i<thresholds.size(); i++){
 795 |     r = pr[i].tp/(double)(pr[i].tp + pr[i].fn);
 796 |     recall.push_back(r);
 797 |     precision[i] = pr[i].tp/(double)(pr[i].tp + pr[i].fp);
 798 | 
 799 | 
 800 | 
 801 |     if(compute_aos)
 802 |       aos[i] = pr[i].similarity/(double)(pr[i].tp + pr[i].fp);
 803 | 
 804 |     if(compute_aos_ground)
 805 |       aos_ground[i] = pr[i].similarity_ground/(double)(pr[i].tp + pr[i].fp);
 806 |   }
 807 | 
 808 |   // filter precision and AOS using max_{i..end}(precision)
 809 |   for (int32_t i=0; i<thresholds.size(); i++){
 810 |     precision[i] = *max_element(precision.begin()+i, precision.end());
 811 |     if(compute_aos)
 812 |       aos[i] = *max_element(aos.begin()+i, aos.end());
 813 |     if(compute_aos_ground)
 814 |       aos_ground[i] = *max_element(aos_ground.begin()+i, aos_ground.end());
 815 |   }
 816 | 
 817 |   // save statisics and finish with success
 818 |   saveStats(precision, aos, filep_det, fp_ori);
 819 |     return true;
 820 | }
 821 | 
 822 | void printAp(string file_name, vector<double> vals[]){
 823 | 
 824 |   float sum[3] = {0, 0, 0};
 825 |   for (int v = 0; v < 3; ++v)
 826 |     for (int i = 0; i < vals[v].size(); i = i + 4)
 827 |       sum[v] += vals[v][i];
 828 |   printf("%s AP: %f %f %f\n", file_name.c_str(), sum[0] / 11 * 100, sum[1] / 11 * 100, sum[2] / 11 * 100);
 829 | 
 830 | }
 831 | void saveAndPlotPlots(string dir_name,string file_name,string obj_type,vector<double> vals[],bool is_aos){
 832 | 
 833 |   char command[1024];
 834 | 
 835 |   // save plot data to file
 836 |   FILE *fp = fopen((dir_name + "/" + file_name + ".txt").c_str(),"w");
 837 |   // printf("save %s\n", (dir_name + "/" + file_name + ".txt").c_str());
 838 | 
 839 |   for (int32_t i=0; i<(int)N_SAMPLE_PTS; i++)
 840 |     fprintf(fp,"%f %f %f %f\n",(double)i/(N_SAMPLE_PTS-1.0),vals[0][i],vals[1][i],vals[2][i]);
 841 |   fclose(fp);
 842 | 
 843 |   float sum[3] = {0, 0, 0};
 844 |   for (int v = 0; v < 3; ++v)
 845 |       for (int i = 0; i < vals[v].size(); i = i + 4)
 846 |           sum[v] += vals[v][i];
 847 |   printf("%s AP: %f %f %f\n", file_name.c_str(), sum[0] / 11 * 100, sum[1] / 11 * 100, sum[2] / 11 * 100);
 848 | 
 849 | 
 850 |   // create png + eps
 851 |   for (int32_t j=0; j<2; j++) {
 852 | 
 853 |     // open file
 854 |     FILE *fp = fopen((dir_name + "/" + file_name + ".gp").c_str(),"w");
 855 | 
 856 |     // save gnuplot instructions
 857 |     if (j==0) {
 858 |       fprintf(fp,"set term png size 450,315 font \"Helvetica\" 11\n");
 859 |       fprintf(fp,"set output \"%s.png\"\n",file_name.c_str());
 860 |     } else {
 861 |       fprintf(fp,"set term postscript eps enhanced color font \"Helvetica\" 20\n");
 862 |       fprintf(fp,"set output \"%s.eps\"\n",file_name.c_str());
 863 |     }
 864 | 
 865 |     // set labels and ranges
 866 |     fprintf(fp,"set size ratio 0.7\n");
 867 |     fprintf(fp,"set xrange [0:1]\n");
 868 |     fprintf(fp,"set yrange [0:1]\n");
 869 |     fprintf(fp,"set xlabel \"Recall\"\n");
 870 |     if (!is_aos) fprintf(fp,"set ylabel \"Precision\"\n");
 871 |     else         fprintf(fp,"set ylabel \"Orientation Similarity\"\n");
 872 |     obj_type[0] = toupper(obj_type[0]);
 873 |     fprintf(fp,"set title \"%s\"\n",obj_type.c_str());
 874 | 
 875 |     // line width
 876 |     int32_t   lw = 5;
 877 |     if (j==0) lw = 3;
 878 | 
 879 |     // plot error curve
 880 |     fprintf(fp,"plot ");
 881 |     fprintf(fp,"\"%s.txt\" using 1:2 title 'Easy' with lines ls 1 lw %d,",file_name.c_str(),lw);
 882 |     fprintf(fp,"\"%s.txt\" using 1:3 title 'Moderate' with lines ls 2 lw %d,",file_name.c_str(),lw);
 883 |     fprintf(fp,"\"%s.txt\" using 1:4 title 'Hard' with lines ls 3 lw %d",file_name.c_str(),lw);
 884 | 
 885 |     // close file
 886 |     fclose(fp);
 887 | 
 888 |     // run gnuplot => create png + eps
 889 |     sprintf(command,"cd %s; gnuplot %s",dir_name.c_str(),(file_name + ".gp").c_str());
 890 |     system(command);
 891 |   }
 892 | 
 893 |   // create pdf and crop
 894 |   //sprintf(command,"cd %s; ps2pdf %s.eps %s_large.pdf",dir_name.c_str(),file_name.c_str(),file_name.c_str());
 895 |   //system(command);
 896 |   //sprintf(command,"cd %s; pdfcrop %s_large.pdf %s.pdf",dir_name.c_str(),file_name.c_str(),file_name.c_str());
 897 |   //system(command);
 898 |   //sprintf(command,"cd %s; rm %s_large.pdf",dir_name.c_str(),file_name.c_str());
 899 |   //system(command);
 900 | }
 901 | 
 902 | vector<int32_t> getEvalIndices(const string& result_dir) {
 903 | 
 904 |     DIR* dir;
 905 |     dirent* entity;
 906 |     dir = opendir(result_dir.c_str());
 907 |     if (dir) {
 908 |         while (entity = readdir(dir)) {
 909 |             string path(entity->d_name);
 910 |             int32_t len = path.size();
 911 |             if (len < 10) continue;
 912 |             int32_t index = atoi(path.substr(len - 10, 10).c_str());
 913 |             indices.push_back(index);
 914 |         }
 915 |     }
 916 |     return indices;
 917 | }
 918 | 
 919 | bool eval(string gt_dir, string result_dir, Mail* mail){
 920 | 
 921 |   // set some global parameters
 922 |   initGlobals();
 923 | 
 924 |   // ground truth and result directories
 925 |   // string gt_dir         = "data/object/label_2";
 926 |   // string result_dir     = "results/" + result_sha;
 927 |   string plot_dir       = result_dir + "/plot";
 928 | 
 929 |   // create output directories
 930 |   system(("mkdir " + plot_dir).c_str());
 931 | 
 932 |   // hold detections and ground truth in memory
 933 |   vector< vector<tGroundtruth> > groundtruth;
 934 |   vector< vector<tDetection> >   detections;
 935 | 
 936 |   // holds wether orientation similarity shall be computed (might be set to false while loading detections)
 937 |   // and which labels where provided by this submission
 938 |   bool compute_aos=true;
 939 |   bool compute_aos_ground=false;
 940 |   vector<bool> eval_image(NUM_CLASS, false);
 941 |   vector<bool> eval_ground(NUM_CLASS, false);
 942 |   vector<bool> eval_3d(NUM_CLASS, false);
 943 | 
 944 |   // for all images read groundtruth and detections
 945 |   //mail->msg("Loading detections...");
 946 |   std::vector<int32_t> indices = getEvalIndices(result_dir + "/data/");
 947 |   //printf("number of files for evaluation: %d\n", (int)indices.size());
 948 | 
 949 |   for (int32_t i=0; i<indices.size(); i++) {
 950 | 
 951 |     // file name
 952 |     char file_name[256];
 953 |     sprintf(file_name,"%08d.txt",indices.at(i));
 954 |     //cout << file_name << endl;
 955 | 
 956 |     // read ground truth and result poses
 957 |     bool gt_success,det_success;
 958 |     vector<tGroundtruth> gt   = loadGroundtruth(gt_dir + "/" + file_name,gt_success);
 959 |     vector<tDetection>   det  = loadDetections(result_dir + "/data/" + file_name,
 960 |             compute_aos, eval_image, eval_ground, eval_3d, det_success);
 961 |     groundtruth.push_back(gt);
 962 |     detections.push_back(det);
 963 | 
 964 |     // check for errors
 965 |     if (!gt_success) {
 966 |       mail->msg("ERROR: Couldn't read: %s of ground truth. Please write me an email!", file_name);
 967 |       return false;
 968 |     }
 969 |     if (!det_success) {
 970 |       mail->msg("ERROR: Couldn't read: %s", file_name);
 971 |       return false;
 972 |     }
 973 |   }
 974 |   mail->msg("  done.");
 975 | 
 976 |   //WZN
 977 |   vector<vector<vector<int32_t> > > detected_gt_2D(3), detected_gt_3D(3), detected_gt_G(3);
 978 |   vector<vector<vector<int32_t> > > accepted_det_2D(3), accepted_det_3D(3), accepted_det_G(3);
 979 |   vector<vector<vector<double> > > overlap_det_2D(3), overlap_det_3D(3), overlap_det_G(3);
 980 |   vector<vector<vector<bool> > > fp_det_2D(3), fp_det_3D(3), fp_det_G(3);
 981 |   for (int difficulty=0; difficulty<3; difficulty++){
 982 |     for (int32_t i=0; i<indices.size(); i++) {
 983 |       if (indices[i]==6493) {cout << "DEBUG: 6493: " << i << endl;}
 984 |       int n_gt = groundtruth[i].size();
 985 |       int n_det = detections[i].size();
 986 |       detected_gt_2D[difficulty].push_back(vector<int32_t>(n_gt,-1));
 987 |       detected_gt_3D[difficulty].push_back(vector<int32_t>(n_gt,-1));
 988 |       detected_gt_G[difficulty].push_back(vector<int32_t>(n_gt,-1));
 989 |       accepted_det_2D[difficulty].push_back(vector<int32_t>(n_det,-1));
 990 |       accepted_det_3D[difficulty].push_back(vector<int32_t>(n_det,-1));
 991 |       accepted_det_G[difficulty].push_back(vector<int32_t>(n_det,-1));
 992 |       overlap_det_2D[difficulty].push_back(vector<double>(n_det,0));
 993 |       overlap_det_3D[difficulty].push_back(vector<double>(n_det,0));
 994 |       overlap_det_G[difficulty].push_back(vector<double>(n_det,0));
 995 |       fp_det_2D[difficulty].push_back(vector<bool>(n_det,false));
 996 |       fp_det_3D[difficulty].push_back(vector<bool>(n_det,false));
 997 |       fp_det_G[difficulty].push_back(vector<bool>(n_det,false));
 998 |     }
 999 |   }
1000 |     
1001 | 
1002 |   // holds pointers for result files
1003 |   FILE *filep_det=0, *fp_ori=0;
1004 | 
1005 | 
1006 |   // don't evaluate AOS for birdview boxes and 3D boxes
1007 |   compute_aos = false;
1008 |   compute_aos_ground = true;
1009 |   // eval bird's eye view bounding boxes
1010 |   for (int c = 0; c < NUM_CLASS; c++) {
1011 |     CLASSES cls = (CLASSES)c;
1012 |     if (eval_ground[c]) {
1013 |       filep_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection_ground.txt").c_str(), "w");
1014 |       vector<double> precision[3], aos[3], aos_ground[3];
1015 |       if(   !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_G[EASY], accepted_det_G[EASY], fp_det_G[EASY], overlap_det_G[EASY], compute_aos,compute_aos_ground, groundBoxOverlap, precision[0], aos[0],aos_ground[0], EASY, GROUND)
1016 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_G[MODERATE], accepted_det_G[MODERATE], fp_det_G[MODERATE], overlap_det_G[MODERATE], compute_aos,compute_aos_ground, groundBoxOverlap, precision[1], aos[1],aos_ground[1], MODERATE, GROUND)
1017 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_G[HARD], accepted_det_G[HARD], fp_det_G[HARD], overlap_det_G[HARD], compute_aos,compute_aos_ground, groundBoxOverlap, precision[2], aos[2],aos_ground[2], HARD, GROUND)) {
1018 |         mail->msg("%s evaluation failed.", CLASS_NAMES[c].c_str());
1019 |         return false;
1020 |       }
1021 |       fclose(filep_det);
1022 |       saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection_BEV",CLASS_NAMES[c], precision, 0);
1023 |       if(compute_aos_ground)
1024 |         printAp(CLASS_NAMES[c] + "_heading_BEV",aos_ground);
1025 |     }
1026 |   }
1027 | 
1028 |   // eval image 2D bounding boxes
1029 |   for (int c = 0; c < NUM_CLASS; c++) {
1030 |     CLASSES cls = (CLASSES)c;
1031 |     if (eval_image[c]) {
1032 |       filep_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection.txt").c_str(), "w");
1033 |       if(compute_aos)
1034 |         fp_ori = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_orientation.txt").c_str(),"w");
1035 |       vector<double> precision[3], aos[3], aos_ground[3];
1036 |       if(   !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_2D[EASY], accepted_det_2D[EASY], fp_det_2D[EASY], overlap_det_2D[EASY], compute_aos,compute_aos_ground, imageBoxOverlap, precision[0], aos[0],aos_ground[0], EASY, IMAGE)
1037 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_2D[MODERATE], accepted_det_2D[MODERATE], fp_det_2D[MODERATE], overlap_det_2D[MODERATE], compute_aos,compute_aos_ground, imageBoxOverlap, precision[1], aos[1],aos_ground[0], MODERATE, IMAGE)
1038 |          || !eval_class(filep_det, fp_ori, cls, groundtruth, detections, detected_gt_2D[HARD], accepted_det_2D[HARD], fp_det_2D[HARD], overlap_det_2D[HARD], compute_aos,compute_aos_ground, imageBoxOverlap, precision[2], aos[2],aos_ground[0], HARD, IMAGE)) {
1039 |         mail->msg("%s evaluation failed.", CLASS_NAMES[c].c_str());
1040 |         return false;
1041 |       }
1042 |       fclose(filep_det);
1043 |       saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection", CLASS_NAMES[c], precision, 0);
1044 |       if(compute_aos){
1045 |         saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_orientation", CLASS_NAMES[c], aos, 1);
1046 |         fclose(fp_ori);
1047 |       }
1048 |     }
1049 |   }
1050 | 
1051 | 
1052 | 
1053 |   // eval 3D bounding boxes
1054 |   for (int c = 0; c < NUM_CLASS; c++) {
1055 |     CLASSES cls = (CLASSES)c;
1056 |     if (eval_3d[c]) {
1057 |       filep_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection_ground.txt").c_str(), "w");
1058 |       vector<double> precision[3], aos[3], aos_ground[3];
1059 |       if(   !eval_class(filep_det, fp_ori,cls, groundtruth, detections, detected_gt_3D[EASY], accepted_det_3D[EASY], fp_det_3D[EASY], overlap_det_3D[EASY], compute_aos,compute_aos_ground, box3DOverlap, precision[0], aos[0],aos_ground[0], EASY, BOX3D)
1060 |          || !eval_class(filep_det, fp_ori,cls, groundtruth, detections, detected_gt_3D[MODERATE], accepted_det_3D[MODERATE], fp_det_3D[MODERATE], overlap_det_3D[MODERATE], compute_aos,compute_aos_ground, box3DOverlap, precision[1], aos[1],aos_ground[1], MODERATE, BOX3D)
1061 |          || !eval_class(filep_det, fp_ori,cls, groundtruth, detections, detected_gt_3D[HARD], accepted_det_3D[HARD], fp_det_3D[HARD], overlap_det_3D[HARD], compute_aos,compute_aos_ground, box3DOverlap, precision[2], aos[2],aos_ground[2], HARD, BOX3D)) {
1062 |         mail->msg("%s evaluation failed.", CLASS_NAMES[c].c_str());
1063 |         return false;
1064 |       }
1065 |       fclose(filep_det);
1066 |       saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection_3D",CLASS_NAMES[c], precision, 0);
1067 |       if(compute_aos_ground)
1068 |         printAp(CLASS_NAMES[c] + "_heading_3D",aos_ground);
1069 |     }
1070 |   }
1071 | 
1072 | 
1073 |   // WZN: write to file
1074 |   cout << "Indices: " << indices[0] << " " << indices[1] << " " << indices[2] << " " << indices[3] << " " << indices[4] << " " << indices[5] << endl;
1075 |   cout << "DEBUG: " << detected_gt_3D.size() << ", " << detected_gt_G.size() << ", " << detected_gt_2D.size() << endl;
1076 |   cout << "DEBUG: " << detected_gt_3D[0].size() << ", " << detected_gt_G[0].size() << ", " << detected_gt_2D[0].size() << endl;
1077 |   write_detection_file(result_dir, indices, accepted_det_3D[EASY], accepted_det_G[EASY], accepted_det_2D[EASY], fp_det_3D[EASY], fp_det_G[EASY], fp_det_2D[EASY], overlap_det_3D[EASY], overlap_det_G[EASY], overlap_det_2D[EASY], "EASY");
1078 |   write_gt_file(result_dir,indices, detected_gt_3D[EASY], detected_gt_G[EASY], detected_gt_2D[EASY], "EASY");
1079 |   write_detection_file(result_dir,indices, accepted_det_3D[MODERATE], accepted_det_G[MODERATE], accepted_det_2D[MODERATE], fp_det_3D[MODERATE], fp_det_G[MODERATE], fp_det_2D[MODERATE], overlap_det_3D[MODERATE], overlap_det_G[MODERATE], overlap_det_2D[MODERATE], "MODERATE");
1080 |   write_gt_file(result_dir,indices, detected_gt_3D[MODERATE], detected_gt_G[MODERATE], detected_gt_2D[MODERATE], "MODERATE");
1081 |   write_detection_file(result_dir,indices, accepted_det_3D[HARD], accepted_det_G[HARD], accepted_det_2D[HARD], fp_det_3D[HARD], fp_det_G[HARD], fp_det_2D[HARD], overlap_det_3D[HARD], overlap_det_G[HARD], overlap_det_2D[HARD], "HARD");
1082 |   write_gt_file(result_dir,indices, detected_gt_3D[HARD], detected_gt_G[HARD], detected_gt_2D[HARD], "HARD");
1083 | 
1084 |   // success
1085 |   return true;
1086 | }
1087 | 
1088 | int32_t main (int32_t argc,char *argv[]) {
1089 | 
1090 |   // we need 2 or 4 arguments!
1091 |   if (argc!=3) {
1092 |     cout << "Usage: ./eval_detection_3d_offline gt_dir result_dir" << endl;
1093 |     return 1;
1094 |   }
1095 | 
1096 |   // read arguments
1097 |   string gt_dir = argv[1];
1098 |   string result_dir = argv[2];
1099 | 
1100 |   // init notification mail
1101 |   Mail *mail;
1102 |   mail = new Mail();
1103 |   //mail->msg("Thank you for participating in our evaluation!");
1104 | 
1105 |   // run evaluation
1106 |   if (eval(gt_dir, result_dir, mail)) {
1107 |     //mail->msg("Your evaluation results are available at:");
1108 |     //mail->msg(result_dir.c_str());
1109 |   } else {
1110 |     system(("rm -r " + result_dir + "/plot").c_str());
1111 |     mail->msg("An error occured while processing your results.");
1112 |   }
1113 | 
1114 |   // send mail and exit
1115 |   delete mail;
1116 | 
1117 |   return 0;
1118 | }
1119 | 


--------------------------------------------------------------------------------
/kitti_evaluation/hack_kitti_evaluation.h:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | #include <fstream>
  3 | #include <stdio.h>
  4 | #include <math.h>
  5 | #include <vector>
  6 | #include <numeric>
  7 | #include <strings.h>
  8 | using namespace std;
  9 | typedef vector<vector<int32_t> > vvvi;
 10 | typedef vector<vector<double> >  vvvd;
 11 | typedef vector<vector<bool> > vvvb;
 12 | 
 13 | 
 14 | void write_detection_file(string result_dir, vector<int32_t> indices, vvvi &accepted_det_3D, vvvi &accepted_det_G, vvvi &accepted_det_2D,
 15 |     vvvb &fp_det_3D, vvvb &fp_det_G, vvvb &fp_det_2D, vvvd &overlap_det_3D, vvvd &overlap_det_G, vvvd &overlap_det_2D, string difficulty){
 16 | 	ofstream fdet;
 17 | 	fdet.open((result_dir + "/hack_det_" + difficulty + "_evaluation.txt").c_str());
 18 | 	fdet << "frame, index, fp_det3D, fpGr, fp2D, associated_gt3D, gtGr, gt2D, overlap_3D,ov_Gr,ov_2D" << endl;
 19 | 	size_t nframe = accepted_det_2D.size();
 20 | 	assert(nframe==indices.size());
 21 | 	int fp_det3, fp_detG, fp_det2;
 22 | 	int ass_gt3, ass_gtG, ass_gt2;
 23 | 	for (int frameNum=0; frameNum<accepted_det_3D.size();frameNum++){
 24 | 		vector<int32_t> i_a_det2 = accepted_det_2D[frameNum];
 25 | 		vector<double> i_o_det2 = overlap_det_2D[frameNum];
 26 | 		vector<bool> i_fp_det2 = fp_det_2D[frameNum];
 27 | 		vector<int32_t> i_a_det3;
 28 | 		vector<double> i_o_det3;
 29 | 		vector<bool> i_fp_det3;
 30 | 		vector<int32_t> i_a_detG;
 31 | 		vector<double> i_o_detG;
 32 | 		vector<bool> i_fp_detG;
 33 | 
 34 | 		size_t n = i_a_det2.size();
 35 | 		if (accepted_det_3D.size()>frameNum){
 36 | 			i_a_det3 = accepted_det_3D[frameNum];
 37 | 			i_o_det3 = overlap_det_3D[frameNum];
 38 | 			i_fp_det3 = fp_det_3D[frameNum];
 39 | 		}else{
 40 | 			i_a_det3 = vector<int32_t>(n,-1);
 41 | 			i_o_det3 = vector<double>(n,0);
 42 | 			i_fp_det3 = vector<bool>(n,false);
 43 | 		}
 44 | 		if (accepted_det_G.size()>frameNum){
 45 | 			i_a_detG = accepted_det_G[frameNum];
 46 | 			i_o_detG = overlap_det_G[frameNum];
 47 | 			i_fp_detG = fp_det_G[frameNum];
 48 | 		}else{
 49 | 			i_a_detG = vector<int32_t>(n,-1);
 50 | 			i_o_detG = vector<double>(n,0);
 51 | 			i_fp_detG = vector<bool>(n,false);
 52 | 		}
 53 | 		for (int j=0; j<i_a_det3.size(); j++){
 54 | 			if (i_a_det3[j] >= 0) {ass_gt3=i_a_det3[j];} else {ass_gt3=-1;};
 55 | 			if (i_a_detG[j] >= 0) {ass_gtG=i_a_detG[j];} else {ass_gtG=-1;};
 56 | 			if (i_a_det2[j] >= 0) {ass_gt2=i_a_det2[j];} else {ass_gt2=-1;};
 57 | 			if (i_fp_det3[j]) {fp_det3=1;} else {fp_det3=0;};
 58 | 			if (i_fp_detG[j]) {fp_detG=1;} else {fp_detG=0;};
 59 | 			if (i_fp_det2[j]) {fp_det2=1;} else {fp_det2=0;};
 60 | 
 61 | 			fdet << setfill(' ') << setw(5) << indices[frameNum] << ", " << setw(5) << j << ", " << setw(4+4) << fp_det3 
 62 | 			<< ", " << setw(4) << fp_detG << ", " << setw(4) << fp_det2 << ", " << setw(5+11) << ass_gt3 << ", " <<
 63 | 			setw(5) << ass_gtG << ", " << setw(5) << ass_gt2 << ", " << setprecision(2) << setw(5+4) << i_o_det3[j] << ", " << setw(5)<<
 64 | 			i_o_detG[j] << ", " << setw(5) << i_o_det2[j] << endl;
 65 | 			//fprintf(fdet, "%4s, %4s, %4s, %4s, %4s, %.2f, %.2f, %.2f\n",(indices[frameNum]),(j),
 66 | 			//	(i_a_det3[j]),(i_a_detG[j]),(i_a_det2[j]),i_o_det3[j],i_o_detG[j],i_o_det2[j]);
 67 | 		}
 68 | 	}
 69 | 	fdet.close();
 70 | }
 71 | 
 72 | void write_gt_file(string result_dir, vector<int32_t> indices, vvvi &detected_gt_3D, vvvi &detected_gt_G, vvvi &detected_gt_2D, string difficulty){
 73 | 	ofstream fgt;
 74 | 	fgt.open((result_dir + "/hack_gt_" + difficulty + "_evaluation.txt").c_str());
 75 | 	fgt << "frame, index, tp_gt_3D, tpGr, tp2D, fn_gt_3D, fnGr, fn2D, associated_det, det_Gr, det_2D" << endl;
 76 | 	int tp_gt3, tp_gtG, tp_gt2;
 77 | 	int fn_gt3, fn_gtG, fn_gt2;
 78 | 	int ass_gt3, ass_gtG, ass_gt2;
 79 | 	size_t nframe = detected_gt_2D.size();
 80 | 	assert(nframe==indices.size());
 81 | 	for (int frameNum=0; frameNum<nframe;frameNum++){
 82 | 		vector<int32_t> i_d_gt2 = detected_gt_2D[frameNum];
 83 | 		size_t n = i_d_gt2.size();
 84 | 		vector<int32_t> i_d_gt3;
 85 | 		i_d_gt3 = detected_gt_3D.size()>frameNum ? detected_gt_3D[frameNum]:vector<int32_t>(n,-1);
 86 | 		vector<int32_t> i_d_gtG;
 87 | 		i_d_gtG = detected_gt_G.size()>frameNum ? detected_gt_G[frameNum]:vector<int32_t>(n,-1);
 88 | 		for (int j=0; j<i_d_gt3.size(); j++){
 89 | 			if (i_d_gt3[j] >= 0) {tp_gt3=1;ass_gt3=i_d_gt3[j];} else {tp_gt3=0;ass_gt3=-1;};
 90 | 			if (i_d_gtG[j] >= 0) {tp_gtG=1;ass_gtG=i_d_gtG[j];} else {tp_gtG=0;ass_gtG=-1;};
 91 | 			if (i_d_gt2[j] >= 0) {tp_gt2=1;ass_gt2=i_d_gt2[j];} else {tp_gt2=0;ass_gt2=-1;};
 92 | 			if (i_d_gt3[j] == -10) {fn_gt3=1;} else {fn_gt3=0;};
 93 | 			if (i_d_gtG[j] == -10) {fn_gtG=1;} else {fn_gtG=0;};
 94 | 			if (i_d_gt2[j] == -10) {fn_gt2=1;} else {fn_gt2=0;};
 95 | 			fgt << setfill(' ') << setw(5) << indices[frameNum] << ", "  << setw(5) << j << ", " << setw(4+4) << tp_gt3 
 96 | 			<< ", " << setw(4) << tp_gtG << ", " << setw(4) << tp_gt2 << ", "  << setw(4+4) << fn_gt3 
 97 | 			<< ", " << setw(4) << fn_gtG << ", " << setw(4) << fn_gt2 << ", " << setw(5+9) << 
 98 | 			ass_gt3 << ", " << setw(5) << ass_gtG << ", " << setw(5) << ass_gt2 << endl;
 99 | 			//fprintf(fgt, "%4s, %4s, %4s, %4s, %4s, %4s, %4s, %4s\n",to_string(indices[frameNum]),j,
100 | 			//	to_string(tp_gt3), to_string(tp_gtG), to_string(tp_gt2), to_string(i_d_gt3[j]), to_string(i_d_gtG[j]), to_string(i_d_gt2[j]));
101 | 		}
102 | 	}
103 | 	fgt.close();
104 | }
105 | 


--------------------------------------------------------------------------------
/mains/pickle_to_kitti_format.py:
--------------------------------------------------------------------------------
 1 | import sys
 2 | import os
 3 | import numpy as np
 4 | sys.path.append(os.path.abspath('../'))
 5 | from utils.probability_utils import Hujie_uncertainty_reader as unc_reader
 6 | from utils.kitti_utils import save_kitti_format
 7 | from utils.calibration import Calibration
 8 | from utils.simple_stats import get_dirs
 9 | 
10 | data_dir = '/mnt/d/Berkeley/Kitti_data/object' 
11 | actual_test = False
12 | pred_3d_dir = ('/mnt/d/Berkeley/Kitti_data/predictions/Hujie_unc_full_val50/')
13 | 
14 | def main():
15 | 	list_dir, img_dir, lidar_dir, calib_dir, label_3d_dir = get_dirs(data_dir, actual_test, val_set='Hujie')
16 | 	with open(list_dir) as fi:
17 | 		file_lists = fi.read().splitlines() 
18 | 		file_lists.sort()
19 | 	unc_data = unc_reader(pred_3d_dir, file_lists)
20 | 	for filename in file_lists:
21 | 		frame = int(filename.split('.')[0])
22 | 		bbox3d = np.concatenate([unc_data['box3D'][frame], unc_data['ry'][frame].reshape((-1,1))], axis=1)
23 | 		calib = Calibration(os.path.join(calib_dir, filename + '.txt'))
24 | 		img_shape = [1240, 375]
25 | 		save_kitti_format(frame, calib, bbox3d, pred_3d_dir+'data/', np.ones(bbox3d.shape[0]), img_shape)
26 | 
27 | if __name__ == '__main__':
28 | 	main()


--------------------------------------------------------------------------------
/mains/write_JIoU.py:
--------------------------------------------------------------------------------
 1 | # This is a simplified version from simple_stats aimed for only generating JIoUs, but with the support for other prediction uncertainty form
 2 | import sys
 3 | import os
 4 | import numpy as np
 5 | sys.path.append(os.path.abspath('../'))
 6 | import utils.metric_utils#probability_utils
 7 | from utils.probability_utils import Hujie_uncertainty_reader as unc_reader
 8 | from utils.simple_stats import label_reader, detect_reader, hacker_reader, evaluate_JIoU, get_dirs, read_points_cam
 9 | from utils.metric_plots import write_JIoU
10 | from tqdm import tqdm
11 | 
12 | # Labels and data
13 | actual_test = False
14 | data_dir = '/mnt/d/Berkeley/Kitti_data/object' 
15 | 
16 | # Detection results
17 | networks = ['PointRCNN_unc_full']
18 | pred_3d_dirs = {}
19 | pred_3d_dirs['PointRCNN_unc_full'] = ('/mnt/d/Berkeley/Kitti_data/predictions/Hujie_unc_full_val50/')
20 | 
21 | # Output directory
22 | output_root = './results/'
23 | label_uncertainty_summary_folder =  'summary/uncertainty/'
24 | 
25 | def main():
26 | 	# (1) get file list, label and images
27 | 	list_dir, img_dir, lidar_dir, calib_dir, label_3d_dir = get_dirs(data_dir, actual_test, val_set='Hujie')
28 | 
29 | 	num_net = len(networks)
30 | 	pred_datas = [[] for net in networks]
31 | 	hack_datas = [[] for net in networks]
32 | 	unc_datas = [[] for net in networks]
33 | 
34 | 	for net in networks:
35 | 		output_dir = output_root + 'results_visualization_' + net
36 | 	output_dir += '/'
37 | 	os.makedirs(output_dir, exist_ok=True)
38 | 	# Read data from filelist.
39 | 	with open(list_dir) as fi:
40 | 		file_lists = fi.read().splitlines() 
41 | 	file_lists.sort()
42 | 
43 | 	label_data = label_reader(label_3d_dir,file_lists,calib_dir)
44 | 	for inet, net in enumerate(networks):
45 | 		hack_datas[inet] = hacker_reader(pred_3d_dirs[net], file_lists)
46 | 		pred_datas[inet] = detect_reader(pred_3d_dirs[net], file_lists, label_3d_dir.find('waymo') != -1)
47 | 		if 'unc' in pred_3d_dirs[net]:
48 | 			unc_datas[inet] = unc_reader(pred_3d_dirs[net], file_lists)
49 | 		else:
50 | 			unc_datas[inet] = None
51 | 
52 | 	#all labels, JIoUs vs IoU
53 | 	difficulty = 'HARD'
54 | 	view = 'G'
55 | 	frames = [int(file.split('.')[0]) for file in file_lists]
56 | 	all_gt_idxs = [{net:[] for net in networks} for frame in frames]
57 | 	all_pd_idxs = [{net:[] for net in networks} for frame in frames]
58 | 	all_IoU_dic = [{net:[] for net in networks} for frame in frames]
59 | 	corner_totalVariances = []
60 | 	print('Processing JIoUs: ')
61 | 	for id_file ,file in enumerate(tqdm(file_lists)):
62 | 		frame = int(file.split('.')[0])
63 | 		points_cam = read_points_cam(lidar_dir, frame, label_data)
64 | 		num_active_gts = 0
65 | 		IoU_dic = {}
66 | 		all_gts = np.zeros(len(hack_datas[0][difficulty]['gt'][view]['tp'][frame]))
67 | 		for inet, net in enumerate(networks):
68 | 			gts = np.logical_or(np.array(hack_datas[inet][difficulty]['gt'][view]['tp'][frame])==1,np.array(hack_datas[inet][difficulty]['gt'][view]['fn'][frame])==1)
69 | 			all_gts = np.logical_or(all_gts, gts)
70 | 		active_gt_idxs = np.argwhere(all_gts)
71 | 		num_active_gts += active_gt_idxs.size
72 | 		for inet, net in enumerate(networks):
73 | 			output_newIoU_dir = output_dir + net + '_JIoUs/'
74 | 			os.makedirs(output_newIoU_dir, exist_ok=True)
75 | 			if active_gt_idxs.size>0:
76 | 				ass_det_idxs = np.array(hack_datas[inet][difficulty]['gt'][view]['det'][frame])[active_gt_idxs]
77 | 				gt_idxs = [active_gt_idxs.item(i) for i in range(active_gt_idxs.size)]
78 | 				pd_idxs =  [ass_det_idxs.item(i) for i in range(ass_det_idxs.size)]
79 | 				IoU = []
80 | 				for pd_idx in pd_idxs:
81 | 					if pd_idx>=0:
82 | 						IoU.append(hack_datas[inet][difficulty]['det'][view]['iou'][frame][pd_idx])
83 | 					else:
84 | 						IoU.append(0)
85 | 				all_gt_idxs[id_file][net] = gt_idxs
86 | 				all_pd_idxs[id_file][net] = pd_idxs
87 | 				JIoUs = evaluate_JIoU(label_data, pred_datas[inet], frame, gt_idxs, pd_idxs, points_cam, grid_size=0.1, sample_grid=0.02, unc_data=unc_datas[inet])
88 | 				for iI in range(len(IoU)):
89 | 					tmp = [IoU[iI]] + NewIoUs[iI]
90 | 					all_IoU_dic[id_file][net].append(tmp)
91 | 
92 | 	output_JIoU_dir = output_dir + label_uncertainty_summary_folder
93 | 	write_JIoU(output_JIoU_dir, networks, frames, all_gt_idxs, all_pd_idxs, all_IoU_dic)
94 | 	#write_corner_variances(output_root, corner_totalVariances)
95 | 
96 | 
97 | if __name__ == '__main__':
98 | 	main()


--------------------------------------------------------------------------------
/utils/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ZiningWang/Inferring-Spatial-Uncertainty-in-Object-Detection/c16ccc2f62047765a48f4fe6c1fa559cbd2784a3/utils/__init__.py


--------------------------------------------------------------------------------
/utils/calibration.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import os
  3 | 
  4 | 
  5 | def get_calib_from_file(calib_file):
  6 |     with open(calib_file) as f:
  7 |         lines = f.readlines()
  8 | 
  9 |     obj = lines[2].strip().split(' ')[1:]
 10 |     P2 = np.array(obj, dtype=np.float32)
 11 |     obj = lines[3].strip().split(' ')[1:]
 12 |     P3 = np.array(obj, dtype=np.float32)
 13 |     obj = lines[4].strip().split(' ')[1:]
 14 |     R0 = np.array(obj, dtype=np.float32)
 15 |     obj = lines[5].strip().split(' ')[1:]
 16 |     Tr_velo_to_cam = np.array(obj, dtype=np.float32)
 17 | 
 18 |     return {'P2': P2.reshape(3, 4),
 19 |             'P3': P3.reshape(3, 4),
 20 |             'R0': R0.reshape(3, 3),
 21 |             'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)}
 22 | 
 23 | 
 24 | class Calibration(object):
 25 |     def __init__(self, calib_file):
 26 |         if isinstance(calib_file, str):
 27 |             calib = get_calib_from_file(calib_file)
 28 |         else:
 29 |             calib = calib_file
 30 | 
 31 |         self.P2 = calib['P2']  # 3 x 4
 32 |         self.R0 = calib['R0']  # 3 x 3
 33 |         self.V2C = calib['Tr_velo2cam']  # 3 x 4
 34 | 
 35 |         # Camera intrinsics and extrinsics
 36 |         self.cu = self.P2[0, 2]
 37 |         self.cv = self.P2[1, 2]
 38 |         self.fu = self.P2[0, 0]
 39 |         self.fv = self.P2[1, 1]
 40 |         self.tx = self.P2[0, 3] / (-self.fu)
 41 |         self.ty = self.P2[1, 3] / (-self.fv)
 42 | 
 43 |     def cart_to_hom(self, pts):
 44 |         """
 45 |         :param pts: (N, 3 or 2)
 46 |         :return pts_hom: (N, 4 or 3)
 47 |         """
 48 |         pts_hom = np.hstack((pts, np.ones((pts.shape[0], 1), dtype=np.float32)))
 49 |         return pts_hom
 50 | 
 51 |     def lidar_to_rect(self, pts_lidar):
 52 |         """
 53 |         :param pts_lidar: (N, 3)
 54 |         :return pts_rect: (N, 3)
 55 |         """
 56 |         pts_lidar_hom = self.cart_to_hom(pts_lidar)
 57 |         pts_rect = np.dot(pts_lidar_hom, np.dot(self.V2C.T, self.R0.T))
 58 |         # pts_rect = reduce(np.dot, (pts_lidar_hom, self.V2C.T, self.R0.T))
 59 |         return pts_rect
 60 | 
 61 |     def rect_to_img(self, pts_rect):
 62 |         """
 63 |         :param pts_rect: (N, 3)
 64 |         :return pts_img: (N, 2)
 65 |         """
 66 |         pts_rect_hom = self.cart_to_hom(pts_rect)
 67 |         pts_2d_hom = np.dot(pts_rect_hom, self.P2.T)
 68 |         pts_img = (pts_2d_hom[:, 0:2].T / pts_rect_hom[:, 2]).T  # (N, 2)
 69 |         pts_rect_depth = pts_2d_hom[:, 2] - self.P2.T[3, 2]  # depth in rect camera coord
 70 |         return pts_img, pts_rect_depth
 71 | 
 72 |     def lidar_to_img(self, pts_lidar):
 73 |         """
 74 |         :param pts_lidar: (N, 3)
 75 |         :return pts_img: (N, 2)
 76 |         """
 77 |         pts_rect = self.lidar_to_rect(pts_lidar)
 78 |         pts_img, pts_depth = self.rect_to_img(pts_rect)
 79 |         return pts_img, pts_depth
 80 | 
 81 |     def img_to_rect(self, u, v, depth_rect):
 82 |         """
 83 |         :param u: (N)
 84 |         :param v: (N)
 85 |         :param depth_rect: (N)
 86 |         :return:
 87 |         """
 88 |         x = ((u - self.cu) * depth_rect) / self.fu + self.tx
 89 |         y = ((v - self.cv) * depth_rect) / self.fv + self.ty
 90 |         pts_rect = np.concatenate((x.reshape(-1, 1), y.reshape(-1, 1), depth_rect.reshape(-1, 1)), axis=1)
 91 |         return pts_rect
 92 | 
 93 |     def depthmap_to_rect(self, depth_map):
 94 |         """
 95 |         :param depth_map: (H, W), depth_map
 96 |         :return:
 97 |         """
 98 |         x_range = np.arange(0, depth_map.shape[1])
 99 |         y_range = np.arange(0, depth_map.shape[0])
100 |         x_idxs, y_idxs = np.meshgrid(x_range, y_range)
101 |         x_idxs, y_idxs = x_idxs.reshape(-1), y_idxs.reshape(-1)
102 |         depth = depth_map[y_idxs, x_idxs]
103 |         pts_rect = self.img_to_rect(x_idxs, y_idxs, depth)
104 |         return pts_rect, x_idxs, y_idxs
105 | 
106 |     def corners3d_to_img_boxes(self, corners3d):
107 |         """
108 |         :param corners3d: (N, 8, 3) corners in rect coordinate
109 |         :return: boxes: (None, 4) [x1, y1, x2, y2] in rgb coordinate
110 |         :return: boxes_corner: (None, 8) [xi, yi] in rgb coordinate
111 |         """
112 |         sample_num = corners3d.shape[0]
113 |         corners3d_hom = np.concatenate((corners3d, np.ones((sample_num, 8, 1))), axis=2)  # (N, 8, 4)
114 | 
115 |         img_pts = np.matmul(corners3d_hom, self.P2.T)  # (N, 8, 3)
116 | 
117 |         x, y = img_pts[:, :, 0] / img_pts[:, :, 2], img_pts[:, :, 1] / img_pts[:, :, 2]
118 |         x1, y1 = np.min(x, axis=1), np.min(y, axis=1)
119 |         x2, y2 = np.max(x, axis=1), np.max(y, axis=1)
120 | 
121 |         boxes = np.concatenate((x1.reshape(-1, 1), y1.reshape(-1, 1), x2.reshape(-1, 1), y2.reshape(-1, 1)), axis=1)
122 |         boxes_corner = np.concatenate((x.reshape(-1, 8, 1), y.reshape(-1, 8, 1)), axis=2)
123 | 
124 |         return boxes, boxes_corner
125 | 
126 |     def camera_dis_to_rect(self, u, v, d):
127 |         """
128 |         Can only process valid u, v, d, which means u, v can not beyond the image shape, reprojection error 0.02
129 |         :param u: (N)
130 |         :param v: (N)
131 |         :param d: (N), the distance between camera and 3d points, d^2 = x^2 + y^2 + z^2
132 |         :return:
133 |         """
134 |         assert self.fu == self.fv, '%.8f != %.8f' % (self.fu, self.fv)
135 |         fd = np.sqrt((u - self.cu)**2 + (v - self.cv)**2 + self.fu**2)
136 |         x = ((u - self.cu) * d) / fd + self.tx
137 |         y = ((v - self.cv) * d) / fd + self.ty
138 |         z = np.sqrt(d**2 - x**2 - y**2)
139 |         pts_rect = np.concatenate((x.reshape(-1, 1), y.reshape(-1, 1), z.reshape(-1, 1)), axis=1)
140 |         return pts_rect
141 | 


--------------------------------------------------------------------------------
/utils/kitti_utils.py:
--------------------------------------------------------------------------------
  1 | import os
  2 | import numpy as np
  3 | from scipy.spatial import Delaunay
  4 | import scipy
  5 | 
  6 | from easydict import EasyDict as edict
  7 | #import lib.utils.object3d as object3d
  8 | 
  9 | '''
 10 | def get_objects_from_label(label_file):
 11 |     with open(label_file, 'r') as f:
 12 |         lines = f.readlines()
 13 |     objects = [object3d.Object3d(line) for line in lines]
 14 |     return objects
 15 | '''
 16 | 
 17 | def box3d_to_boxBEV(box3d, ry=None):
 18 |     # Kitti 3D bounding box is [x, y_top, z, h, w, l]
 19 |     assert len(box3d.shape)==2, 'The box3d must be n*6 or n*7.'
 20 |     if box3d.shape[1] == 7:
 21 |         boxBEV = box3d[:, [0,2,5,4,6]]
 22 |     elif ry is not None:
 23 |         boxBEV = np.concatenate((box3d[:, [0,2,5,4]], ry.reshape([-1,1])), axis=1)
 24 |     else:
 25 |         assert False, 'Error: box3d and ry not provided correctly.'
 26 |     return boxBEV
 27 | 
 28 | def dist_to_plane(plane, points):
 29 |     """
 30 |     Calculates the signed distance from a 3D plane to each point in a list of points
 31 |     :param plane: (a, b, c, d)
 32 |     :param points: (N, 3)
 33 |     :return: (N), signed distance of each point to the plane
 34 |     """
 35 |     a, b, c, d = plane
 36 | 
 37 |     points = np.array(points)
 38 |     x = points[:, 0]
 39 |     y = points[:, 1]
 40 |     z = points[:, 2]
 41 | 
 42 |     return (a*x + b*y + c*z + d) / np.sqrt(a**2 + b**2 + c**2)
 43 | 
 44 | 
 45 | def rotate_pc_along_y(pc, rot_angle):
 46 |     """
 47 |     params pc: (N, 3+C), (N, 3) is in the rectified camera coordinate
 48 |     params rot_angle: rad scalar
 49 |     Output pc: updated pc with XYZ rotated
 50 |     """
 51 |     cosval = np.cos(rot_angle)
 52 |     sinval = np.sin(rot_angle)
 53 |     rotmat = np.array([[cosval, -sinval], [sinval, cosval]])
 54 |     pc[:, [0, 2]] = np.dot(pc[:, [0, 2]], np.transpose(rotmat))
 55 |     return pc
 56 | 
 57 | 
 58 | def boxes3d_to_corners3d(boxes3d, rotate=True):
 59 |     """
 60 |     :param boxes3d: (N, 7) [x, y, z, h, w, l, ry]
 61 |     :param rotate:
 62 |     :return: corners3d: (N, 8, 3)
 63 |     """
 64 |     boxes_num = boxes3d.shape[0]
 65 |     h, w, l = boxes3d[:, 3], boxes3d[:, 4], boxes3d[:, 5]
 66 |     x_corners = np.array([l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.], dtype=np.float32).T  # (N, 8)
 67 |     z_corners = np.array([w / 2., -w / 2., -w / 2., w / 2., w / 2., -w / 2., -w / 2., w / 2.], dtype=np.float32).T  # (N, 8)
 68 | 
 69 |     y_corners = np.zeros((boxes_num, 8), dtype=np.float32)
 70 |     y_corners[:, 4:8] = -h.reshape(boxes_num, 1).repeat(4, axis=1)  # (N, 8)
 71 | 
 72 |     if rotate:
 73 |         ry = boxes3d[:, 6]
 74 |         zeros, ones = np.zeros(ry.size, dtype=np.float32), np.ones(ry.size, dtype=np.float32)
 75 |         rot_list = np.array([[np.cos(ry), zeros, -np.sin(ry)],
 76 |                              [zeros,       ones,       zeros],
 77 |                              [np.sin(ry), zeros,  np.cos(ry)]])  # (3, 3, N)
 78 |         R_list = np.transpose(rot_list, (2, 0, 1))  # (N, 3, 3)
 79 | 
 80 |         temp_corners = np.concatenate((x_corners.reshape(-1, 8, 1), y_corners.reshape(-1, 8, 1),
 81 |                                        z_corners.reshape(-1, 8, 1)), axis=2)  # (N, 8, 3)
 82 |         rotated_corners = np.matmul(temp_corners, R_list)  # (N, 8, 3)
 83 |         x_corners, y_corners, z_corners = rotated_corners[:, :, 0], rotated_corners[:, :, 1], rotated_corners[:, :, 2]
 84 | 
 85 |     x_loc, y_loc, z_loc = boxes3d[:, 0], boxes3d[:, 1], boxes3d[:, 2]
 86 | 
 87 |     x = x_loc.reshape(-1, 1) + x_corners.reshape(-1, 8)
 88 |     y = y_loc.reshape(-1, 1) + y_corners.reshape(-1, 8)
 89 |     z = z_loc.reshape(-1, 1) + z_corners.reshape(-1, 8)
 90 | 
 91 |     corners = np.concatenate((x.reshape(-1, 8, 1), y.reshape(-1, 8, 1), z.reshape(-1, 8, 1)), axis=2)
 92 | 
 93 |     return corners.astype(np.float32)
 94 | 
 95 | def enlarge_box3d(boxes3d, extra_width):
 96 |     """
 97 |     :param boxes3d: (N, 7) [x, y, z, h, w, l, ry]
 98 |     """
 99 |     if isinstance(boxes3d, np.ndarray):
100 |         large_boxes3d = boxes3d.copy()
101 |     else:
102 |         large_boxes3d = boxes3d.clone()
103 |     large_boxes3d[:, 3:6] += extra_width * 2
104 |     large_boxes3d[:, 1] += extra_width
105 |     return large_boxes3d
106 | 
107 | 
108 | def in_hull(p, hull):
109 |     """
110 |     :param p: (N, K) test points
111 |     :param hull: (M, K) M corners of a box
112 |     :return (N) bool
113 |     """
114 |     try:
115 |         if not isinstance(hull, Delaunay):
116 |             hull = Delaunay(hull)
117 |         flag = hull.find_simplex(p) >= 0
118 |     except scipy.spatial.qhull.QhullError:
119 |         print('Warning: not a hull %s' % str(hull))
120 |         flag = np.zeros(p.shape[0], dtype=np.bool)
121 | 
122 |     return flag
123 | 
124 | 
125 | def objs_to_boxes3d(obj_list):
126 |     boxes3d = np.zeros((obj_list.__len__(), 7), dtype=np.float32)
127 |     for k, obj in enumerate(obj_list):
128 |         boxes3d[k, 0:3], boxes3d[k, 3], boxes3d[k, 4], boxes3d[k, 5], boxes3d[k, 6] \
129 |             = obj.pos, obj.h, obj.w, obj.l, obj.ry
130 |     return boxes3d
131 | 
132 | 
133 | def objs_to_scores(obj_list):
134 |     scores = np.zeros((obj_list.__len__()), dtype=np.float32)
135 |     for k, obj in enumerate(obj_list):
136 |         scores[k] = obj.score
137 |     return scores
138 | 
139 | 
140 | def get_iou3d(corners3d, query_corners3d, need_bev=False):
141 |     """	
142 |     :param corners3d: (N, 8, 3) in rect coords	
143 |     :param query_corners3d: (M, 8, 3)	
144 |     :return:	
145 |     """
146 |     from shapely.geometry import Polygon
147 |     A, B = corners3d, query_corners3d
148 |     N, M = A.shape[0], B.shape[0]
149 |     iou3d = np.zeros((N, M), dtype=np.float32)
150 |     iou_bev = np.zeros((N, M), dtype=np.float32)
151 | 
152 |     # for height overlap, since y face down, use the negative y
153 |     min_h_a = -A[:, 0:4, 1].sum(axis=1) / 4.0
154 |     max_h_a = -A[:, 4:8, 1].sum(axis=1) / 4.0
155 |     min_h_b = -B[:, 0:4, 1].sum(axis=1) / 4.0
156 |     max_h_b = -B[:, 4:8, 1].sum(axis=1) / 4.0
157 | 
158 |     for i in range(N):
159 |         for j in range(M):
160 |             max_of_min = np.max([min_h_a[i], min_h_b[j]])
161 |             min_of_max = np.min([max_h_a[i], max_h_b[j]])	
162 |             h_overlap = np.max([0, min_of_max - max_of_min])
163 |             if h_overlap == 0:
164 |                 continue
165 | 
166 |             bottom_a, bottom_b = Polygon(A[i, 0:4, [0, 2]].T), Polygon(B[j, 0:4, [0, 2]].T)
167 |             if bottom_a.is_valid and bottom_b.is_valid:
168 |                 # check is valid,  A valid Polygon may not possess any overlapping exterior or interior rings.
169 |                 bottom_overlap = bottom_a.intersection(bottom_b).area
170 |             else:
171 |                 bottom_overlap = 0.
172 |             overlap3d = bottom_overlap * h_overlap
173 |             union3d = bottom_a.area * (max_h_a[i] - min_h_a[i]) + bottom_b.area * (max_h_b[j] - min_h_b[j]) - overlap3d
174 |             iou3d[i][j] = overlap3d / union3d
175 |             iou_bev[i][j] = bottom_overlap / (bottom_a.area + bottom_b.area - bottom_overlap)
176 | 
177 |     if need_bev:
178 |         return iou3d, iou_bev
179 | 
180 |     return iou3d
181 | 
182 | 
183 | def save_kitti_format(sample_id, calib, bbox3d, kitti_output_dir, scores, img_shape, class_str='Car'):
184 |     corners3d = boxes3d_to_corners3d(bbox3d)
185 |     img_boxes, _ = calib.corners3d_to_img_boxes(corners3d)
186 | 
187 |     img_boxes[:, 0] = np.clip(img_boxes[:, 0], 0, img_shape[1] - 1)
188 |     img_boxes[:, 1] = np.clip(img_boxes[:, 1], 0, img_shape[0] - 1)
189 |     img_boxes[:, 2] = np.clip(img_boxes[:, 2], 0, img_shape[1] - 1)
190 |     img_boxes[:, 3] = np.clip(img_boxes[:, 3], 0, img_shape[0] - 1)
191 | 
192 |     img_boxes_w = img_boxes[:, 2] - img_boxes[:, 0]
193 |     img_boxes_h = img_boxes[:, 3] - img_boxes[:, 1]
194 | 
195 |     kitti_output_file = os.path.join(kitti_output_dir, '%06d.txt' % sample_id)
196 |     with open(kitti_output_file, 'w') as f:
197 |         for k in range(bbox3d.shape[0]):
198 |             x, z, ry = bbox3d[k, 0], bbox3d[k, 2], bbox3d[k, 6]
199 |             beta = np.arctan2(z, x)
200 |             alpha = -np.sign(beta) * np.pi / 2 + beta + ry
201 |             while alpha < -np.pi:
202 |                 alpha += 2*np.pi
203 |             while alpha > np.pi:
204 |                 alpha -= 2*np.pi
205 |             while ry > np.pi:
206 |                 ry -= 2*np.pi
207 |             while ry < -np.pi:
208 |                 ry += 2*np.pi
209 | 
210 |             print('%s -1 -1 %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f' %
211 |                   (class_str, alpha, img_boxes[k, 0], img_boxes[k, 1], img_boxes[k, 2], img_boxes[k, 3],
212 |                    bbox3d[k, 3], bbox3d[k, 4], bbox3d[k, 5], bbox3d[k, 0], bbox3d[k, 1], bbox3d[k, 2],
213 |                    bbox3d[k, 6], scores[k]), file=f)
214 | 
215 | 
216 | def box3DtoObj(box3D, ry = None):
217 |     obj = edict({})
218 |     if ry:
219 |         obj.R = ry
220 |     else:
221 |         obj.R = box3D[6]
222 |     obj.width = box3D[4]
223 |     obj.length = box3D[5]
224 |     obj.x = box3D[0]#-y_lidar
225 |     obj.y = box3D[2]#x_lidar
226 |     return obj 


--------------------------------------------------------------------------------
/utils/metric_plots.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import os
  3 | import sys
  4 | import matplotlib.pyplot as plt
  5 | from matplotlib.patches import Polygon, Circle, Arrow
  6 | import matplotlib.gridspec as gridspec
  7 | import matplotlib.patches as patches
  8 | import scipy.misc
  9 | import logging
 10 | import coloredlogs
 11 | from PIL import Image, ImageDraw, ImageFont
 12 | from easydict import EasyDict as edict
 13 | from tqdm import tqdm
 14 | import glob
 15 | import cv2
 16 | import math
 17 | sys.path.append('../')
 18 | from utils.paul_geometry import draw_3dbox_in_2d, read_calib_mat, camera_to_lidar, draw_birdeye, cal_box3d_iou, lidar_to_camera, clip_by_BEV_box, get_cov_ellipse, draw_birdeye_ellipse, center_to_corner_BEV, align_BEV_to_corner
 19 | 
 20 | 
 21 | def draw_spatial_uncertainty_contour(obj, points_clip_BEV, uncertain_class, IoUcalculator, axes, sample_grid, grid_size):
 22 | 	levels = np.array(range(256))/255.0
 23 | 	#draw spatial distribution
 24 | 	sample_points, pxShape = IoUcalculator.get_sample_points(uncertain_class)
 25 | 	px = uncertain_class.sample_prob(sample_points, sample_grid=sample_grid)/grid_size**2
 26 | 	innerContour, outerContour = uncertain_class.calc_uncertainty_box(std=2)
 27 | 	innerContour = np.vstack((innerContour, innerContour[0,:]))
 28 | 	outerContour = np.vstack((outerContour, outerContour[0,:]))
 29 | 	#axes.plot(outerContour[:,0], outerContour[:,1],'y--', linewidth=1)
 30 | 	#axes.plot(innerContour[:, 0], innerContour[:, 1], 'y--', linewidth=1)
 31 | 	px[px>1] = 1.0
 32 | 	#px[px<0.04]=-0.1
 33 | 	jet_map = plt.cm.jet
 34 | 	#cmap.set_under('white',0.1)
 35 | 	cntr = axes.contourf(sample_points[:,0].reshape(pxShape), sample_points[:,1].reshape(pxShape), px.reshape(pxShape), levels, cmap=jet_map, vmin=0, vmax=1)
 36 | 	draw_birdeye(obj, axes, fill=False, color='r', linewidth=1)
 37 | 	#axes.scatter(points_clip_BEV[:, 0], points_clip_BEV[:, 1], c='r', marker='x', s=5)
 38 | 	axes.set(xticks=[],yticks=[])
 39 | 	axes.set_facecolor((0,0,0.53))
 40 | 	return cntr
 41 | 
 42 | def plot_multiview_label(label_data, img_dir, points_cam, frame, idxs, output_dir, IoU_dic=None, pd_idxs=None, pred_data=None, draw_uncertainty=True):
 43 | 	name = "{:06d}".format(frame)
 44 | 	out_file_name = name
 45 | 	img_path = os.path.join(img_dir,name+'.png')
 46 | 	plt.figure(figsize=(10,10))
 47 | 	ax = plt.gca()
 48 | 	gs = gridspec.GridSpec(3,3)
 49 | 	if os.path.exists(img_path):
 50 | 		img = scipy.misc.imread(img_path)
 51 | 		ax = plt.subplot(gs[0,0:3])
 52 | 		ax.imshow(img)
 53 | 		for idx in idxs:
 54 | 			box2D = label_data['box2D'][frame][idx]
 55 | 			ax.add_patch(patches.Rectangle((box2D[0],box2D[1]), box2D[2]-box2D[0], box2D[3]-box2D[1], edgecolor='red', fill=False))
 56 | 	for idx in idxs:
 57 | 		out_file_name += '_{}'.format(idx)
 58 | 	#DEBUG inference
 59 | 	ax = plt.subplot(gs[1:3,0:3])
 60 | 	ax.scatter(points_cam[:,0],points_cam[:,2],c='b',marker='o',s=0.1)
 61 | 	ax.set_xlim(-20.0,20.0)#ax.set_xlim(0.0,60.0)#
 62 | 	ax.set_ylim(0.0,60.0)#ax.set_ylim(-20.0,20.0)#
 63 | 	if draw_uncertainty:
 64 | 		infer_gt_uncertainty(label_data, points_cam, frame, idxs, ax)
 65 | 	centers = []
 66 | 	for idx in idxs:
 67 | 		box3D = label_data['box3D'][frame][idx]
 68 | 		ry = label_data['ry'][frame][idx]
 69 | 		#x_lidar, y_lidar, z_lidar = camera_to_lidar(box3D[0], box3D[1], box3D[2], calib_mat)
 70 | 		obj = edict({})
 71 | 		obj.R = ry
 72 | 		obj.width = box3D[4]
 73 | 		obj.length = box3D[5]
 74 | 		obj.x = box3D[0]#-y_lidar
 75 | 		obj.y = box3D[2]#x_lidar
 76 | 		centers.append([obj.x, obj.y])
 77 | 		draw_birdeye(obj, ax, fill=False, color='g', linewidth=1.0)
 78 | 		points_clip = clip_by_BEV_box(points_cam, box3D[0:3], box3D[3:6], ry, buffer_size = 0.1)
 79 | 		#x_lidar, y_lidar, z_lidar = camera_to_lidar(points_clip[:,0], points_clip[:,1], points_clip[:,2], calib_mat)
 80 | 		#points_lidar_clip = np.array([x_lidar, y_lidar, z_lidar]).transpose()
 81 | 		ax.scatter(points_clip[:,0],points_clip[:,2],c='r',marker='o',s=0.2)
 82 | 
 83 | 	if IoU_dic:
 84 | 		annotate_IoUs(ax, IoU_dic, centers)
 85 | 	if pd_idxs and pred_data:
 86 | 		for idx in pd_idxs:
 87 | 			if idx<0:
 88 | 				continue
 89 | 			box3D = pred_data['box3D'][frame][idx]
 90 | 			ry = pred_data['ry'][frame][idx]
 91 | 			# x_lidar, y_lidar, z_lidar = camera_to_lidar(box3D[0], box3D[1], box3D[2], calib_mat)
 92 | 			obj = edict({})
 93 | 			obj.R = ry
 94 | 			obj.width = box3D[4]
 95 | 			obj.length = box3D[5]
 96 | 			obj.x = box3D[0]  # -y_lidar
 97 | 			obj.y = box3D[2]  # x_lidar
 98 | 			centers.append([obj.x, obj.y])
 99 | 			draw_birdeye(obj, ax, fill=False, color='b', linewidth=1.0)
100 | 
101 | 	out_file_name += '.png'
102 | 	out_file_path = os.path.join(output_dir,out_file_name)
103 | 	plt.savefig(out_file_path)
104 | 	plt.close()
105 | 
106 | def write_stats_label(label_data, frame_idxs_pairs, output_dir,description='Unknown: ',reg_err_idxs=None):
107 | 	from time import gmtime, strftime
108 | 	time_str = strftime("%a, %d %b %Y %H:%M:%S", gmtime())
109 | 	difficulty_counts = [0,0,0,0]
110 | 	frame_counts = len(frame_idxs_pairs)
111 | 	idx_counts = 0
112 | 	for frame, idxs in frame_idxs_pairs:
113 | 		name = "{:06d}".format(frame)
114 | 		for idx in idxs:
115 | 			occ  = label_data['occlusion'][frame][idx]
116 | 			trun = label_data['truncation'][frame][idx]
117 | 			idx_counts+=1
118 | 			if trun<=0.15 and occ<=0:
119 | 				difficulty_counts[0]+=1
120 | 			elif trun<=0.3 and occ<=1:
121 | 				difficulty_counts[1]+=1
122 | 			elif trun<=0.5 and occ<=2:
123 | 				difficulty_counts[2]+=1
124 | 			else:
125 | 				difficulty_counts[3]+=1
126 | 	with open(os.path.join(output_dir,'statistics.txt'),'a') as fo:
127 | 		fo.write('\n')
128 | 		fo.write(description+time_str+'\n')
129 | 		fo.write('# frames: {}\n'.format(frame_counts))
130 | 		fo.write('# object: {}\n'.format(idx_counts))
131 | 		fo.write('# difficulties (E,M,H,Unknown): {}, {}, {}, {}\n'.format(difficulty_counts[0],difficulty_counts[1],difficulty_counts[2],difficulty_counts[3]))
132 | 		if reg_err_idxs:
133 | 			fo.write('# regression errors: {}\n'.format(len(reg_err_idxs)))
134 | 			for reg_idx in reg_err_idxs:
135 | 				fo.write('   regression errors(frame, idx, iou): {}, {}, {}\n'.format(reg_idx[0],reg_idx[1],reg_idx[2]))
136 | 
137 | 
138 | def plot_npoint_histogram_label(label_data, points_cam, frame_idxs_pairs, output_dir):
139 | 	key_names = ['ground_truth']
140 | 	npoints_hists = {ikey:[] for ikey in range(len(key_names))}
141 | 	for frame, idxs in tqdm(frame_idxs_pairs):
142 | 		for idx in idxs:
143 | 			box3D = label_data['box3D'][frame][idx]
144 | 			ry = label_data['ry'][frame][idx]
145 | 			points_clip = clip_by_BEV_box(points_cam, box3D[0:3], box3D[3:6], ry, buffer_size = 0.25)
146 | 			npoints_hists[0].append([frame,idx,min(100,points_clip.shape[0])])
147 | 	plot_histogram_network_wise(output_dir, 'num_points_histogram.png', key_names, npoints_hists, 2, x_label='number of points', y_label='number of false negatives', n_bins=50)
148 | 
149 | 
150 | def plot_histogram_network_wise(output_dir, file_name, networks, plot_data, icol, x_label='None', y_label='number', n_bins=10):
151 | 	os.makedirs(output_dir, exist_ok=True)
152 | 	plt.figure(figsize=(10,10))
153 | 	gs = gridspec.GridSpec(len(networks),len(networks))
154 | 	ax = plt.gca()
155 | 	for inet in range(len(networks)):
156 | 		iou_hist = np.array(plot_data[inet])
157 | 		ax = plt.subplot(gs[inet,:])
158 | 		ax.hist(iou_hist[:,icol], bins=n_bins)
159 | 		plt.xlabel(x_label)
160 | 		plt.ylabel(y_label)
161 | 	plt.savefig(os.path.join(output_dir,file_name))
162 | 
163 | 
164 | def infer_gt_uncertainty(label_data, points_cam, frame, idxs, ax=None):
165 | 	from metric_utils import label_inference_BEV, label_uncertainty_IoU
166 | 	inference = label_inference_BEV(degree_register=1)
167 | 	for idx in idxs:
168 | 		box3D = label_data['box3D'][frame][idx]
169 | 		ry = label_data['ry'][frame][idx]
170 | 		points_clip = clip_by_BEV_box(points_cam, box3D[0:3], box3D[3:6], ry, buffer_size = 0.1)
171 | 		points_clip_BEV = points_clip[:,[0,2]]
172 | 		uncertain_label = inference.infer(points_clip_BEV, [box3D[0],box3D[2],box3D[5],box3D[4],ry])
173 | 		centers_plot, covs_plot = uncertain_label.calc_uncertainty_contour()
174 | 		#DEBUG_eigvals = np.zeros((covs_plot.shape[0],2))
175 | 		for i in range(covs_plot.shape[0]):
176 | 			draw_birdeye_ellipse(ax, covs_plot[i,:,:].reshape((2,2)), centers_plot[i,:].reshape(2), nstd=1, alpha=0.1)#1.0/covs_plot.shape[0])
177 | 	return
178 | 
179 | def annotate_IoUs(ax, IoU_dic, centers, offset = [0,2.0], give_name=True):
180 | 	c = 0
181 | 	for key in IoU_dic:
182 | 		c += 1
183 | 		for i in range(len(centers)):
184 | 			x = centers[i][0] + c * offset[0]
185 | 			y = centers[i][1] + c * offset[1]
186 | 			if give_name:
187 | 				IoU_text = 'net%d: '% (c)
188 | 			else:
189 | 				IoU_text = ''
190 | 			for IoU in IoU_dic[key][i]:
191 | 				IoU_text += '%1.2f, ' % (IoU)
192 | 			ax.text(x,y,IoU_text, size=11)
193 | 
194 | def write_JIoU(output_dir, networks, frames, all_gt_idxs, all_pd_idxs, all_IoU_dic):
195 | 	os.makedirs(output_dir, exist_ok=True)
196 | 	for net in networks:
197 | 		with open(os.path.join(output_dir, net+'_IoU_summary.txt'), 'w') as fo1:
198 | 			with open(os.path.join(output_dir, net+'_interest.txt'), 'w') as fo2:
199 | 				fo1.write('frame, gt_idx, det_idx,  IoU, JIoU(gt_unc&det_unc), JIoU(gt_unc&gt), JIoU(gt_unc&det), JIoU ratio\n')
200 | 				fo2.write('frame, gt_idx, det_idx,  IoU, JIoU(gt_unc&det_unc), JIoU(gt_unc&gt), JIoU(gt_unc&det), JIoU ratio\n')
201 | 				for idfile, frame in enumerate(frames):
202 | 					gt_idxs = all_gt_idxs[idfile][net]
203 | 					pd_idxs = all_pd_idxs[idfile][net]
204 | 					IoU_dics = all_IoU_dic[idfile][net]
205 | 					for i, gt_idx in enumerate(gt_idxs):
206 | 						pd_idx = pd_idxs[i]
207 | 						IoU_dic = IoU_dics[i]
208 | 						fo1.write('{:06d}, {:5d}, {:6d}, {:5.2f}, {:6.2f}, {:8.2f}, {:8.2f}, {:8.2f}\n'.format(frame, gt_idx, pd_idx, IoU_dic[0], IoU_dic[1], IoU_dic[2], IoU_dic[3], IoU_dic[1]/IoU_dic[2]))
209 | 						if IoU_dic[2]<0.7:
210 | 							fo2.write('{:06d}, {:5d}, {:6d}, {:5.2f}, {:6.2f}, {:8.2f}, {:8.2f}, {:8.2f}\n'.format(frame, gt_idx, pd_idx, IoU_dic[0], IoU_dic[1], IoU_dic[2], IoU_dic[3], IoU_dic[1]/IoU_dic[2]))
211 | 
212 | 
213 | def rewrite_detection_aligned(output_dir, pred_data, label_data, hack_data):
214 | 	#rewrite the prediction to align it with its associated label (the gt object with highest IoU)
215 | 	#bounding box is aligned to the closest corner
216 | 	views = ['2D', 'G', '3D']
217 | 	difficulty = 'HARD'
218 | 	output_corner_dir = os.path.join(output_dir, 'corner_aligned', 'data')
219 | 	output_center_dir = os.path.join(output_dir, 'center_aligned', 'data')
220 | 	os.makedirs(output_corner_dir, exist_ok=True)
221 | 	os.makedirs(output_center_dir, exist_ok=True)
222 | 	if output_dir.find('waymo') != -1 or output_dir.find('Waymo') != -1:
223 | 		using_waymo = True
224 | 	else:
225 | 		using_waymo = False
226 | 	for frame in tqdm(range(len(pred_data['file_exists']))):
227 | 		if pred_data['file_exists'][frame]:
228 | 			if using_waymo:
229 | 				file1name = os.path.join(output_corner_dir, '{:08d}.txt'.format(frame))
230 | 				file2name = os.path.join(output_center_dir, '{:08d}.txt'.format(frame))
231 | 			else:
232 | 				file1name = os.path.join(output_corner_dir, '{:06d}.txt'.format(frame))
233 | 				file2name = os.path.join(output_center_dir, '{:06d}.txt'.format(frame))
234 | 			with open(file1name, 'w') as fo1, open(file2name, 'w') as fo2:
235 | 			#output a prediction file
236 | 				for det_idx in range(len(pred_data['box3D'][frame])):
237 | 					det_ry = pred_data['ry'][frame][det_idx]
238 | 					det_box3D = pred_data['box3D'][frame][det_idx]
239 | 					det_boxBEV = [det_box3D[0], det_box3D[2], det_box3D[5], det_box3D[4], det_ry]
240 | 					det_cornersBEV = center_to_corner_BEV(det_boxBEV)
241 | 					det_cornersBEV_dist = np.linalg.norm(det_cornersBEV, axis=1)
242 | 					det_box3D_corner_aligned = det_box3D.copy()
243 | 					det_box3D_center_aligned = det_box3D.copy()
244 | 					associated_gt = -1
245 | 					for view in views:
246 | 						if hack_data[difficulty]['det'][view]['gt'][frame][det_idx]>0:
247 | 							associated_gt = hack_data[difficulty]['det'][view]['gt'][frame][det_idx]
248 | 					if associated_gt > -1: #is associated with a ground truth
249 | 						#align the bounding box
250 | 						gt_idx = associated_gt
251 | 						gt_ry = label_data['ry'][frame][gt_idx]
252 | 						gt_box3D = label_data['box3D'][frame][gt_idx]
253 | 						gt_boxBEV = [gt_box3D[0], gt_box3D[2], gt_box3D[5], gt_box3D[4], gt_ry]
254 | 						gt_cornersBEV = center_to_corner_BEV(gt_boxBEV)
255 | 						#get the nearest corner
256 | 						det_corner_idx = np.argmin(det_cornersBEV_dist)
257 | 						#create corner aligned and center aligned boxes
258 | 						det_boxBEV_corner_aligned = align_BEV_to_corner(det_boxBEV, det_corner_idx, gt_boxBEV[2:4])
259 | 						det_box3D_corner_aligned[0] = det_boxBEV_corner_aligned[0]
260 | 						det_box3D_corner_aligned[2] = det_boxBEV_corner_aligned[1]
261 | 						det_box3D_corner_aligned[5] = det_boxBEV_corner_aligned[2]
262 | 						det_box3D_corner_aligned[4] = det_boxBEV_corner_aligned[3]
263 | 						det_box3D_center_aligned[5] = gt_box3D[5]
264 | 						det_box3D_center_aligned[4] = gt_box3D[4]
265 | 					#write the new prediction file
266 | 					det_box2D = pred_data['box2D'][frame][det_idx]
267 | 					det_score = pred_data['score'][frame][det_idx]
268 | 					if not using_waymo:
269 | 						y_center_aligned = det_box3D_center_aligned[1]
270 | 						z_center_aligned = det_box3D_center_aligned[2]
271 | 						y_corner_aligned = det_box3D_corner_aligned[1]
272 | 						z_corner_aligned = det_box3D_corner_aligned[2]
273 | 						ry = pred_data['ry'][frame][det_idx]
274 | 					else:
275 | 						# data converter for waymo, since it uses x,y,z center in LIDAR frame
276 | 						y_center_aligned = det_box3D_center_aligned[2]
277 | 						z_center_aligned = det_box3D_center_aligned[1]-det_box3D_center_aligned[3]/2
278 | 						y_corner_aligned = det_box3D_corner_aligned[2]
279 | 						z_corner_aligned = det_box3D_corner_aligned[1]-det_box3D_corner_aligned[3] / 2
280 | 						ry = -pred_data['ry'][frame][det_idx]
281 | 					fo1.write('{:s} {:d} {:d} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.3f} {:.2f} {:.3f} {:.2f} {:.4f}\n'.format(pred_data['class'][frame][det_idx],
282 | 													-1, -1, pred_data['alpha'][frame][det_idx], det_box2D[0], det_box2D[1], det_box2D[2], det_box2D[3], 
283 | 													det_box3D_corner_aligned[3], det_box3D_corner_aligned[4], det_box3D_corner_aligned[5], 
284 | 													det_box3D_corner_aligned[0], y_corner_aligned, z_corner_aligned, ry, det_score
285 | 													))
286 | 					fo2.write('{:s} {:d} {:d} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.3f} {:.2f} {:.3f} {:.2f} {:.4f}\n'.format(pred_data['class'][frame][det_idx],
287 | 													-1, -1, pred_data['alpha'][frame][det_idx], det_box2D[0], det_box2D[1], det_box2D[2], det_box2D[3], 
288 | 													det_box3D_center_aligned[3], det_box3D_center_aligned[4], det_box3D_center_aligned[5], 
289 | 													det_box3D_center_aligned[0], y_center_aligned, z_center_aligned, ry, det_score
290 | 													))
291 | 
292 | 
293 | 
294 | def get_corner_variances(label_data, frame, gt_idxs, points_cam):
295 | 	from metric_utils import label_inference_BEV, label_uncertainty_IoU, uncertain_prediction_BEVdelta, uncertain_prediction_BEV
296 | 	inference = label_inference_BEV(degree_register=1,gen_std=0.3, prob_outlier=0.03)
297 | 	uncertain_labels = []
298 | 	corner_totalVariances = []
299 | 	center_totalVariances = []
300 | 	LIDAR_variances = []
301 | 	for i, gt_idx in enumerate(gt_idxs):
302 | 		box3D = label_data['box3D'][frame][gt_idx]
303 | 		ry = label_data['ry'][frame][gt_idx]
304 | 		gt_boxBEV = [box3D[0],box3D[2],box3D[5],box3D[4],ry]
305 | 		points_clip = clip_by_BEV_box(points_cam, box3D[0:3], box3D[3:6], ry, buffer_size = 0.1)
306 | 		points_clip_BEV = points_clip[:,[0,2]]
307 | 		uncertain_labels.append(inference.infer(points_clip_BEV, [box3D[0],box3D[2],box3D[5],box3D[4],ry]))
308 | 		means, covs = uncertain_labels[i].calc_uncertainty_corners()
309 | 		#cov is 4x2x2, totalVariance is 4x1
310 | 		totalVariance = np.trace(covs,axis1=1,axis2=2)
311 | 		gt_cornersBEV = center_to_corner_BEV(gt_boxBEV)
312 | 		gt_cornersBEV_dist = np.linalg.norm(gt_cornersBEV, axis=1)
313 | 		sort_idx = np.argsort(gt_cornersBEV_dist)
314 | 		mean_center,cov_center = uncertain_labels[i].calc_uncertainty_points(np.array([[0,0]]))
315 | 		totalVariance_center = np.trace(cov_center,axis1=1,axis2=2)
316 | 		corner_totalVariances.append(totalVariance[sort_idx])
317 | 		center_totalVariances.append(totalVariance_center)
318 | 		if uncertain_labels[i].mean_LIDAR_variance:
319 | 			LIDAR_variances.append([uncertain_labels[i].mean_LIDAR_variance])
320 | 		else:
321 | 			LIDAR_variances.append([-1])
322 | 	#return a N*5 totalVariance matrix of each corner + a center, it is sorted according to the distance to origin (ascending)
323 | 	return np.concatenate((np.stack(corner_totalVariances,axis=0), np.stack(center_totalVariances,axis=0), np.stack(LIDAR_variances,axis=0)), axis=1)
324 | 
325 | def write_corner_variances(output_root, corner_totalVariances):
326 | 	#corner_totalVariances is N*6, which is [corner1, corner2, corner3, corner4, center, LIDAR_measurement_noise_variance(LIDAR_variance)]
327 | 	output_file = output_root+'Lshapes/uncertaintyV2/Waymo_0.3_0.03_corner_totalVariances_summary.txt'
328 | 	output_file2 = output_root + 'Lshapes/uncertaintyV2/Waymo_0.3_0.03_corner_totalVariances.txt'
329 | 	cTVs = np.vstack(corner_totalVariances)
330 | 	mean_cTV = np.mean(cTVs[:,0:5], axis=0)
331 | 	std_cTV = np.std(cTVs[:,0:5], axis=0)
332 | 	with open(output_file, 'w') as fo:
333 | 		fo.write('mean {:.2f} {:.2f} {:.2f} {:.2f}\n'.format(mean_cTV[0], mean_cTV[1], mean_cTV[2], mean_cTV[3], mean_cTV[4]))
334 | 		fo.write('std  {:.2f} {:.2f} {:.2f} {:.2f}'.format(std_cTV[0], std_cTV[1], std_cTV[2], std_cTV[3], std_cTV[4]))
335 | 	with open(output_file2, 'w') as fo:
336 | 		fo.write('corner1 corner2 corner3 corner4 center LIDAR\n')
337 | 		for i in range(cTVs.shape[0]):
338 | 			fo.write('{:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.5f}\n'.format(cTVs[i,0],cTVs[i,1],cTVs[i,2],cTVs[i,3],cTVs[i,4],cTVs[i,5]))
339 | 
340 | 
341 | def calc_mAPs(pred_data, label_data, metric, metric_thres=np.arange(0,1.01,0.01)):
342 | 	mAPs = np.zeros_like(metric_thres)
343 | 	


--------------------------------------------------------------------------------
/utils/metric_utils.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import math
  3 | from scipy.stats import multivariate_normal
  4 | from utils.paul_geometry import z_normed_corners_default, center_to_corner_BEV
  5 | from utils.probability_utils import cov_interp_BEV_full
  6 | 
  7 | ##############################################################
  8 | ### Utilities
  9 | ##############################################################
 10 | def create_BEV_box_sample_grid(sample_grid):
 11 | 	x = np.arange(-0.5 + sample_grid / 2, 0.5, sample_grid)
 12 | 	y = x
 13 | 	zx, zy = np.meshgrid(x, y)
 14 | 	z_normed = np.concatenate((zx.reshape((-1, 1)), zy.reshape((-1, 1))), axis=1)
 15 | 	return z_normed
 16 | ##############################################################
 17 | ### Inference of Label Uncertainty class ###
 18 | ##############################################################
 19 | 
 20 | class label_inference:
 21 | 	def __int__(self):
 22 | 		# x is label, y is observation
 23 | 		self.description = 'a template of label inference'
 24 | 
 25 | 	def generator(self, x, y):
 26 | 		# return p(y|x)
 27 | 		prob_y_x = 0
 28 | 		return prob_y_x
 29 | 
 30 | 	def register(self, y, z):
 31 | 		# return a matrix of association between y and z, or p(z|y)
 32 | 		prob_z_y = 0
 33 | 		return prob_z_y
 34 | 
 35 | 	def infer(self, prob_y_x, prob_z_y):
 36 | 		# return the reference
 37 | 		prob_x_y = 0
 38 | 		return prob_x_y
 39 | 
 40 | 
 41 | class label_inference_BEV(label_inference):
 42 | 	def __init__(self, degree_register=0, gen_std=0.2, prob_outlier=0.1, eps_prior_scaler=0.04, boundary_sample_interval=0.1):
 43 | 		#eps_prior is to prevent deprecated posterior variance. It the posterior covariance is not full rank, add some epsilon of prior
 44 | 		self.description = 'use Gaussian model to get posterior covariance of labels'
 45 | 		# l0=3.89+-0.44, w0=1.63+-0.10, h0=1.52+-0.13
 46 | 		# self.dim_mean0 = [3.89,1.63]
 47 | 		self.generator_std = gen_std  # GMM std, in meter
 48 | 		self.boundary_sample_interval = boundary_sample_interval
 49 | 		# the prob_outlier should be normalized by the number of registration
 50 | 		self.prob_outlier = 2*np.pi*self.generator_std**2*prob_outlier/(1-prob_outlier)*(2*degree_register+1)#prob_outlier * (2*degree_register+1) / 3
 51 | 		self.eps_prior_scaler = eps_prior_scaler
 52 | 		# use Variantional Bayes to allow analytical result with multiple register
 53 | 		self.degree_register = degree_register
 54 | 		self.z_coords = [z_coord * self.boundary_sample_interval for z_coord in
 55 | 						 range(-self.degree_register, self.degree_register + 1)]
 56 | 
 57 | 		self.num_z_y = 1 + 2 * degree_register  # maximum number of registered z given y
 58 | 		self.feature_dim = 6
 59 | 		self.space_dim = 2 
 60 | 		self.surface_dim = self.space_dim-1
 61 | 
 62 | 	def generator(self, z_surface, y):
 63 | 		# modeled as Gaussian, only store the first and second order term (discard constant term)
 64 | 		ny = y.shape[0]
 65 | 		K = z_surface.shape[2]
 66 | 		prob_y_x_quad = np.zeros((ny, K, self.feature_dim, self.feature_dim))
 67 | 		prob_y_x_lin = np.zeros((ny, K, 1, self.feature_dim))
 68 | 		var_generator = self.generator_std ** 2
 69 | 		for ik in range(K):
 70 | 			Jacobians_point = self.label.Jacobian_surface(z_surface[:, :, ik])
 71 | 			# norm(y-Jacobian*x_feature, 2)/self.generator_std^2
 72 | 			for iy in range(ny):
 73 | 				Jacobian_point = np.squeeze(Jacobians_point[iy, :, :])
 74 | 				prob_y_x_quad[iy, ik, :, :] = np.matmul(Jacobian_point.transpose(), Jacobian_point) / var_generator
 75 | 				prob_y_x_lin[iy, ik, :, :] = -2 * np.matmul(y[iy, :], Jacobian_point) / var_generator
 76 | 
 77 | 		return (prob_y_x_quad, prob_y_x_lin)
 78 | 
 79 | 	def register(self, y, z=None):
 80 | 		# y: ny*2
 81 | 		ny = y.shape[0]
 82 | 		nz = self.num_z_y
 83 | 		prob_z_y = np.zeros((ny, nz))
 84 | 		yz_distances = np.zeros((ny,nz))
 85 | 		z_surface_out = np.zeros((ny,self.surface_dim, nz))
 86 | 
 87 | 		yc = np.matmul(y - self.label.x0[0:self.space_dim], self.label.rotmat)
 88 | 		zc = yc / self.label.x0[self.space_dim:2*self.space_dim]  # transposed twice
 89 | 		zc_norm = np.abs(zc)
 90 | 		z_embed = zc / zc_norm.max(1, keepdims=True) * self.label.x0[self.space_dim:2*self.space_dim] / 2  # z in 3D space, map to surface
 91 | 		z_surface = self.label.project(self.label.x0, z_embed)  # z in surface_manifold
 92 | 
 93 | 		var_generator = self.generator_std ** 2
 94 | 		for iz, z_coord in enumerate(self.z_coords):
 95 | 			z_surface_out[:, :, iz] = self.label.add_surface_coord(z_surface, z_coord).reshape((ny,self.surface_dim))
 96 | 			z_embeded = self.label.embedding(self.label.x0, z_surface_out[:,:,iz])
 97 | 			yz_distances[:, iz] = np.linalg.norm(yc - z_embed, 2, axis=1)
 98 | 			prob_z_y[:, iz] = np.exp(-yz_distances[:, iz]**2 / 2 / var_generator)
 99 | 
100 | 		prob_inlier = np.sum(prob_z_y, axis=1)
101 | 		for iz in range(len(self.z_coords)):
102 | 			prob_z_y[:, iz] = prob_z_y[:, iz] / (prob_inlier + self.prob_outlier)
103 | 		return prob_z_y, z_surface_out, yz_distances
104 | 
105 | 	def infer(self, y, boxBEV, prior_scaler=0):
106 | 		# only one z possible z for each y, so prob_z_y no need
107 | 		self.label = self.construct_uncertain_label(boxBEV) 
108 | 		if y.shape[0] < self.space_dim+1:
109 | 			#too few observations, directly use prior
110 | 			self.label.set_posterior(self.label.x0, self.return_prior_precision_matrix())
111 | 			return self.label
112 | 		else:
113 | 			prob_z_y, z_surface, yz_distances = self.register(y)
114 | 			prob_y_x_quad, prob_y_x_lin = self.generator(z_surface, y)
115 | 			ny = prob_y_x_quad.shape[0]
116 | 			# get Gaussian of feature vector
117 | 			Q = np.squeeze(np.tensordot(prob_z_y, prob_y_x_quad, axes=((0, 1), (0, 1))))  # self.feature_dim*self.feature_dim
118 | 			P = np.tensordot(prob_z_y, prob_y_x_lin, axes=((0, 1), (0, 1))).reshape((self.feature_dim, 1))
119 | 			Q += prior_scaler*self.label.Q0_feature #scale the prior of variance, default is 0, which is no prior
120 | 			if np.linalg.matrix_rank(Q,tol=2.5e-3) < Q.shape[0]:
121 | 				Q += self.eps_prior_scaler * self.label.Q0_feature
122 | 			self.label.set_posterior(np.linalg.solve(-2 * Q, P), np.linalg.inv(Q))
123 | 			#estimate the LIDAR measurement noise
124 | 			mean_variance = np.sum(np.sum(prob_z_y*yz_distances**2, axis=1), axis=0)/np.sum(np.sum(prob_z_y, axis=1),axis=0)/2
125 | 			self.label.mean_LIDAR_variance = mean_variance
126 | 			return self.label
127 | 
128 | 	def return_prior_precision_matrix(self):
129 | 		return np.linalg.inv(self.eps_prior_scaler*self.label.Q0_X)
130 | 	def construct_uncertain_label(self, boxBEV):
131 | 		return uncertain_label_BEV(boxBEV)
132 | 
133 | class label_inference_3D(label_inference_BEV):
134 | 	def __init__(self, degree_register=0, gen_std=0.2, prob_outlier=0.1, eps_prior_scaler=0.04, boundary_sample_interval=0.1):
135 | 		label_inference_BEV.__init__(self, degree_register, gen_std, prob_outlier, eps_prior_scaler, boundary_sample_interval)
136 | 		self.z_coords = []
137 | 		for zBEV_coord in range(-self.degree_register, self.degree_register + 1):
138 | 			self.z_coords.append(np.array([zBEV_coord*self.boundary_sample_interval, 0]))
139 | 		for zH_coord in range(-self.degree_register, 0):
140 | 			self.z_coords.append(np.array([0, zH_coord*self.boundary_sample_interval]))
141 | 		for zH_coord in range(1, self.degree_register + 1):
142 | 			self.z_coords.append(np.array([0, zH_coord*self.boundary_sample_interval]))
143 | 
144 | 
145 | 		self.num_z_y = 1 + 4 * degree_register  # neighborhood points in 2D surface (axis aligned)
146 | 		self.feature_dim = 8 #BEV + y center + height
147 | 		self.space_dim = 3
148 | 		self.surface_dim = self.space_dim-1
149 | 		
150 | 	def return_prior_precision_matrix(self):
151 | 		return np.linalg.inv(self.eps_prior_scaler*self.label.Q0_feature)
152 | 	def construct_uncertain_label(self, box3D):
153 | 		return uncertain_label_3D(box3D)
154 | 
155 | 
156 | 
157 | ##############################################################
158 | ### JIoU Calculation class ###
159 | ##############################################################
160 | 
161 | class label_uncertainty_IoU:
162 | 	def __init__(self, grid_size=0.1, range=3.0):
163 | 		self.grid_size = grid_size
164 | 		self.range = range  # sample upto how many times of the size of the car
165 | 
166 | 	def calc_IoU(self, uncertain_label, pred_boxBEVs, sample_grid=0.1):
167 | 		sample_points, _ = self.get_sample_points(uncertain_label)
168 | 		px = uncertain_label.sample_prob(sample_points, sample_grid=sample_grid)
169 | 		JIoUs = []
170 | 		for i in range(len(pred_boxBEVs)):
171 | 			py = pred_boxBEVs[i].sample_prob(sample_points, sample_grid=sample_grid)
172 | 			if np.sum(py) > 0 and np.sum(px) > 0:
173 | 				JIoUs.append(self.Jaccard_discrete(px, py).item())
174 | 			else:
175 | 				JIoUs.append(0)
176 | 		return JIoUs
177 | 
178 | 	def get_sample_points(self, uncertain_label):
179 | 		x = np.arange(-self.range / 2 + self.grid_size / 2, self.range / 2, self.grid_size)
180 | 		y = x
181 | 		zx, zy = np.meshgrid(x, y)
182 | 		z_normed = np.concatenate((zx.reshape((-1, 1)), zy.reshape((-1, 1))), axis=1)
183 | 		# samples are aligned with label bounding box
184 | 		sample_points = np.matmul(z_normed * uncertain_label.x0[2:4],
185 | 								  uncertain_label.rotmat.transpose()) + uncertain_label.x0[0:2]
186 | 		return sample_points, np.array((x.shape[0], y.shape[0]))
187 | 	
188 | 	def Jaccard_discrete(self, px, py):
189 | 		# Yiyang's Implementation
190 | 		similarity = np.array(0)
191 | 		sort_index = np.argsort(px / (py+np.finfo(np.float64).eps))
192 | 		px = px[sort_index]
193 | 		py = py[sort_index]
194 | 
195 | 		px_sorted_sum = np.zeros(px.size)
196 | 		py_sorted_sum = np.zeros(py.size)
197 | 
198 | 		py_sorted_sum[0] = py[0]
199 | 
200 | 		for i in range(1, px.size):
201 | 			px_sorted_sum[px.size - i - 1] = px_sorted_sum[px.size - i] + px[px.size - i]
202 | 			py_sorted_sum[i] = py_sorted_sum[i - 1] + py[i]
203 | 
204 | 		idx = np.argwhere((px > 0) & (py > 0))
205 | 		for i in idx:
206 | 			x_y_i = px[i] / py[i]
207 | 			temp = px[i] / (px_sorted_sum[i] + x_y_i * py_sorted_sum[i])
208 | 			similarity = similarity + temp
209 | 
210 | 		return similarity
211 | 
212 | 
213 | ##############################################################
214 | ### Uncertain Label class ###
215 | ##############################################################
216 | 
217 | class uncertain_label:
218 | 	def __init__(self, box, prior_std):
219 | 		self.description = "a template of uncertain label"
220 | 		self.posterior = None
221 | 
222 | 	def embedding(self, z_surface):
223 | 		# embed surface points from surface coord to Euclidean coord
224 | 		z_embed = []
225 | 		return z_embed
226 | 
227 | 	def project(self, z_embed):
228 | 		# project surface points from Euclidean coord to surface coord
229 | 		z_surface = []
230 | 		return z_surface
231 | 
232 | 	def sample_prob(self, points):
233 | 		# sample the spatial distribution at points
234 | 		prob = []
235 | 		return prob
236 | 
237 | class uncertain_label_BEV(uncertain_label):
238 | 	def __init__(self, boxBEV, x_std=[0.44, 0.10, 0.25, 0.25, 0.1745]):
239 | 		self.x0 = np.array(boxBEV).reshape(5)
240 | 		# reverse ry so that the rotation is x->z instead of z->x normally
241 | 		self.x0[4] *= -1
242 | 		ry = self.x0[4]
243 | 		self.rotmat = np.array([[math.cos(ry), -math.sin(ry)], [math.sin(ry), math.cos(ry)]])
244 | 		self.feature0 = np.array(
245 | 			[self.x0[0], self.x0[1], self.x0[2] * math.cos(ry), self.x0[2] * math.sin(ry), self.x0[3] * math.cos(ry),
246 | 			 self.x0[3] * math.sin(ry)])
247 | 		# initialize prior uncertainty
248 | 		self.dim_std = [x_std[0], x_std[1]]  # l,w
249 | 		self.pos_std = [x_std[2], x_std[3]]  # assume to be the same as dimension?
250 | 		self.ry_std = [x_std[4]]  # rotation, about 10deg.
251 | 		self.Q0_feature = np.diag(1 / np.array([self.pos_std[0], self.pos_std[1],
252 | 												self.dim_std[0] * abs(math.cos(ry)) + self.x0[2] * self.ry_std[0] * abs(math.sin(ry)),
253 | 												self.dim_std[0] * abs(math.sin(ry)) + self.x0[2] * self.ry_std[0] * abs(math.cos(ry)),
254 | 												self.dim_std[1] * abs(math.cos(ry)) + self.x0[3] * self.ry_std[0] * abs(math.sin(ry)),
255 | 												self.dim_std[1] * abs(math.sin(ry)) + self.x0[3] * self.ry_std[0] * abs(math.cos(ry))]) ** 2)
256 | 		self.Q0_X = np.diag(
257 | 			1 / np.array([self.pos_std[0], self.pos_std[1], self.dim_std[0], self.dim_std[1], self.ry_std[0]]) ** 2)
258 | 		self.posterior = None
259 | 		self.mean_LIDAR_variance = None
260 | 
261 | 	def set_posterior(self, mean, cov):
262 | 		# assert (cov.shape[0]==6),  "must set posterior of feature vector!"
263 | 		self.posterior = (mean, cov)
264 | 
265 | 	def get_corner_norms(self):
266 | 		corner_norms = z_normed_corners_default
267 | 		return corner_norms
268 | 
269 | 	def embedding(self, x0, z_surface):
270 | 		#x0 = [x,z,l,w,ry]
271 | 		# surface is 1D in BEV, from 0 to 4 ([-l,-w]/2->[l,-w]/2->[l,w]/2->[-l,w]/2), l along 1st axis
272 | 		assert(x0.shape[0]==5)
273 | 		z_surface[z_surface > 4] -= 4
274 | 		z_surface[z_surface < 0] += 4
275 | 		z_surface.reshape(-1)
276 | 		z_embed = np.zeros((z_surface.shape[0], 2))
277 | 		temp_idx = np.squeeze(z_surface >= 3)
278 | 		z_embed[temp_idx, 1] = ((-(z_surface[temp_idx] - 3) + 0.5) * x0[3]).reshape(-1)
279 | 		z_embed[temp_idx, 0] = -x0[2] / 2
280 | 		temp_idx = np.squeeze(np.logical_and(z_surface >= 2, z_surface < 3))
281 | 		z_embed[temp_idx, 1] = x0[3] / 2
282 | 		z_embed[temp_idx, 0] = ((-(z_surface[temp_idx] - 2) + 0.5) * x0[2]).reshape(-1)
283 | 		temp_idx = np.squeeze(np.logical_and(z_surface >= 1, z_surface < 2))
284 | 		z_embed[temp_idx, 1] = (((z_surface[temp_idx] - 1) - 0.5) * x0[3]).reshape(-1)
285 | 		z_embed[temp_idx, 0] = x0[2] / 2
286 | 		temp_idx = np.squeeze(np.logical_and(z_surface >= 0, z_surface < 1))
287 | 		z_embed[temp_idx, 1] = -x0[3] / 2
288 | 		z_embed[temp_idx, 0] = ((z_surface[temp_idx] - 0.5) * x0[2]).reshape(-1)
289 | 		return z_embed
290 | 
291 | 	def project(self, x0, z_embed):
292 | 		#x0 = [x,z,l,w,ry]
293 | 		assert(x0.shape[0]==5)
294 | 		z_normed = z_embed / x0[2:4]
295 | 		amax = np.argmax(np.abs(z_normed), axis=1)
296 | 		z_surface = np.zeros((z_normed.shape[0]))
297 | 
298 | 		temp_idx = np.squeeze(np.logical_and(amax == 1, z_normed[:, 1] < 0))
299 | 		z_surface[temp_idx] = z_normed[temp_idx, 0] + 0.5
300 | 		temp_idx = np.squeeze(np.logical_and(amax == 0, z_normed[:, 0] >= 0))
301 | 		z_surface[temp_idx] = z_normed[temp_idx, 1] + 0.5 + 1
302 | 		temp_idx = np.squeeze(np.logical_and(amax == 1, z_normed[:, 1] >= 0))
303 | 		z_surface[temp_idx] = -z_normed[temp_idx, 0] + 0.5 + 2
304 | 		temp_idx = np.squeeze(np.logical_and(amax == 0, z_normed[:, 0] < 0))
305 | 		z_surface[temp_idx] = -z_normed[temp_idx, 1] + 0.5 + 3
306 | 		return z_surface
307 | 
308 | 	def add_surface_coord(self, z_surface, z_coord):
309 | 		return z_surface+z_coord
310 | 
311 | 
312 | 	def Jacobian_surface(self, z_surface):
313 | 		# parameterize surface point w.r.t. BEV box feature vector
314 | 		# the feature vector is (xc1, xc2, cos(ry)*l, sin(ry)*l, cos(ry)*w, sin(ry)*w), 1*6
315 | 		z_normed = self.embedding(self.x0, z_surface) / self.x0[2:4]
316 | 		Jacobian = self.Jacobian_z_normed(z_normed)
317 | 		return Jacobian
318 | 
319 | 	def Jacobian_z_normed(self, z_normed):
320 | 		# parameterize surface point w.r.t. BEV box feature vector
321 | 		# the feature vector is (xc1, xc2, cos(ry)*l, sin(ry)*l, cos(ry)*w, sin(ry)*w), 1*6
322 | 		nz = z_normed.shape[0]
323 | 		Jacobian = np.zeros((nz, 2, 6))
324 | 		Jacobian[:, 0, 0] = 1.0
325 | 		Jacobian[:, 1, 1] = 1.0
326 | 		Jacobian[:, 0, 2] = z_normed[:, 0]
327 | 		Jacobian[:, 1, 3] = z_normed[:, 0]
328 | 		Jacobian[:, 1, 4] = z_normed[:, 1]
329 | 		Jacobian[:, 0, 5] = -z_normed[:, 1]
330 | 		return Jacobian
331 | 
332 | 	def sample_boundary_points(self):
333 | 		z_surface = np.arange(0, 4, 0.05)
334 | 		z_embed = self.embedding(self.x0, z_surface)
335 | 		z_normed = z_embed / self.x0[2:4]
336 | 		return z_surface, z_embed, z_normed
337 | 
338 | 	def calc_uncertainty_box(self, std=1):
339 | 		#by default, calculate the contour of 1 standard deviation by variance along the boundary of bounding box
340 | 		z_normed = np.array([[0.5,0],[0,0.5],[-0.5,0],[0,-0.5]])
341 | 		centers, covs = self.calc_uncertainty_points(z_normed)
342 | 		#get variance along the direction to center
343 | 		direction = centers - self.x0[0:2]
344 | 		lengths = np.linalg.norm(direction, axis=1, keepdims=True)
345 | 		direction = direction / lengths
346 | 		#get a n*1 distance array
347 | 		dists2 = np.sum(np.sum(covs*np.expand_dims(direction,axis=2),axis=1)*direction, axis=1).reshape([-1,1])
348 | 		dists = np.sqrt(dists2)
349 | 		ratio = 1+dists*std/lengths
350 | 		outer_corners = self.x0[0:2] + direction[[0,0,2,2],:]*lengths[[0,0,2,2]]*ratio[[0,0,2,2]] + direction[[3,1,1,3],:]*lengths[[3,1,1,3]]*ratio[[3,1,1,3]]
351 | 		inner_corners = self.x0[0:2] + direction[[0,0,2,2],:]*lengths[[0,0,2,2]]/ratio[[0,0,2,2]] + direction[[3,1,1,3],:]*lengths[[3,1,1,3]]/ratio[[3,1,1,3]]
352 | 		return inner_corners, outer_corners
353 | 
354 | 	def calc_uncertainty_contour_boundary(self, std=1):
355 | 		#by default, calculate the contour of 1 standard deviation by variance along the boundary of bounding box
356 | 		_, _, z_normed = self.sample_boundary_points()
357 | 		centers, covs = self.calc_uncertainty_points(z_normed)
358 | 		#get variance along the direction to center
359 | 		direction = centers - self.x0[0:2]
360 | 		lengths = np.linalg.norm(direction, axis=1, keepdims=True)
361 | 		direction = direction / lengths
362 | 		#get a n*1 distance array
363 | 		dists2 = np.sum(np.sum(covs*np.expand_dims(direction,axis=2),axis=1)*direction, axis=1).reshape([-1,1])
364 | 		dists = np.sqrt(dists2)
365 | 		ratio = 1+dists*std/lengths
366 | 		outer_contour_boundary = self.x0[0:2] + direction*ratio*lengths
367 | 		inner_contour_boundary = self.x0[0:2] - direction/ratio*lengths
368 | 		return inner_contour_boundary, outer_contour_boundary 
369 | 
370 | 	def calc_uncertainty_contour(self):
371 | 		# cov is for self.label.x0
372 | 		_, _, z_normed = self.sample_boundary_points()
373 | 		# cov at the surface point, than return the covariance of these points
374 | 
375 | 		return self.calc_uncertainty_points(z_normed)
376 | 
377 | 	def calc_uncertainty_corners(self):
378 | 		return self.calc_uncertainty_points(self.get_corner_norms())
379 | 
380 | 	def calc_uncertainty_points(self, z_normed):
381 | 		nz = z_normed.shape[0]
382 | 		cov = self.posterior[1]
383 | 		Jacobians_point = self.Jacobian_z_normed(z_normed)
384 | 		covs_out = np.zeros((z_normed.shape[0], 2, 2))
385 | 		centers_out = np.matmul(z_normed * self.x0[2:4], self.rotmat.transpose()) + self.x0[0:2]
386 | 		if cov.shape[0] == 6:
387 | 			for i in range(nz):
388 | 				Jacobian_point = np.squeeze(Jacobians_point[i, :, :])
389 | 				covs_out[i, :, :] = (Jacobian_point @ cov) @ Jacobian_point.transpose()
390 | 		elif cov.shape[0] == 5:
391 | 			lin_trans = np.zeros((2, 5))
392 | 			lin_trans[0, 0] = 1
393 | 			lin_trans[1, 1] = 1
394 | 			for i in range(nz):
395 | 				lin_trans[0, 2] = z_normed[i, 0]
396 | 				lin_trans[1, 3] = z_normed[i, 1]
397 | 				covs_out[i, :, :] = (lin_trans @ cov) @ lin_trans.transpose()
398 | 		return centers_out, covs_out
399 | 
400 | 	def sample_prob(self, points, sample_grid=0.1):
401 | 		# sample probablity given points
402 | 		nk = points.shape[0]
403 | 		cov = self.posterior[1]
404 | 		probs = np.zeros(nk)
405 | 		if np.max(np.sqrt(np.linalg.eig(cov)[0]))<sample_grid*5:
406 | 			#empirical coeffienct, if cov too low, then sample_grid is not thick enough. Just assume the distribution is uniform (delta uncertainty)
407 | 			l2 = self.x0[2] / 2
408 | 			w2 = self.x0[3] / 2
409 | 			points_aligned_to_pred = np.matmul(points - self.x0[0:2], self.rotmat)
410 | 			clip_idx = points_aligned_to_pred[:, 0] >= -l2
411 | 			clip_idx = np.logical_and(clip_idx, points_aligned_to_pred[:, 0] < l2)
412 | 			clip_idx = np.logical_and(clip_idx, points_aligned_to_pred[:, 1] >= -w2)
413 | 			clip_idx = np.logical_and(clip_idx, points_aligned_to_pred[:, 1] < w2)
414 | 			# calculate JIoU with discrete sample
415 | 			n_in = np.sum(clip_idx)
416 | 			if n_in > 0:
417 | 				probs[clip_idx] = 1 / n_in
418 | 		else:
419 | 			#use the definition we proposed for spatial distribution
420 | 			z_normed = create_BEV_box_sample_grid(sample_grid)
421 | 			centers_out, covs_out = self.calc_uncertainty_points(z_normed)
422 | 			for i in range(covs_out.shape[0]):
423 | 				tmp_probs = multivariate_normal.pdf(points, mean=centers_out[i, :], cov=covs_out[i, :, :])
424 | 				# exponents = np.sum((points-centers_out[i,:])*((points-centers_out[i,:])@ np.linalg.inv(covs_out[i,:,:])),axis=1)/-2
425 | 				# tmp_probs = np.exp(exponents)
426 | 				probs += tmp_probs
427 | 			probs /= np.sum(probs)
428 | 		return probs
429 | 
430 | class uncertain_label_3D(uncertain_label):
431 | 	# A minimum unccertain_label_3D. TODO: implement sample_prob member
432 | 	def __init__(self, box3D, x_std=[0.44, 0.10, 0.10, 0.25, 0.25, 0.25, 0.1745]):
433 | 		# input box3D is xc,y_top,zc,h,w,l,ry in KITTI format
434 | 		# x0 is xc,yc,zc,l,h,w 
435 | 		self.x0 = np.array(box3D).reshape(7)
436 | 		self.x0[3:6] = self.x0[[5,3,4]]
437 | 		self.x0[1] = self.x0[1]-self.x0[4]/2
438 | 		ry = self.x0[6]
439 | 		self.rotmat = np.array([[math.cos(ry), 0, -math.sin(ry)], [0,1,0], [math.sin(ry), 0, math.cos(ry)]])
440 | 		self.feature0 = np.array(
441 | 			[self.x0[0], self.x0[1], self.x0[2], self.x0[3] * math.cos(ry), self.x0[3] * math.sin(ry), self.x0[5] * math.cos(ry),
442 | 			 self.x0[5] * math.sin(ry), self.x0[4]])
443 | 		# initialize prior uncertainty
444 | 		self.dim_std = [x_std[0], x_std[1], x_std[2]]  # l,h,w
445 | 		self.pos_std = [x_std[3], x_std[4], x_std[5]]  # assume to be the same as dimension?
446 | 		self.ry_std = [x_std[4]]  # rotation, about 10deg.
447 | 		self.Q0_feature = np.diag(1 / np.array([self.pos_std[0], self.pos_std[1], self.pos_std[2],
448 | 												self.dim_std[0] * abs(math.cos(ry)) + self.x0[3] * self.ry_std[0] * abs(math.sin(ry)),
449 | 												self.dim_std[0] * abs(math.sin(ry)) + self.x0[3] * self.ry_std[0] * abs(math.cos(ry)),
450 | 												self.dim_std[2] * abs(math.cos(ry)) + self.x0[4] * self.ry_std[0] * abs(math.sin(ry)),
451 | 												self.dim_std[2] * abs(math.sin(ry)) + self.x0[4] * self.ry_std[0] * abs(math.cos(ry)),
452 | 												self.dim_std[1]]) ** 2)
453 | 		self.posterior = None
454 | 		self.mean_LIDAR_variance = None
455 | 
456 | 	def set_posterior(self, mean, cov):
457 | 		# assert (cov.shape[0]==6),  "must set posterior of feature vector!"
458 | 		self.posterior = (mean, cov)
459 | 
460 | 	def embedding(self, x0, z_surface):
461 | 		#x0=[x,y,z,l,h,w,ry]
462 | 		# surface is 1D in BEV, from 0 to 4 ([-l,-w]/2->[l,-w]/2->[l,w]/2->[-l,w]/2), l along 1st axis
463 | 		# height is [-h, 0]
464 | 		z_embed = np.zeros((z_surface.shape[0], 3))
465 | 		x0_BEV = x0[[0,2,3,5,6]]
466 | 		z_embed[:,[0,2]] = uncertain_label_BEV.embedding(self, x0_BEV, z_surface[:,0])
467 | 		z_surface[z_surface[:,1]<-0.5,1] += 1
468 | 		z_surface[z_surface[:,1]> 0.5,1] -= 1 
469 | 		z_embed[:,1] = z_surface[:,1]*x0[4]
470 | 		return z_embed
471 | 
472 | 	def project(self, x0, z_embed):
473 | 		z_surface = np.zeros((z_embed.shape[0],2))
474 | 		x0_BEV = x0[[0,2,3,5,6]]
475 | 		z_surface[:,0] = uncertain_label_BEV.project(self, x0_BEV, z_embed[:,[0,2]])
476 | 		z_surface[:,1] = z_embed[:,1]/x0[4]
477 | 
478 | 		return z_surface
479 | 
480 | 	def add_surface_coord(self, z_surface, z_coord):
481 | 		z_surface_added = z_surface+z_coord
482 | 		z_surface_added[:,1] = np.minimum(-0.5,np.maximum(z_surface_added[:,1],0.5))
483 | 		return z_surface_added
484 | 
485 | 	def Jacobian_z_normed(self, z_normed):
486 | 		# parameterize surface point w.r.t. BEV box feature vector
487 | 		# the feature vector is (x, y, z, cos(ry)*l, sin(ry)*l, cos(ry)*w, sin(ry)*w, h), 1*8
488 | 		nz = z_normed.shape[0]
489 | 		Jacobian = np.zeros((nz, 3, 8))
490 | 		Jacobian[:, 0, 0] = 1.0
491 | 		Jacobian[:, 1, 1] = 1.0
492 | 		Jacobian[:, 2, 2] = 1.0
493 | 		Jacobian[:, 0, 3] = z_normed[:, 0]
494 | 		Jacobian[:, 1, 4] = z_normed[:, 0]
495 | 		Jacobian[:, 1, 5] = z_normed[:, 2]
496 | 		Jacobian[:, 0, 6] = -z_normed[:, 2]
497 | 		Jacobian[:, 2, 7] = z_normed[:, 1]
498 | 		return Jacobian
499 | 
500 | 	def Jacobian_surface(self, z_surface):
501 | 		# parameterize surface point w.r.t. BEV box feature vector
502 | 		z_normed = self.embedding(self.x0, z_surface) / self.x0[3:6]
503 | 		Jacobian = self.Jacobian_z_normed(z_normed)
504 | 		return Jacobian
505 | 
506 | 	def calc_uncertainty_corners(self):
507 | 		z_normed = np.array([[0.5,0.5,0.5,1],[0.5,0.5,-0.5,1],[-0.5,0.5,-0.5,1],[-0.5,0.5,0.5,1],[0.5,-0.5,0.5,1],[0.5,-0.5,-0.5,1],[-0.5,-0.5,-0.5,1],[-0.5,-0.5,0.5,1]])
508 | 		nz = z_normed.shape[0]
509 | 		cov = self.posterior[1]
510 | 		Jacobians_point = self.Jacobian_z_normed(z_normed)
511 | 		covs_out = np.zeros((z_normed.shape[0], 3, 3))
512 | 		assert(cov.shape[0] == 8)
513 | 		for i in range(nz):
514 | 			Jacobian_point = np.squeeze(Jacobians_point[i, :, :])
515 | 			covs_out[i, :, :] = (Jacobian_point @ cov) @ Jacobian_point.transpose()
516 | 		return covs_out
517 | 
518 | ##############################################################
519 | ### Uncertain Prediction class ###
520 | ##############################################################
521 | 
522 | class uncertain_prediction():
523 | 	def __init__(self, box):
524 | 		self.description = "a template of uncertain prediction"
525 | 		self.posterior = None
526 | 
527 | 	def sample_prob(self, points):
528 | 		prob = []
529 | 		return prob
530 | 
531 | class uncertain_prediction_BEVdelta(uncertain_prediction):
532 | # actually a deterministic prediction, just to unify the API
533 | 	def __init__(self, boxBEV):
534 | 		self.boxBEV = boxBEV
535 | 		self.x0 = np.array(boxBEV).reshape(5)
536 | 		# reverse ry so that the rotation is x->z instead of z->x normally
537 | 		self.x0[4] *= -1
538 | 		self.ry = self.x0[4]
539 | 		self.cry = math.cos(self.ry)
540 | 		self.sry = math.sin(self.ry)
541 | 		self.rotmat = np.array([[self.cry, -self.sry], [self.sry, self.cry]])
542 | 
543 | 	def get_corner_norms(self):
544 | 		corner_norms = z_normed_corners_default
545 | 		return corner_norms
546 | 
547 | 	def calc_uncertainty_corners(self, std=1):
548 | 		return self.calc_uncertainty_points(self.get_corner_norms())
549 | 
550 | 	def calc_uncertainty_box(self, std=1):
551 | 		#by default, calculate the contour of 1 standard deviation by variance along the boundary of bounding box
552 | 		corners = center_to_corner_BEV(self.boxBEV)
553 | 		return corners, corners
554 | 
555 | 	def calc_uncertainty_points(self, std=1):
556 | 		assert False, 'This must be overloaded by the child class.'
557 | 		return None
558 | 
559 | 	def sample_prob(self, points, sample_grid=0.1):
560 | 
561 | 		return self.sample_prob_delta(points)
562 | 
563 | 	def sample_prob_delta(self, points, sample_grid=0.1):
564 | 		# sample probablity given points
565 | 		nk = points.shape[0]
566 | 		probs = np.zeros(nk)
567 | 		l2 = self.x0[2] / 2
568 | 		w2 = self.x0[3] / 2
569 | 		points_aligned_to_pred = np.matmul(points - self.x0[0:2], self.rotmat)
570 | 		clip_idx = points_aligned_to_pred[:, 0] >= -l2
571 | 		clip_idx = np.logical_and(clip_idx, points_aligned_to_pred[:, 0] < l2)
572 | 		clip_idx = np.logical_and(clip_idx, points_aligned_to_pred[:, 1] >= -w2)
573 | 		clip_idx = np.logical_and(clip_idx, points_aligned_to_pred[:, 1] < w2)
574 | 		# calculate JIoU with discrete sample
575 | 		n_in = np.sum(clip_idx)
576 | 		if n_in > 0:
577 | 			probs[clip_idx] = 1/n_in
578 | 		return probs
579 | 
580 | class uncertain_prediction_BEV(uncertain_prediction_BEVdelta):
581 | # The uncertain prediction bounding box from Di Feng's model, not the same as WZN's label uncertainty model
582 | # std = [xc1, xc2, log(l), log(w), 0, 0]
583 | # approximate std(l) = exp(std(log(l)))-1
584 | 	def __init__(self, boxBEV, std):
585 | 		uncertain_prediction_BEVdelta.__init__(self,boxBEV)
586 | 		self.std0 = std
587 | 		self.feature0 = np.array([self.x0[0], self.x0[1], self.x0[2], self.x0[3]]) # dim 4 #self.x0[2]*np.exp(0.5*std[2]**2), self.x0[3]*np.exp(0.5*std[2]**2)]
588 | 		#print(std[2],np.sqrt(np.exp(std[2]**2)*(np.exp(std[2]**2)-1)),std[3],np.sqrt(np.exp(std[3]**2)*(np.exp(std[3]**2)-1)))
589 | 		self.cov0_feature = np.diag(np.array([std[0]**2, std[1]**2, (self.x0[2]*(np.exp(std[2])-1))**2, (self.x0[3]*(np.exp(std[3])-1))**2]))#WZN: the wrong one until uncertaintyV3 is np.exp(std[3])-1] # np.exp(std[2]**2)*(np.exp(std[2]**2)-1)*self.x0[2]**2, np.exp(std[3]**2)*(np.exp(std[3]**2)-1)*self.x0[3]**2
590 | 		self.max_sample_grid = max(0.01,min(std[0],std[1])/2)
591 | 		#print("This is an uncertainty prediction with max_sample_grid={}".format(self.max_sample_grid))
592 | 
593 | 	def Jacobian_z_normed(self, z_normed):
594 | 		# parameterize surface point w.r.t. BEV box feature vector
595 | 		# the output vector is (xc1+cos(ry)*l-sin(ry)*w, xc2+sin(ry)*l+cos(ry)*w), 1*2
596 | 		nz = z_normed.shape[0]
597 | 		Jacobian = np.zeros((nz, 2, 4))
598 | 		Jacobian[:, 0, 0] = 1.0
599 | 		Jacobian[:, 1, 1] = 1.0
600 | 		Jacobian[:, 0, 2] = z_normed[:, 0]*self.cry
601 | 		Jacobian[:, 0, 3] = -z_normed[:, 1]*self.sry
602 | 		Jacobian[:, 1, 2] = z_normed[:, 0]*self.sry
603 | 		Jacobian[:, 1, 3] = z_normed[:, 1]*self.cry
604 | 		return Jacobian
605 | 
606 | 	def calc_uncertainty_box(self, std=1):
607 | 		#by default, calculate the contour of 1 standard deviation by variance along the boundary of bounding box
608 | 		z_normed = np.array([[0.5,0],[0,0.5],[-0.5,0],[0,-0.5]])
609 | 		centers, covs = self.calc_uncertainty_points(z_normed)
610 | 		#get variance along the direction to center
611 | 		direction = centers - self.x0[0:2]
612 | 		lengths = np.linalg.norm(direction, axis=1, keepdims=True)
613 | 		direction = direction / lengths
614 | 		#get a n*1 distance array
615 | 		dists2 = np.sum(np.sum(covs*np.expand_dims(direction,axis=2),axis=1)*direction, axis=1).reshape([-1,1])
616 | 		dists = np.sqrt(dists2)
617 | 		ratio = 1+dists*std/lengths
618 | 		outer_corners = self.x0[0:2] + direction[[0,0,2,2],:]*lengths[[0,0,2,2]]*ratio[[0,0,2,2]] + direction[[3,1,1,3],:]*lengths[[3,1,1,3]]*ratio[[3,1,1,3]]
619 | 		inner_corners = self.x0[0:2] + direction[[0,0,2,2],:]*lengths[[0,0,2,2]]/ratio[[0,0,2,2]] + direction[[3,1,1,3],:]*lengths[[3,1,1,3]]/ratio[[3,1,1,3]]
620 | 		return inner_corners, outer_corners
621 | 
622 | 	def calc_uncertainty_points(self, z_normed):
623 | 		nz = z_normed.shape[0]
624 | 		cov = self.cov0_feature
625 | 		Jacobians_point = self.Jacobian_z_normed(z_normed)
626 | 		covs_out = np.zeros((z_normed.shape[0], 2, 2))
627 | 		centers_out = np.matmul(z_normed * self.x0[2:4], self.rotmat.transpose()) + self.x0[0:2]
628 | 		for i in range(nz):
629 | 			Jacobian_point = np.squeeze(Jacobians_point[i, :, :])
630 | 			covs_out[i, :, :] = (Jacobian_point @ cov) @ Jacobian_point.transpose()
631 | 		return centers_out, covs_out
632 | 
633 | 	def sample_prob(self, points, sample_grid=0.1):
634 | 		#WZN: I think for this model we can have explicit theoretical solution
635 | 		# sample probablity given points
636 | 		sample_grid = min(sample_grid, self.max_sample_grid)
637 | 		nk = points.shape[0]
638 | 		cov = self.cov0_feature
639 | 		if np.max(np.sqrt(np.linalg.eig(cov)[0]))<sample_grid*5:
640 | 			#empirical coeffienct, if cov too low, then sample_grid is not thick enough. Just assume the distribution is uniform (delta uncertainty)
641 | 			probs = self.sample_prob_delta(points)
642 | 		else:
643 | 			probs = np.zeros(nk)
644 | 			z_normed = create_BEV_box_sample_grid(sample_grid)
645 | 			centers_out, covs_out = self.calc_uncertainty_points(z_normed)
646 | 			for i in range(covs_out.shape[0]):
647 | 				tmp_probs = multivariate_normal.pdf(points, mean=centers_out[i, :], cov=covs_out[i, :, :])
648 | 				# exponents = np.sum((points-centers_out[i,:])*((points-centers_out[i,:])@ np.linalg.inv(covs_out[i,:,:])),axis=1)/-2
649 | 				# tmp_probs = np.exp(exponents)
650 | 				probs += tmp_probs
651 | 			probs /= np.sum(probs)
652 | 		return probs
653 | 
654 | class uncertain_prediction_BEV_interp(uncertain_prediction_BEVdelta):
655 | 	# The uncertain prediction bounding box with sampled corners (from Hujie's paper) as input.
656 | 	def __init__(self, boxBEV, Ws, Vs):
657 | 		'''
658 | 		Construct the box with corner uncertainty inputs.
659 | 
660 | 		Args:
661 | 			Ws: The homogeneous coordinates of sampled points. (4 or 6 points)
662 | 			Vs: The covariance matrix of sampled points.
663 | 		'''
664 | 		uncertain_prediction_BEVdelta.__init__(self, boxBEV)
665 | 		self.num_points = len(Ws)
666 | 		assert Vs.shape[1] == 2, 'Varaicne matrix error.'
667 | 		if self.num_points == 6:
668 | 			self.cov_interpolation = cov_interp_BEV_full(Ws, Vs)
669 | 		elif self.num_points == 4: 
670 | 			self.cov_interpolation = cov_interp_BEV_corner(Ws, Vs)
671 | 		else: 
672 | 			raise ValueError('the number of sampled points can only be 4 or 6 for BEV while %d is given.' % self.num_points)
673 | 
674 | 	def calc_uncertainty_box(self, std=1):
675 | 		return uncertain_prediction_BEV.calc_uncertainty_box(self, std=std)
676 | 
677 | 	def calc_uncertainty_points(self, z_normed):
678 | 		ws = np.concatenate((z_normed, np.ones([z_normed.shape[0], 1])), axis=1)
679 | 		covs_out = self.cov_interpolation.interps(ws)
680 | 		centers_out = np.matmul(z_normed * self.x0[2:4], self.rotmat.transpose()) + self.x0[0:2]
681 | 		return centers_out, covs_out
682 | 
683 | 	def sample_prob(self, points, sample_grid=0.1):
684 | 		nk = points.shape[0]
685 | 		probs = np.zeros(nk)
686 | 		z_normed = create_BEV_box_sample_grid(sample_grid)
687 | 		centers_out, covs_out = self.calc_uncertainty_points(z_normed)	
688 | 
689 | 		# This is to avoid negative variance due to interpolation.
690 | 		w, v = np.linalg.eig(covs_out)
691 | 		if np.sum(w < 0) > 0:
692 | 			print('Warning: Negative variance detected at a sample')
693 | 			w[w < 0.02**2] = 0.02**2
694 | 			covs_out = np.matmul(np.transpose(v, axes=(0, 2, 1)), np.expand_dims(w, axis=-2) * v)
695 | 
696 | 		for i in range(covs_out.shape[0]):
697 | 			tmp_probs = multivariate_normal.pdf(points, mean=centers_out[i, :], cov=covs_out[i, :, :])
698 | 			probs += tmp_probs
699 | 		probs /= np.sum(probs)
700 | 		return probs
701 | 
702 | 


--------------------------------------------------------------------------------
/utils/probability_utils.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import os
  3 | import sys
  4 | from tqdm import tqdm
  5 | from utils.kitti_utils import boxes3d_to_corners3d
  6 | 
  7 | 
  8 | class cov_interp:
  9 | 	#interpterlate covariance matrix of Var[X(w)] = cov[\Phi(Y)ww^T\Phi(Y)^T] where w is n*1 and Y is m*n (Var[X] is m*m)
 10 | 	#Y is a fixed random variable matrix, w is the coordinate (input argument)
 11 | 	def __init__(self, W, Vs):
 12 | 		#W is tuple of k input w's
 13 | 		#Vs are the k corresponding Var[X(w)]'s
 14 | 		#the solved preprocessed matrix is Vinterp so that new Var[X(w)] = flatten(uppertriangle(ww^T))^T * Vinterp
 15 | 		k = len(W)
 16 | 		Vstack = np.zeros([k,Vs[0].size])
 17 | 		self.m = Vs[0].shape[0]
 18 | 		self.n = W[0].size
 19 | 		wwstack = np.zeros([self.ww_flatten_size(),k])
 20 | 		for i in range(k):
 21 | 			w = W[i]
 22 | 			ww = np.matmul(w.reshape([self.n,1]),w.reshape([1,self.n]))
 23 | 			wwstack[:,i] = self.extract_upper_triangle_ww(ww)
 24 | 			Vstack[i,:] = Vs[i].reshape([1,-1])
 25 | 		#print(w, ww, self.extract_upper_triangle_ww(ww))
 26 | 		#print(np.transpose(wwstack).shape, Vstack.shape)
 27 | 		#print(np.linalg.eig(wwstack))
 28 | 		#print(wwstack)
 29 | 		self.Vinterp = np.linalg.solve(np.transpose(wwstack),Vstack)
 30 | 
 31 | 	def interp(self,w):
 32 | 		#a single point w: n*1
 33 | 		ww = np.matmul(w.reshape([self.n,1]),w.reshape([1,self.n]))
 34 | 		ww_flatten = self.extract_upper_triangle_ww(ww)
 35 | 		Vout = ww_flatten*self.Vinterp
 36 | 		return Vout.reshape([self.m,self.m])
 37 | 
 38 | 	def interps(self,ws):
 39 | 		#multiple points ws: k*n
 40 | 		wws_flatten = self.extract_upper_triangle_ws(ws)
 41 | 		Vouts = np.matmul(wws_flatten, self.Vinterp)
 42 | 		return Vouts.reshape([ws.shape[0], self.m, self.m])
 43 | 
 44 | 	def ww_flatten_size(self):
 45 | 		return int(self.n*(self.n+1)/2)
 46 | 
 47 | 	def extract_upper_triangle_ww(self,ww):
 48 | 		return ww[np.triu_indices(self.n)]
 49 | 
 50 | 	def extract_upper_triangle_ws(self,ws):
 51 | 		k = ws.shape[0]
 52 | 		wws_flatten = np.zeros([k,self.ww_flatten_size()])
 53 | 		j0 = 0
 54 | 		for i in range(self.n):
 55 | 			wws_flatten[:,j0:j0+self.n-i] = ws[:,i:i+1] * ws[:,i:]
 56 | 			j0 += self.n-i
 57 | 		return wws_flatten
 58 | 
 59 | 
 60 | class cov_interp_BEV_full(cov_interp):
 61 | 	#all the same, BEV requires k = 6 points (eg. 4 corners + 2 points on the side)
 62 | 	def __init__(self, W, Vs):
 63 | 		assert len(W) == 6
 64 | 		cov_interp.__init__(self, W, Vs) 
 65 | 
 66 | 
 67 | class cov_interp_3D(cov_interp):
 68 | 	#3D is a little bit different, because pitch and roll angle are not active
 69 | 	#if take the full triangular matrix, would require 10 points
 70 | 	#if ignore the pitch and roll, would require 8 points, eg. 6(BEV, say on the top) points + 2 bottom corner points
 71 | 	def __init__(self, W, Vs):
 72 | 		#for 3D, only take these cols points
 73 | 		self.flatten_idx = [0,2,3,4,6,7,8,9]
 74 | 		cov_interp.__init__(self, W, Vs)
 75 | 
 76 | 	def ww_flatten_size(self):
 77 | 		return len(self.flatten_idx)
 78 | 
 79 | 	def extract_upper_triangle_ww(self,ww):
 80 | 		ww_flatten = ww[np.triu_indices(self.n)]
 81 | 		return ww_flatten[self.flatten_idx]
 82 | 
 83 | 	def extract_upper_triangle_ws(self,ws):
 84 | 		wws_flatten = cov_interp.extract_upper_triangle_ws(self,ws)
 85 | 		return wws_flatten[:,self.flatten_idx]
 86 | 
 87 | 
 88 | #def bbox_uncertainty_to_spatial_uncertainty_BEV(boxBEV, std):
 89 | 	#assume inputs are joint Gaussian, use importance sampling with student distribution to calculate variances of corners
 90 | 
 91 | def W_Vs_3D_to_BEV_full(W_3D, Vs_diag_3D):
 92 | 	assert len(Vs_diag_3D.shape) == 3, 'The Vs shape should be n*8*3.'
 93 | 	W_BEV = W_3D[[0,1,2,4,5,7], :][:, [0,2,3]]
 94 | 	Vs_BEV = np.zeros([Vs_diag_3D.shape[0], 6, 2, 2])
 95 | 	for i in range(Vs_diag_3D.shape[0]):
 96 | 		Vs_diag = Vs_diag_3D[i]
 97 | 		Vs = np.array([np.diag(Vs_diag[i]) for i in range(Vs_diag.shape[0])])
 98 | 		Vs_BEV[i] = Vs[[0,1,2,4,5,7], :, :][:, [0,2], :][:, :, [0,2]]
 99 | 	return W_BEV, Vs_BEV
100 | 
101 | def Hujie_uncertainty_reader(pred_dir, file_lists, max_num=7518):
102 | 	import pickle
103 | 	pickle_dir = pred_dir + 'unc_data/'
104 | 	attrs = ['box3D','corners','points_unc_sigma','ry','homogeneous_w','uncertainty_format']
105 | 	unc_data = {attr:[[] for name in range(max_num)] for attr in attrs}
106 | 	print('reading data from: {}'.format(pickle_dir))
107 | 	for file in file_lists:
108 | 		file_num = file.split('.')[0]
109 | 		name = file_num
110 | 		frame = int(name)
111 | 		if int(float(name)) >= max_num:
112 | 			break
113 | 		corner_unc_file_dir = os.path.join(pickle_dir,"{}_UNC.pickle".format(file_num))
114 | 		unc_info = pickle.load(open(corner_unc_file_dir,"rb"))
115 | 		# box3D is [x, y_top, z, h, w, l]
116 | 		unc_data['box3D'][frame] = unc_info['bbox3d'][:,0:6]
117 | 		# Some predictions got negative lengths....
118 | 		unc_data['box3D'][frame][:, 3:6] = np.abs(unc_data['box3D'][frame][:, 3:6])
119 | 		unc_data['ry'][frame] = unc_info['bbox3d'][:,6]
120 | 		unc_data['corners'][frame] = boxes3d_to_corners3d(unc_info['bbox3d'])
121 | 		if 'full' in pickle_dir:
122 | 			unc_data['homogeneous_w'][frame] = np.array([[0.5,-1,0.5,1],[0.5,-1,-0.5,1],[-0.5,-1,-0.5,1],[0.5,0,-0.5,1],[0.5,-1,0,1],[0,-1,-0.5,1],[0.5,-0.5,-0.5,1],[-0.5,-1,0.5,1]])  #8*3
123 | 			unc_data['uncertainty_format'] = 'full'
124 | 		else:
125 | 			print('The uncertainty configuration cannot infer full covariance distribution inside the bounding box!')
126 | 			assert False, 'The warning is treated as error for now.'
127 | 			unc_data['homogeneous_w'][frame] = np.array([[0.5,-1,0.5,1],[0.5,-1,-0.5,1],[-0.5,-1,-0.5,1],[-0.5,-1,0.5,1],[0.5,0,0.5,1],[0.5,0,-0.5,1],[-0.5,0,-0.5,1],[-0.5,-0.5,0.5,1]])  #8*3
128 | 			unc_data['uncertainty_format'] = 'corner'
129 | 		unc_data['points_unc_sigma'][frame] = unc_info['bbox3d_sigma'].reshape([-1,8,3])
130 | 	return unc_data
131 | 


--------------------------------------------------------------------------------
/utils/simple_stats.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import os
  3 | import sys
  4 | import matplotlib.pyplot as plt
  5 | from matplotlib.patches import Polygon, Circle, Arrow
  6 | import matplotlib.gridspec as gridspec
  7 | import matplotlib.patches as patches
  8 | import scipy.misc
  9 | import logging
 10 | import coloredlogs
 11 | from PIL import Image, ImageDraw, ImageFont
 12 | from easydict import EasyDict as edict
 13 | from tqdm import tqdm
 14 | import glob
 15 | import cv2
 16 | import math
 17 | 
 18 | sys.path.append('../')
 19 | from utils.paul_geometry import draw_3dbox_in_2d, read_calib_mat, camera_to_lidar, draw_birdeye, cal_box3d_iou, lidar_to_camera, clip_by_BEV_box, get_cov_ellipse, draw_birdeye_ellipse
 20 | from utils.metric_plots import *
 21 | from utils.kitti_utils import box3d_to_boxBEV
 22 | from utils.probability_utils import W_Vs_3D_to_BEV_full
 23 | from utils.metric_utils import label_inference_BEV, label_uncertainty_IoU, uncertain_prediction_BEVdelta, uncertain_prediction_BEV, uncertain_prediction_BEV_interp
 24 | 
 25 | #modify this to view different networks
 26 | networks = ['PointRCNN']
 27 | #['STD']#['AVOD']#['SECOND']#['PIXORbad']#['PIXORnew']#['Voxel']#['PointRCNN']#['ProbPIXOR']#['ProbCalibPIXOR']#['PIXORwaymo']#['ProbPIXORwaymo']#['ProbCalibPIXORwaymo]#['ProbPIXORwaymo_boxonly']
 28 | pred_3d_dirs = []
 29 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/std_val/')
 30 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/avod_kitti_val50/')
 31 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/second_kitti_val50/')
 32 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/pixor_bad_kitti_val50/')
 33 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/pixor_new_kitti_val50/')
 34 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/voxelnet_kitti_val50/')
 35 | pred_3d_dirs.append('/data/RPN/coop_DiFeng/PointRCNN_val50/')
 36 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/probablistic_pixor/')
 37 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/probablistic_calibrated_pixor/')
 38 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/waymo_detection_2d/')
 39 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/waymo_detection_2d_probablistic/')
 40 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/waymo_detection_2d_probablistic_calibrated/')
 41 | #pred_3d_dirs.append('/data/RPN/coop_DiFeng/waymo_detection_2d_probablistic_boxonly/')
 42 | 
 43 | actual_test = False
 44 | data_dir = '/home/msc/KITTI/object' #'/data/RPN/coop_DiFeng/WaymoData' #
 45 | 
 46 | def get_dirs(data_dir_in, actual_test_in, val_set=None):
 47 | 	if actual_test_in == False:
 48 | 		folder_name = 'training'
 49 | 		if val_set == 'Hujie':
 50 | 			print('Using validataion set of Hujie Pan.')
 51 | 			list_name = 'val_Hujie'
 52 | 		else:
 53 | 			list_name = 'val'#'val_uncertainty_debug'#
 54 | 		label_3d_dir = os.path.join(data_dir_in, folder_name, 'label_2')
 55 | 	else:
 56 | 		folder_name = 'testing'
 57 | 		list_name = 'test'
 58 | 	if data_dir_in.find('Waymo')!=-1 or data_dir_in.find('waymo') != -1:
 59 | 		list_dir = '/data/RPN/coop_DiFeng/waymo_detection_2d/imageset/val.txt'#
 60 | 	else:
 61 | 		list_dir = os.path.join(data_dir_in, 'kitti/ImageSets/{}.txt'.format(list_name))#
 62 | 
 63 | 	img_dir = os.path.join(data_dir_in, folder_name, 'image_2')
 64 | 	lidar_dir = os.path.join(data_dir_in, folder_name, 'velodyne')
 65 | 	calib_dir = os.path.join(data_dir_in, folder_name, 'calib')
 66 | 	return list_dir, img_dir, lidar_dir, calib_dir, label_3d_dir
 67 | 
 68 | 
 69 | def label_reader(label_3d_dir,file_lists,calib_dir,max_num=7518):
 70 | 	#specify the range of files or names of files
 71 | 	start_file_num = -1
 72 | 	end_file_num = 1000000
 73 | 	test_name = None#['007065']
 74 | 	using_waymo = False
 75 | 	if label_3d_dir.find('Waymo')!=-1 or label_3d_dir.find('waymo') != -1:
 76 | 		#require a special converter for waymo dataset
 77 | 		using_waymo = True
 78 | 
 79 | 	attrs = ['class','class_id','box2D','box3D','ry','calib','occlusion','truncation','difficulty','alpha']
 80 | 	label_data = {attr:[[] for name in range(max_num)] for attr in attrs}
 81 | 	print('reading label_data from: {}'.format(label_3d_dir))
 82 | 	for label_file in tqdm(file_lists):
 83 | 		file_num = label_file.split('.')[0] 
 84 | 		name = file_num
 85 | 		frame = int(name)
 86 | 		if test_name is not None:
 87 | 			if name not in test_name:
 88 | 				continue
 89 | 		if int(float(name)) <= start_file_num:
 90 | 			continue
 91 | 		if int(float(name)) > end_file_num:
 92 | 			break
 93 | 		calib_mat = read_calib_mat(calib_dir, frame)
 94 | 		label_data['calib'][frame] = calib_mat
 95 | 		label_3d_file_dir = os.path.join(label_3d_dir, "{}.txt".format(file_num))
 96 | 		#print("label_3d_file_dir: {}".format(label_3d_file_dir))
 97 | 		with open(label_3d_file_dir,'r') as fi:
 98 | 			line = fi.readline()
 99 | 			while len(line)>4:
100 | 				label_infos = line.split()	
101 | 				# print("label_infos: {}".format(label_infos))	
102 | 				label_data['class'][frame].append(label_infos[0])
103 | 				if (label_infos[0]=='Car') or (label_infos[0]=='Van') or (label_infos[0]=='Truck'):
104 | 					#vehicles
105 | 					label_data['class_id'][frame].append(1)
106 | 				else:
107 | 					label_data['class_id'][frame].append(-1)
108 | 				trun = float(label_infos[1])
109 | 				occ  = int(label_infos[2])
110 | 				label_data['alpha'][frame] = float(label_infos[3])
111 | 				label_data['truncation'][frame].append(trun)
112 | 				label_data['occlusion'][frame].append(occ)
113 | 				if trun<=0.15 and occ<=0:
114 | 					label_data['difficulty'][frame].append(0)
115 | 				elif trun<=0.3 and occ<=1:
116 | 					label_data['difficulty'][frame].append(1)
117 | 				elif trun<=0.5 and occ<=2:
118 | 					label_data['difficulty'][frame].append(2)
119 | 				else:
120 | 					label_data['difficulty'][frame].append(-1)
121 | 				x1 = int(float(label_infos[4]))
122 | 				y1 = int(float(label_infos[5]))
123 | 				x2 = int(float(label_infos[6]))
124 | 				y2 = int(float(label_infos[7]))
125 | 				label_data['box2D'][frame].append([x1,y1,x2,y2])
126 | 				obj_h = float(label_infos[8])
127 | 				obj_w = float(label_infos[9])
128 | 				obj_l = float(label_infos[10])
129 | 				x_cam = float(label_infos[11])
130 | 				if not using_waymo:
131 | 					y_cam = float(label_infos[12])
132 | 					z_cam = float(label_infos[13])
133 | 					label_data['ry'][frame].append(float(label_infos[14]))
134 | 				else:
135 | 					#data converter for waymo, since it uses x,y,z center in LIDAR frame
136 | 					y_cam = float(label_infos[13])+obj_h/2
137 | 					z_cam = float(label_infos[12])
138 | 					label_data['ry'][frame].append(-float(label_infos[14]))
139 | 				label_data['box3D'][frame].append([x_cam,y_cam,z_cam,obj_h,obj_w,obj_l])
140 | 				
141 | 				line = fi.readline()
142 | 	return label_data
143 | 
144 | def detect_reader(pred_3d_dir,file_lists,waymo_in_label_dir,max_num=7518):
145 | 	attrs = ['class','box2D','box3D','ry','score','uncertainty','file_exists','alpha']
146 | 	pred_data = {attr:[[] for name in range(max_num)] for attr in attrs}
147 | 	print('reading prediction_data from: {}'.format(pred_3d_dir))
148 | 	count = 0
149 | 	using_waymo = False
150 | 	if waymo_in_label_dir or pred_3d_dir.find('Waymo') != -1:
151 | 		# require a special converter for waymo dataset
152 | 		using_waymo = True
153 | 	if os.path.isfile(pred_3d_dir+'uncertainty_calibration/scale.txt'):
154 | 		print('using calibration result for prediction uncertainty')
155 | 		with open(pred_3d_dir+'uncertainty_calibration/scale.txt','r') as fi:
156 | 			line = fi.readline()
157 | 			uncertainty_calibs = line.split()
158 | 			cosry_s = np.sqrt(float(uncertainty_calibs[0]))
159 | 			sinry_s = np.sqrt(float(uncertainty_calibs[1]))
160 | 			xc1_s = np.sqrt(float(uncertainty_calibs[2]))
161 | 			xc2_s = np.sqrt(float(uncertainty_calibs[3]))
162 | 			logl_s = np.sqrt(float(uncertainty_calibs[4]))
163 | 			logw_s = np.sqrt(float(uncertainty_calibs[5]))
164 | 	else:
165 | 		xc1_s = 1; xc2_s = 1; logl_s = 1; logw_s = 1; cosry_s = 1; sinry_s = 1
166 | 	for pred_file in tqdm(file_lists):
167 | 		file_num = pred_file.split('.')[0] 
168 | 		name = file_num
169 | 		frame = int(name)
170 | 		pred_file_dir = os.path.join(pred_3d_dir,'data/', "{}.txt".format(file_num))
171 | 		pred_uncertainty_dir = os.path.join(pred_3d_dir,'uncertainty/', "{}.txt".format(file_num))
172 | 		#print("pred_file_dir: {}".format(pred_file_dir))
173 | 		if not os.path.isfile(pred_file_dir):
174 | 			continue
175 | 		count += 1
176 | 		pred_data['file_exists'][frame] = True
177 | 		with open(pred_file_dir,'r') as fi:
178 | 			line = fi.readline()
179 | 			while len(line)>4:
180 | 				pred_infos = line.split()
181 | 				pred_data['class'][frame].append(pred_infos[0])
182 | 				pred_data['alpha'][frame].append(float(pred_infos[3]))
183 | 				alpha = float(pred_infos[3])
184 | 				x1 = float(pred_infos[4])
185 | 				y1 = float(pred_infos[5])
186 | 				x2 = float(pred_infos[6])
187 | 				y2 = float(pred_infos[7])
188 | 				pred_data['box2D'][frame].append([x1,y1,x2,y2])
189 | 				# Some predictions have negative lengths...
190 | 				obj_h = abs(float(pred_infos[8]))
191 | 				obj_w = abs(float(pred_infos[9]))
192 | 				obj_l = abs(float(pred_infos[10]))
193 | 				x_cam = float(pred_infos[11])
194 | 				if not using_waymo:
195 | 					y_cam = float(pred_infos[12])
196 | 					z_cam = float(pred_infos[13])
197 | 					pred_data['ry'][frame].append(float(pred_infos[14]))
198 | 				else:
199 | 					# data converter for waymo, since it uses x,y,z center in LIDAR frame
200 | 					y_cam = float(pred_infos[13]) + obj_h / 2
201 | 					z_cam = float(pred_infos[12])
202 | 					pred_data['ry'][frame].append(-float(pred_infos[14]))
203 | 				pred_data['box3D'][frame].append([x_cam,y_cam,z_cam,obj_h,obj_w,obj_l])
204 | 				pred_data['score'][frame].append(float(pred_infos[-1]))
205 | 				line = fi.readline()
206 | 		if not os.path.isfile(pred_uncertainty_dir):
207 | 			continue
208 | 		with open(pred_uncertainty_dir,'r') as fi:
209 | 			line = fi.readline()
210 | 			while len(line)>4:
211 | 				uncertainty_infos = line.split()
212 | 				cosry = np.sqrt(float(uncertainty_infos[0]))*cosry_s
213 | 				sinry = np.sqrt(float(uncertainty_infos[1]))*sinry_s
214 | 				#angle uncertainty read but should not be used
215 | 				xc1 = np.sqrt(float(uncertainty_infos[2]))*xc1_s
216 | 				xc2 = np.sqrt(float(uncertainty_infos[3]))*xc2_s
217 | 				logl = np.sqrt(float(uncertainty_infos[4]))*logl_s
218 | 				logw = np.sqrt(float(uncertainty_infos[5]))*logw_s
219 | 				pred_data['uncertainty'][frame].append([xc1,xc2,logl,logw,cosry,sinry])
220 | 				line = fi.readline()
221 | 		assert(len(pred_data['uncertainty'][frame])==len(pred_data['score'][frame]))
222 | 
223 | 	print('detection files read: ',count)
224 | 	return pred_data
225 | 
226 | 
227 | def hacker_reader(folder,file_lists,max_num=7518):
228 | 	difficulties = ['EASY','MODERATE','HARD']
229 | 	views = ['3D','G','2D']
230 | 	det_attrs = ['fp','gt','iou']
231 | 	gt_attrs = ['tp','fn','det']
232 | 
233 | 	hack_data = {diff:{'det':{view:{attr:[[] for name in range(max_num)] for attr in det_attrs} for view in views}, 
234 | 						'gt':{view:{attr:[[] for name in range(max_num)] for attr in gt_attrs} for view in views}} for diff in difficulties}
235 | 	print('reading hack_data from: {}'.format(folder))
236 | 	for difficulty in tqdm(difficulties):
237 | 		hack_det_file = 'hack_det_{}_evaluation.txt'.format(difficulty)
238 | 		hack_gt_file = 'hack_gt_{}_evaluation.txt'.format(difficulty)
239 | 		with open(os.path.join(folder,hack_det_file),'r') as fi:
240 | 			line = fi.readline()
241 | 			line = fi.readline()
242 | 			while len(line) > 4:
243 | 				hack_info = line.split(',')
244 | 				frame = int(float(hack_info[0]))
245 | 				idx = int(float(hack_info[1]))
246 | 				assert (idx == len(hack_data[difficulty]['det'][views[0]][det_attrs[0]][frame])), 'unsorted or missing detection index'
247 | 				for q,attr in enumerate(det_attrs[0:2]):
248 | 					for p,view in enumerate(views):
249 | 						hack_data[difficulty]['det'][view][attr][frame].append(int(float(hack_info[2+q*3+p])))
250 | 				#for iou, save float
251 | 				q = len(det_attrs)-1
252 | 				attr = det_attrs[q]
253 | 				for p,view in enumerate(views):
254 | 						hack_data[difficulty]['det'][view][attr][frame].append(float(hack_info[2+q*3+p]))
255 | 				line = fi.readline()
256 | 
257 | 		with open(os.path.join(folder,hack_gt_file),'r') as fi:
258 | 			line = fi.readline()
259 | 			line = fi.readline()
260 | 			while len(line) > 0:
261 | 				hack_info = line.split(',')
262 | 				frame = int(float(hack_info[0]))
263 | 				idx = int(float(hack_info[1]))
264 | 				assert (idx == len(hack_data[difficulty]['gt'][views[0]][gt_attrs[0]][frame])), 'unsorted or missing gt index'
265 | 				for q,attr in enumerate(gt_attrs):
266 | 					for p,view in enumerate(views):
267 | 						hack_data[difficulty]['gt'][view][attr][frame].append(int(float(hack_info[2+q*3+p])))
268 | 				line = fi.readline()
269 | 		#assign associations to gts labeled as fns
270 | 		for view in views:
271 | 			for frame in range(len(hack_data[difficulty]['gt'][view]['fn'])):
272 | 				max_iou = [0 for gt_idx in hack_data[difficulty]['gt'][view]['fn'][frame]]
273 | 				for j, gt_idx in enumerate(hack_data[difficulty]['det'][view]['gt'][frame]):
274 | 					iou = hack_data[difficulty]['det'][view]['iou'][frame][j]
275 | 					if gt_idx>=0 and hack_data[difficulty]['gt'][view]['det'][frame][gt_idx]<0 and iou>=max_iou[gt_idx]:
276 | 						hack_data[difficulty]['gt'][view]['det'][frame][gt_idx] = j
277 | 	return hack_data
278 | 
279 | def read_points_cam(lidar_dir, frame, label_data):
280 | 	if lidar_dir.find('Waymo')!=-1:
281 | 		name = "{:08d}".format(frame)
282 | 	else:
283 | 		name = "{:06d}".format(frame)
284 | 	lidar_path = os.path.join(lidar_dir,name+'.bin')
285 | 	points = np.fromfile(lidar_path, dtype=np.float32).reshape(-1,4)
286 | 	points = points[:,0:3]
287 | 	if lidar_dir.find('Waymo')!=-1:
288 | 		points_cam = np.array((points[:,0],points[:,2],points[:,1])).transpose()
289 | 	else:
290 | 		calib_mat = label_data['calib'][frame]
291 | 		xs, ys, zs = lidar_to_camera(points[:, 0], points[:, 1], points[:, 2], calib_mat)
292 | 		points_cam = np.array((xs,ys,zs)).transpose()
293 | 	return points_cam
294 | 
295 | def evaluate_JIoU(label_data, pred_data, frame, gt_idxs, pd_idxs, points_cam, grid_size=0.1, sample_grid=0.1, unc_data=None):
296 | 	assert(len(gt_idxs)==len(pd_idxs))
297 | 	inference = label_inference_BEV(degree_register=1,gen_std=0.25, prob_outlier=0.8,boundary_sample_interval=0.05)
298 | 	IoUcalculator = label_uncertainty_IoU(grid_size=grid_size, range=3)
299 | 	uncertain_labels = []
300 | 	JIoUs = []
301 | 	pred_uncertainty_ind = True if pred_data['uncertainty'][frame] or unc_data else False
302 | 	uncertainty_format = unc_data['uncertainty_format'] if unc_data else None
303 | 	label_boxBEVs = box3d_to_boxBEV(np.array(label_data['box3D'][frame]), np.array(label_data['ry'][frame]))
304 | 	pred_boxBEVs = box3d_to_boxBEV(np.array(pred_data['box3D'][frame]), np.array(pred_data['ry'][frame]))
305 | 	if uncertainty_format == 'full':
306 | 		W_BEV, Vs_BEV = W_Vs_3D_to_BEV_full(unc_data['homogeneous_w'][frame], unc_data['points_unc_sigma'][frame]**2)
307 | 		assert (Vs_BEV.shape[0]==pred_boxBEVs.shape[0]), 'inconsistent prediction and uncertainty data at frame %d, %d vs %d.' % (frame, Vs_BEV.shape[0], pred_boxBEVs.shape[0])
308 | 	for i, gt_idx in enumerate(gt_idxs):
309 | 		points_clip_BEV = clip_by_BEV_box(points_cam, label_data['box3D'][frame][gt_idx][0:3], label_data['box3D'][frame][gt_idx][3:6],
310 | 										  label_data['ry'][frame][gt_idx], buffer_size = 0.1)[:,[0,2]]
311 | 		uncertain_labels.append(inference.infer(points_clip_BEV, label_boxBEVs[gt_idx]))
312 | 		certain_label = uncertain_prediction_BEVdelta(label_boxBEVs[gt_idx])
313 | 		pd_idx = pd_idxs[i]
314 | 		if pd_idx >= 0:
315 | 			certain_pred = uncertain_prediction_BEVdelta(pred_boxBEVs[pd_idx])
316 | 			if pred_uncertainty_ind:
317 | 				if uncertainty_format == 'full':
318 | 					if frame < 10:
319 | 						print('using interpolated corner uncertainty as prediction uncertainty')
320 | 					uncertain_pred = uncertain_prediction_BEV_interp(pred_boxBEVs[pd_idx], W_BEV, Vs_BEV[pd_idx])
321 | 				elif uncertainty_format == 'corner':
322 | 					assert False, 'Unimplemented prediction uncertainty model.'
323 | 				else:
324 | 					uncertain_pred = uncertain_prediction_BEV(pred_boxBEVs[pd_idx], pred_data['uncertainty'][frame][pd_idx])
325 | 			else:
326 | 				uncertain_pred = uncertain_prediction_BEVdelta(pred_boxBEVs[pd_idx])
327 | 			JIoU = IoUcalculator.calc_IoU(uncertain_labels[i], [uncertain_pred,certain_label,certain_pred], sample_grid=sample_grid)
328 | 		else:
329 | 			JIoU = [0] + IoUcalculator.calc_IoU(uncertain_labels[i], [certain_label], sample_grid=sample_grid) + [0]
330 | 		JIoUs.append(JIoU)
331 | 	return JIoUs
332 | 
333 | def main():
334 | 	# (1) get file list, label and images
335 | 	list_dir, img_dir, lidar_dir, calib_dir = get_dirs(data_dir, actual_test)
336 | 
337 | 	num_net = len(networks)
338 | 	pred_datas = [[] for net in networks]
339 | 	hack_datas = [[] for net in networks]
340 | 
341 | 	output_root = './results/'
342 | 	output_dir = output_root + 'results_visualizer_new'
343 | 	for net in networks:
344 | 		output_dir += '_'+net
345 | 	output_dir += '/'
346 | 	os.makedirs(output_dir, exist_ok=True)
347 | 
348 | 
349 | 	with open(list_dir) as fi:
350 | 		file_lists = fi.read().splitlines() 
351 | 	file_lists.sort()
352 | 
353 | 	label_data = label_reader(label_3d_dir,file_lists,calib_dir)
354 | 	for inet in range(len(networks)):
355 | 		hack_datas[inet] = hacker_reader(pred_3d_dirs[inet], file_lists)
356 | 		pred_datas[inet] = detect_reader(pred_3d_dirs[inet], file_lists, label_3d_dir.find('waymo') != -1)
357 | 
358 | 	#all labels, JIoUs vs IoU
359 | 	difficulty = 'HARD'
360 | 	view = 'G'
361 | 	frames = [int(file.split('.')[0]) for file in file_lists]
362 | 	all_gt_idxs = [{net:[] for net in networks} for frame in frames]
363 | 	all_pd_idxs = [{net:[] for net in networks} for frame in frames]
364 | 	all_IoU_dic = [{net:[] for net in networks} for frame in frames]
365 | 	corner_totalVariances = []
366 | 	for id_file ,file in enumerate(tqdm(file_lists)):
367 | 		frame = int(file.split('.')[0])
368 | 		points_cam = read_points_cam(lidar_dir, frame, label_data)
369 | 		num_active_gts = 0
370 | 		IoU_dic = {}
371 | 		all_gts = np.zeros(len(hack_datas[0][difficulty]['gt'][view]['tp'][frame]))
372 | 		for inet, net in enumerate(networks):
373 | 			gts = np.logical_or(np.array(hack_datas[inet][difficulty]['gt'][view]['tp'][frame])==1,np.array(hack_datas[inet][difficulty]['gt'][view]['fn'][frame])==1)
374 | 			all_gts = np.logical_or(all_gts, gts)
375 | 		active_gt_idxs = np.argwhere(all_gts)
376 | 		num_active_gts += active_gt_idxs.size
377 | 		for inet, net in enumerate(networks):
378 | 			output_JIoU_dir = output_dir + net + '_JIoUs/'
379 | 			os.makedirs(output_JIoU_dir, exist_ok=True)
380 | 			if active_gt_idxs.size>0:
381 | 				ass_det_idxs = np.array(hack_datas[inet][difficulty]['gt'][view]['det'][frame])[active_gt_idxs]
382 | 				gt_idxs = [active_gt_idxs.item(i) for i in range(active_gt_idxs.size)]
383 | 				pd_idxs =  [ass_det_idxs.item(i) for i in range(ass_det_idxs.size)]
384 | 				IoU = []
385 | 				for pd_idx in pd_idxs:
386 | 					if pd_idx>=0:
387 | 						IoU.append(hack_datas[inet][difficulty]['det'][view]['iou'][frame][pd_idx])
388 | 					else:
389 | 						IoU.append(0)
390 | 				all_gt_idxs[id_file][net] = gt_idxs
391 | 				all_pd_idxs[id_file][net] = pd_idxs
392 | 
393 | 				JIoUs = evaluate_JIoU(label_data, pred_datas[inet], frame, gt_idxs, pd_idxs, points_cam, grid_size=0.1, sample_grid=0.02)
394 | 				for iI in range(len(IoU)):
395 | 					tmp = [IoU[iI]] + JIoUs[iI]
396 | 					all_IoU_dic[id_file][net].append(tmp)
397 | 
398 | 		if active_gt_idxs.size>0:
399 | 			pass
400 | 			#corner_totalVariances.append(get_corner_variances(label_data, frame, gt_idxs, points_cam))
401 | 			#plot_multiview_label(label_data, img_dir, points_cam, frame, gt_idxs, output_JIoU_dir, IoU_dic=all_IoU_dic[id_file], pd_idxs=pd_idxs, pred_data=pred_datas[len(pred_datas)-1])
402 | 	output_JIoU_dir = output_dir + 'summary/uncertaintyV3_for_labelwellness_0.25_0.8_deg1/'
403 | 	write_JIoU(output_JIoU_dir, networks, frames, all_gt_idxs, all_pd_idxs, all_IoU_dic)
404 | 	#write_corner_variances(output_root, corner_totalVariances)
405 | 
406 | 	'''
407 | 	#count common fn's and plot
408 | 	difficulty = 'HARD'
409 | 	view = 'G'
410 | 	num_fns = np.zeros(num_net)
411 | 	num_common_fns = 0
412 | 	regression_error_idxs = []
413 | 	output_common_fn_dir = output_dir + 'common_fns/'
414 | 	os.makedirs(output_common_fn_dir, exist_ok=True)
415 | 	frame_idxs_pairs = []
416 | 	iou_hists = {inet: [] for inet in range(len(networks))}
417 | 	print('find common false negatives from {} and view {}'.format(difficulty,view))
418 | 	for file in tqdm(file_lists):
419 | 		frame = int(file.split('.')[0])
420 | 		points_cam = read_points_cam(lidar_dir, frame, label_data)
421 | 		fns = []
422 | 		for inet, net in enumerate(networks):
423 | 			fns.append(hack_datas[inet][difficulty]['gt'][view]['fn'][frame])
424 | 		fns = np.array(fns)==1
425 | 		common_fns = fns[0,:]
426 | 		for inet in range(1,num_net):
427 | 			common_fns = np.logical_and(common_fns,fns[inet,:])
428 | 		common_fns_idx = np.argwhere(common_fns)
429 | 		num_common_fns += common_fns_idx.size
430 | 		if common_fns_idx.size > 0:
431 | 			idxs = [common_fns_idx.item(icommon) for icommon in range(common_fns_idx.size)]
432 | 			#plot_multiview_label(label_data, img_dir, points_cam, frame, idxs, output_common_fn_dir)
433 | 
434 | 			#find overlaps and plot historgram
435 | 			for inet in range(len(networks)):
436 | 				ass_gts = hack_datas[inet][difficulty]['det'][view]['gt'][frame]
437 | 				for idx in idxs:
438 | 					max_iou = 0.0
439 | 					for j,ass_gt in enumerate(ass_gts):
440 | 						if idx == ass_gt:
441 | 							max_iou = max(max_iou, hack_datas[inet][difficulty]['det'][view]['iou'][frame][j])
442 | 					if max_iou>0.1 and max_iou<0.70001: #DEBUG
443 | 						regression_error_idxs.append([frame,idx, max_iou])
444 | 						print('DEBUG: #regression error: ',frame,idx, max_iou)
445 | 					iou_hists[inet].append([frame,idx,max_iou])
446 | 			#count the number of points inside the gt box
447 | 			frame_idxs_pairs.append((frame,idxs))
448 | 	output_common_fn_stats_dir = output_common_fn_dir+'stats/'
449 | 	plot_histogram_network_wise(output_common_fn_stats_dir, 'iou_histogram.png', networks, iou_hists, 2, x_label='IoU', y_label='number of false negatives',n_bins=50)
450 | 	plot_npoint_histogram_label(label_data, points_cam, frame_idxs_pairs, output_common_fn_stats_dir)
451 | 	write_stats_label(label_data, frame_idxs_pairs, output_common_fn_stats_dir, description="false positive stats: ", reg_err_idxs=regression_error_idxs)
452 | 	'''
453 | 
454 | 
455 | 
456 | 
457 | if __name__ == '__main__':
458 | 	main()
459 | 


--------------------------------------------------------------------------------