From ba8c28238433922d6699abfd82ee19875bb180e5 Mon Sep 17 00:00:00 2001
From: Davide Sapienza <sapienza.dav@gmail.com>
Date: Wed, 27 May 2020 18:03:09 +0200
Subject: [PATCH] Add 3D CenterNet detection class

Signed-off-by: Davide Sapienza <sapienza.dav@gmail.com>
---
 include/tkDNN/CenternetDetection3D.h | 100 ++++++
 include/tkDNN/DetectionNN3D.h        | 136 +++++++
 include/tkDNN/Layer.h                |  10 +
 include/tkDNN/kernelsThrust.h        |   2 +
 src/CenternetDetection3D.cpp         | 517 +++++++++++++++++++++++++++
 src/kernels/postprocessing.cu        |  14 +
 6 files changed, 779 insertions(+)
 create mode 100644 include/tkDNN/CenternetDetection3D.h
 create mode 100644 include/tkDNN/DetectionNN3D.h
 create mode 100644 src/CenternetDetection3D.cpp

diff --git a/include/tkDNN/CenternetDetection3D.h b/include/tkDNN/CenternetDetection3D.h
new file mode 100644
index 0000000..7b9ea7a
--- /dev/null
+++ b/include/tkDNN/CenternetDetection3D.h
@@ -0,0 +1,100 @@
+#ifndef CENTERNETDETECTION3D_H
+#define CENTERNETDETECTION3D_H
+
+#include "kernels.h"
+#include <opencv2/videoio.hpp>
+#include "opencv2/opencv.hpp"
+#include <time.h>
+#include <vector>
+#include <numeric>      // std::iota
+#include <algorithm>    // std::sort
+
+#include "DetectionNN3D.h"
+
+#include "kernelsThrust.h"
+
+
+namespace tk { namespace dnn { 
+
+class CenternetDetection3D : public DetectionNN3D
+{
+private:
+    tk::dnn::dataDim_t dim;
+    tk::dnn::dataDim_t dim2;
+    tk::dnn::dataDim_t dim_hm; 
+    tk::dnn::dataDim_t dim_wh; 
+    tk::dnn::dataDim_t dim_reg;
+    tk::dnn::dataDim_t dim_dep;
+    tk::dnn::dataDim_t dim_rot;
+    tk::dnn::dataDim_t dim_dim;
+    float *topk_scores;
+    int *topk_inds_;
+    float *topk_ys_;
+    float *topk_xs_;
+    int *ids_d, *ids_;
+
+    float *ones;
+
+    float *scores, *scores_d;
+    int *clses, *clses_d; 
+    int *topk_inds_d;
+    float *topk_ys_d; 
+    float *topk_xs_d;
+    int *inttopk_xs_d, *inttopk_ys_d;
+
+    float *xs, *ys;
+
+    float *dep, *rot, *dim_, *wh;
+    float *dep_d, *rot_d, *dim_d, *wh_d;
+    
+    float *target_coords;
+
+    #ifdef OPENCV_CUDACONTRIB
+        float *mean_d;
+        float *stddev_d;
+    #else
+        cv::Vec<float, 3> mean;
+        cv::Vec<float, 3> stddev;
+        dnnType *input;
+    #endif
+    cv::Mat r;
+    cv::Mat calibs;
+    float *d_ptrs;
+
+    cv::Mat src;
+    cv::Mat dst;
+    cv::Mat dst2;  
+    cv::Mat trans, trans2;
+    //processing
+    int K = 100;
+    int width = 128;//56;        // TODO
+
+    // pointer used in the kernels
+    float *src_out;
+    int *ids_out;
+
+    struct threshold op;
+    float peakThreshold = 0.2;
+    float centerThreshold = 0.3; //default 0.5
+    cv::Mat corners, pts3DHomo;  
+
+    std::vector<box3D> detected3D;
+    std::vector<int>cls3D;
+    std::vector<std::vector<int>> face_id;
+
+public:
+    CenternetDetection3D() {};
+    ~CenternetDetection3D() {}; 
+
+    bool init(const std::string& tensor_path, const int n_classes=3);
+    void preprocess(cv::Mat &frame);
+    void postprocess();
+    cv::Mat draw(cv::Mat &frame);
+};
+
+
+} // namespace dnn
+} // namespace tk
+
+
+#endif /*CENTERNETDETECTION_H*/
\ No newline at end of file
diff --git a/include/tkDNN/DetectionNN3D.h b/include/tkDNN/DetectionNN3D.h
new file mode 100644
index 0000000..111d7e2
--- /dev/null
+++ b/include/tkDNN/DetectionNN3D.h
@@ -0,0 +1,136 @@
+#ifndef DETECTIONNN3D_H
+#define DETECTIONNN3D_H
+
+#include <iostream>
+#include <signal.h>
+#include <stdlib.h>    
+#include <unistd.h>
+#include <mutex>
+#include "utils.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "tkdnn.h"
+
+// #define OPENCV_CUDACONTRIB //if OPENCV has been compiled with CUDA and contrib.
+
+#ifdef OPENCV_CUDACONTRIB
+#include <opencv2/cudawarping.hpp>
+#include <opencv2/cudaarithm.hpp>
+#endif
+
+
+namespace tk { namespace dnn {
+
+class DetectionNN3D {
+
+    protected:
+        tk::dnn::NetworkRT *netRT = nullptr;
+        dnnType *input_d;
+
+        cv::Size originalSize;
+
+        cv::Scalar colors[256];
+
+#ifdef OPENCV_CUDACONTRIB
+        cv::cuda::GpuMat bgr[3];
+        cv::cuda::GpuMat imagePreproc;
+#else
+        cv::Mat bgr[3];
+        cv::Mat imagePreproc;
+        dnnType *input;
+#endif
+
+        /**
+         * This method preprocess the image, before feeding it to the NN.
+         *
+         * @param frame original frame to adapt for inference.
+         */
+        virtual void preprocess(cv::Mat &frame) = 0;
+
+        /**
+         * This method postprocess the output of the NN to obtain the correct 
+         * boundig boxes. 
+         * 
+         */
+        virtual void postprocess() = 0;
+
+    public:
+        int classes = 0;
+        float confThreshold = 0.3; /*threshold on the confidence of the boxes*/
+
+        std::vector<tk::dnn::box> detected; /*bounding boxes in output*/
+        std::vector<double> stats; /*keeps track of inference times (ms)*/
+        std::vector<std::string> classesNames;
+
+        DetectionNN3D() {};
+        ~DetectionNN3D(){};
+
+        /**
+         * Method used to inialize the class, allocate memory and compute 
+         * needed data.
+         * 
+         * @param tensor_path path to the rt file og the NN.
+         * @param n_classes number of classes for the given dataset.
+         * @return true if everything is correct, false otherwise.
+         */
+        virtual bool init(const std::string& tensor_path, const int n_classes=3) = 0;
+
+        /**
+         * Method to draw boundixg boxes and labels on a frame.
+         * 
+         * @param frame orginal frame to draw bounding box on.
+         * @return frame with boundig boxes.
+         */
+        virtual cv::Mat draw(cv::Mat &frame){};
+
+        /**
+         * This method performs the whole detection of the NN.
+         * 
+         * @param frame frame to run detection on.
+         * @param save_times if set to true, preprocess, inference and postprocess times 
+         *        are saved on a csv file, otherwise not.
+         * @param times pointer to the output stream where to write times
+         */
+        void update(cv::Mat &frame, bool save_times=false, std::ofstream *times=nullptr){
+            if(!frame.data)
+                FatalError("No image data feed to detection");
+
+            if(save_times && times==nullptr)
+                FatalError("save_times set to true, but no valid ofstream given");
+
+            originalSize = frame.size();
+            printCenteredTitle(" TENSORRT detection ", '=', 30); 
+            {
+                TIMER_START
+                preprocess(frame);
+                TIMER_STOP
+                if(save_times) *times<<t_ns<<";";
+            }
+
+            //do inference
+            tk::dnn::dataDim_t dim = netRT->input_dim;
+            {
+                dim.print();
+                TIMER_START
+                netRT->infer(dim, input_d);
+                TIMER_STOP
+                dim.print();
+                stats.push_back(t_ns);
+                if(save_times) *times<<t_ns<<";";
+            }
+
+            {
+                TIMER_START
+                postprocess();
+                TIMER_STOP
+                if(save_times) *times<<t_ns<<"\n";
+            }
+        }              
+};
+
+}}
+
+#endif /* DETECTIONNN3D_H*/
diff --git a/include/tkDNN/Layer.h b/include/tkDNN/Layer.h
index c38b5bc..49a70be 100644
--- a/include/tkDNN/Layer.h
+++ b/include/tkDNN/Layer.h
@@ -546,6 +546,16 @@ struct sortable_bbox {
     int cl;
     float **probs;
 };
+struct box3D {
+    int cl;
+    std::vector<float> corners;
+    float prob;
+
+    void print() 
+    {
+        std::cout<<"\tcl: "<<cl<<"\tprob: "<<prob<<"\tshape corners: "<<corners.size()<<std::endl;
+    }
+};
 
 /**
     Yolo3 layer
diff --git a/include/tkDNN/kernelsThrust.h b/include/tkDNN/kernelsThrust.h
index a7b32e9..215befa 100644
--- a/include/tkDNN/kernelsThrust.h
+++ b/include/tkDNN/kernelsThrust.h
@@ -29,11 +29,13 @@ void topk(dnnType *src_begin, int *idsrc, int K, float *topk_scores,
             int *topk_inds, float *topk_ys, float *topk_xs);
 // void sortAndTopKonDevice(dnnType *src_begin, int *idsrc, float *topk_scores, int *topk_inds, float *topk_ys, float *topk_xs, const int size, const int K, const int n_classes);            
 void normalize(float *bgr, const int ch, const int h, const int w, const float *mean, const float *stddev);
+void transformDep(float *src_begin, float *src_end, float *dst_begin, float *dst_end);
 void subtractWithThreshold(dnnType *src_begin, dnnType *src_end, dnnType *src2_begin, dnnType *src_out, struct threshold op);
 void topKxyclasses(int *ids_begin, int *ids_end, const int K, const int size, const int wh, int *clses, int *xs, int *ys);
 void topKxyAddOffset(int * ids_begin, const int K, const int size, int *intxs_begin, int *intys_begin, 
                      float *xs_begin, float *ys_begin, dnnType *src_begin, float *src_out, int *ids_out);
 void bboxes(int * ids_begin, const int K, const int size, float *xs_begin, float *ys_begin, 
             dnnType *src_begin, float *bbx0, float *bbx1, float *bby0, float *bby1, float *src_out, int *ids_out);
+void getRecordsFromTopKId(int * ids_begin, const int K, const int ch, const int size, dnnType *src_begin, float *src_out, int *ids_out);
 
 #endif //KERNELSTHRUST_H
\ No newline at end of file
diff --git a/src/CenternetDetection3D.cpp b/src/CenternetDetection3D.cpp
new file mode 100644
index 0000000..1803381
--- /dev/null
+++ b/src/CenternetDetection3D.cpp
@@ -0,0 +1,517 @@
+#include "CenternetDetection3D.h"
+
+
+namespace tk { namespace dnn {
+
+bool CenternetDetection3D::init(const std::string& tensor_path, const int n_classes){
+    std::cout<<(tensor_path).c_str()<<"\n";
+    netRT = new tk::dnn::NetworkRT(NULL, (tensor_path).c_str() );
+    classes = n_classes;
+
+    dim = netRT->input_dim;
+
+    const char *kitti_class_name[] = {
+            "person", "car", "bicycle"};
+    classesNames = std::vector<std::string>(kitti_class_name, std::end( kitti_class_name));
+    
+    for(int c=0; c<classes; c++) {
+        int offset = c*123457 % classes;
+        float r = getColor(2, offset, classes);
+        float g = getColor(1, offset, classes);
+        float b = getColor(0, offset, classes);
+        colors[c] = cv::Scalar(int(255.0*b), int(255.0*g), int(255.0*r));
+    }
+    
+    src = cv::Mat(cv::Size(2,3), CV_32F);
+    dst = cv::Mat(cv::Size(2,3), CV_32F);
+    dst2 = cv::Mat(cv::Size(2,3), CV_32F);
+    trans = cv::Mat(cv::Size(3,2), CV_32F);
+    trans2 = cv::Mat(cv::Size(3,2), CV_32F);
+
+    checkCuda(cudaMalloc(&input_d, sizeof(dnnType)*netRT->input_dim.tot()));
+
+    dim_hm = tk::dnn::dataDim_t(1, 3, 128, 128, 1);
+    dim_wh = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
+    dim_reg = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
+    dim_dep = tk::dnn::dataDim_t(1, 1, 128, 128, 1);
+    dim_rot = tk::dnn::dataDim_t(1, 8, 128, 128, 1);
+    dim_dim = tk::dnn::dataDim_t(1, 3, 128, 128, 1);
+
+    checkCuda( cudaMalloc(&topk_scores, dim_hm.c * K *sizeof(float)) );
+    checkCuda( cudaMalloc(&topk_inds_, dim_hm.c * K *sizeof(int)) );      
+    checkCuda( cudaMalloc(&topk_ys_, dim_hm.c * K *sizeof(float)) );      
+    checkCuda( cudaMalloc(&topk_xs_, dim_hm.c * K *sizeof(float)) );    
+    checkCuda( cudaMalloc(&ids_d, dim_hm.c * dim_hm.h * dim_hm.w*sizeof(int)) );
+    checkCuda( cudaMallocHost(&ids_, dim_hm.c * dim_hm.h * dim_hm.w*sizeof(int)) );
+    for(int i =0; i<dim_hm.c * dim_hm.h * dim_hm.w; i++){
+        ids_[i] = i;
+    }
+    
+    checkCuda( cudaMalloc(&ones, dim_dep.c * dim_dep.h * dim_dep.w * sizeof(float)) );
+    float *ones_h;
+    checkCuda( cudaMallocHost(&ones_h, dim_dep.c * dim_dep.h * dim_dep.w * sizeof(float)) );
+    for(int i=0; i<dim_dep.c * dim_dep.h * dim_dep.w; i++)
+        ones_h[i]=1.0f;
+    checkCuda( cudaMemcpy(ones, ones_h, dim_dep.c * dim_dep.h * dim_dep.w * sizeof(float), cudaMemcpyHostToDevice) );
+    checkCuda( cudaFreeHost(ones_h) );
+
+    checkCuda( cudaMallocHost(&scores, K *sizeof(float)) );
+    checkCuda( cudaMalloc(&scores_d, K *sizeof(float)) );
+
+    checkCuda( cudaMallocHost(&clses, K *sizeof(int)) );
+    checkCuda( cudaMalloc(&clses_d, K *sizeof(int)) );
+
+    checkCuda( cudaMalloc(&topk_inds_d, K *sizeof(int)) );
+    checkCuda( cudaMalloc(&topk_ys_d, K *sizeof(float)) );     
+    checkCuda( cudaMalloc(&topk_xs_d, K *sizeof(float)) ); 
+    checkCuda( cudaMalloc(&inttopk_ys_d, K *sizeof(int)) );
+    checkCuda( cudaMalloc(&inttopk_xs_d, K *sizeof(int)) );
+
+    checkCuda( cudaMallocHost(&xs, K  * sizeof(float)) ); 
+    checkCuda( cudaMallocHost(&ys, K  * sizeof(float)) ); 
+
+    checkCuda( cudaMallocHost(&dep, K * dim_dep.c * sizeof(float)) ); 
+    checkCuda( cudaMallocHost(&rot, K * dim_rot.c * sizeof(float)) ); 
+    checkCuda( cudaMallocHost(&dim_, K * dim_dim.c * sizeof(float)) ); 
+    checkCuda( cudaMallocHost(&wh, K * dim_wh.c * sizeof(float)) ); 
+    checkCuda( cudaMalloc(&dep_d, K * dim_dep.c * sizeof(float)) ); 
+    checkCuda( cudaMalloc(&rot_d, K * dim_rot.c * sizeof(float)) ); 
+    checkCuda( cudaMalloc(&dim_d, K * dim_dim.c * sizeof(float)) ); 
+    checkCuda( cudaMalloc(&wh_d, K * dim_wh.c * sizeof(float)) ); 
+
+    checkCuda( cudaMallocHost(&target_coords, 4 * K *sizeof(float)) );
+
+#ifdef OPENCV_CUDACONTRIB
+
+    checkCuda( cudaMalloc(&mean_d, 3 * sizeof(float)) );
+    checkCuda( cudaMalloc(&stddev_d, 3 * sizeof(float)) );
+    float mean[3] = {0.485, 0.456, 0.406};
+    float stddev[3] = {0.229, 0.224, 0.225};
+    
+    checkCuda(cudaMemcpy(mean_d, mean, 3*sizeof(float), cudaMemcpyHostToDevice));
+    checkCuda(cudaMemcpy(stddev_d, stddev, 3*sizeof(float), cudaMemcpyHostToDevice));
+#else
+    checkCuda(cudaMallocHost(&input, sizeof(dnnType)*netRT->input_dim.tot()));
+    mean << 0.485, 0.456, 0.406;
+    stddev << 0.229, 0.224, 0.225;
+#endif
+
+    calibs = cv::Mat(cv::Size(4,3), CV_32F);
+    calibs.at<float>(0,0) = 707.0493;
+    calibs.at<float>(0,1) = 0.0;
+    calibs.at<float>(0,2) = 604.0814; 
+    calibs.at<float>(0,3) = 45.75831;
+    calibs.at<float>(1,0) = 0.0;
+    calibs.at<float>(1,1) = 707.0493;
+    calibs.at<float>(1,2) = 180.5066;
+    calibs.at<float>(1,3) = -0.3454157;
+    calibs.at<float>(2,0) = 0.0;
+    calibs.at<float>(2,1) = 0.0;
+    calibs.at<float>(2,2) = 1.0;
+    calibs.at<float>(2,3) = 0.004981016;
+
+    r = cv::Mat(cv::Size(3,3), CV_32F);
+    r.at<float>(0,1) = 0.0;
+    r.at<float>(1,0) = 0.0;
+    r.at<float>(1,1) = 1.0;
+    r.at<float>(1,2) = 0.0;
+    r.at<float>(2,1) = 0.0;
+                        
+    corners = cv::Mat(cv::Size(8,3), CV_32F);
+    corners.at<float>(1,0) = 0.0;
+    corners.at<float>(1,1) = 0.0;
+    corners.at<float>(1,2) = 0.0;
+    corners.at<float>(1,3) = 0.0;
+
+    pts3DHomo = cv::Mat(cv::Size(8,4), CV_32F);
+    pts3DHomo.at<float>(3,0) = 1.0;
+    pts3DHomo.at<float>(3,1) = 1.0;
+    pts3DHomo.at<float>(3,2) = 1.0;
+    pts3DHomo.at<float>(3,3) = 1.0;
+    pts3DHomo.at<float>(3,4) = 1.0;
+    pts3DHomo.at<float>(3,5) = 1.0;
+    pts3DHomo.at<float>(3,6) = 1.0;
+    pts3DHomo.at<float>(3,7) = 1.0;
+    
+    checkCuda( cudaMalloc(&d_ptrs, dim.c * dim.h*dim.w * sizeof(float)) );
+
+    // Alloc array used in the kernel 
+    checkCuda( cudaMalloc(&src_out, K *sizeof(float)) );
+    checkCuda( cudaMalloc(&ids_out, K *sizeof(int)) );
+
+    dst2.at<float>(0,0)=width * 0.5;
+    dst2.at<float>(0,1)=width * 0.5;
+    dst2.at<float>(1,0)=width * 0.5;
+    dst2.at<float>(1,1)=width * 0.5 +  width * -0.5; 
+    
+    dst2.at<float>(2,0)=dst2.at<float>(1,0) + (-dst2.at<float>(0,1)+dst2.at<float>(1,1) );
+    dst2.at<float>(2,1)=dst2.at<float>(1,1) + (dst2.at<float>(0,0)-dst2.at<float>(1,0) );
+
+    face_id.push_back({0,1,5,4});
+    face_id.push_back({1,2,6, 5});
+    face_id.push_back({2,3,7,6});
+    face_id.push_back({3,0,4,7});
+    // ([[0,1,5,4], [1,2,6, 5], [2,3,7,6], [3,0,4,7]]);
+}
+
+void CenternetDetection3D::preprocess(cv::Mat &frame){
+     // -----------------------------------pre-process ------------------------------------------
+    
+    // auto start_t = std::chrono::steady_clock::now();
+    // auto step_t = std::chrono::steady_clock::now();
+    // auto end_t = std::chrono::steady_clock::now();
+    cv::Size sz = originalSize;
+    // std::cout<<"image: "<<sz.width<<", "<<sz.height<<std::endl;
+    cv::Size sz_old;
+    float scale = 1.0;
+    float new_height = sz.height * scale;
+    float new_width = sz.width * scale;
+    if(sz.height != sz_old.height && sz.width != sz_old.width){
+        float c[] = {new_width / 2.0f, new_height /2.0f};
+        float s[] = {new_width, new_height};
+        // ----------- get_affine_transform
+        // rot_rad = pi * 0 / 100 --> 0
+        
+        src.at<float>(0,0)=c[0];
+        src.at<float>(0,1)=c[1];
+        src.at<float>(1,0)=c[0];
+        src.at<float>(1,1)=c[1] + s[0] * -0.5;
+        dst.at<float>(0,0)=netRT->input_dim.w * 0.5;
+        dst.at<float>(0,1)=netRT->input_dim.h * 0.5;
+        dst.at<float>(1,0)=netRT->input_dim.w * 0.5;
+        dst.at<float>(1,1)=netRT->input_dim.h * 0.5 +  netRT->input_dim.w * -0.5; 
+        
+        src.at<float>(2,0)=src.at<float>(1,0) + (-src.at<float>(0,1)+src.at<float>(1,1) );
+        src.at<float>(2,1)=src.at<float>(1,1) + (src.at<float>(0,0)-src.at<float>(1,0) );
+        dst.at<float>(2,0)=dst.at<float>(1,0) + (-dst.at<float>(0,1)+dst.at<float>(1,1) );
+        dst.at<float>(2,1)=dst.at<float>(1,1) + (dst.at<float>(0,0)-dst.at<float>(1,0) );
+
+        trans = cv::getAffineTransform( src, dst );
+        // end_t = std::chrono::steady_clock::now();
+        // std::cout << " TIME gett affine trans: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+        // step_t = end_t;
+        
+        trans2 = cv::getAffineTransform( dst2, src );
+        // end_t = std::chrono::steady_clock::now();
+        // std::cout << " TIME getAffineTrans 2: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+        // step_t = end_t;
+    }
+    sz_old = sz;
+#ifdef OPENCV_CUDACONTRIB
+    std::cout<<"OPENCV CPMTROB\n";
+    cv::cuda::GpuMat im_Orig; 
+    cv::cuda::GpuMat imageF1_d, imageF2_d;
+        
+    im_Orig = cv::cuda::GpuMat(frame);
+    // cv::cuda::resize (im_Orig, imageF1_d, cv::Size(new_width, new_height)); 
+    imageF1_d = im_Orig;
+    checkCuda( cudaDeviceSynchronize() );
+    
+    sz = imageF1_d.size();
+    // std::cout<<"size: "<<sz.height<<" "<<sz.width<<" - "<<std::endl;
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME resize: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+    
+    cv::cuda::warpAffine(imageF1_d, imageF2_d, trans, cv::Size(netRT->input_dim.w, netRT->input_dim.h), cv::INTER_LINEAR );
+    checkCuda( cudaDeviceSynchronize() );
+    
+    imageF2_d.convertTo(imageF1_d, CV_32FC3, 1/255.0); 
+    checkCuda( cudaDeviceSynchronize() );
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME convert: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+    
+    dim2 = dim;
+    cv::cuda::GpuMat bgr[3]; 
+    cv::cuda::split(imageF1_d,bgr);//split source
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME split: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+
+    for(int i=0; i<dim.c; i++)
+        checkCuda( cudaMemcpy(d_ptrs + i*dim.h * dim.w, (float*)bgr[i].data, dim.h * dim.w * sizeof(float), cudaMemcpyDeviceToDevice) );
+        
+    normalize(d_ptrs, dim.c, dim.h, dim.w, mean_d, stddev_d);
+    
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME normalize: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+
+    checkCuda(cudaMemcpy(input_d, d_ptrs, dim2.tot()*sizeof(dnnType), cudaMemcpyDeviceToDevice));
+    
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME Memcpy to input_d: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+#else
+    std::cout<<"NO OPENCV CPMTROB\n";
+    cv::Mat imageF;
+    // resize(frame, imageF, cv::Size(new_width, new_height));
+    imageF = frame;
+    sz = imageF.size();
+    // std::cout<<"size: "<<sz.height<<" "<<sz.width<<" - "<<std::endl;
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME resize: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+
+    cv::Mat trans = cv::getAffineTransform( src, dst );
+    cv::warpAffine(imageF, imageF, trans, cv::Size(netRT->input_dim.w, netRT->input_dim.h), cv::INTER_LINEAR );
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME warpAffine: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+
+    sz = imageF.size();
+    // std::cout<<"size: "<<sz.height<<" "<<sz.width<<" - "<<std::endl;
+    imageF.convertTo(imageF, CV_32FC3, 1/255.0); 
+    // end_t = std::chrono::steady_clock::now();
+    // std::cout << " TIME convertto: " << std::chrono::duration_cast<std::chrono:: microseconds>(end_t - step_t).count() << "  us" << std::endl;
+    // step_t = end_t;
+    dim2 = dim;
+    //split channels
+    cv::Mat bgr[3]; 
+    cv::split(imageF,bgr);//split source
+    for(int i=0; i<3; i++){
+        bgr[i] = bgr[i] - mean[i];
+        bgr[i] = bgr[i] / stddev[i];
+    }
+
+    //write channels
+    for(int i=0; i<dim2.c; i++) {
+        int idx = i*imageF.rows*imageF.cols;
+        int ch = dim2.c-3 +i;
+        // std::cout<<"i: "<<i<<", idx: "<<idx<<", ch: "<<ch<<std::endl;
+        memcpy((void*)&input[idx], (void*)bgr[ch].data, imageF.rows*imageF.cols*sizeof(dnnType));
+    }
+    checkCuda(cudaMemcpyAsync(input_d, input, dim2.tot()*sizeof(dnnType), cudaMemcpyHostToDevice));
+#endif
+}
+
+void CenternetDetection3D::postprocess(){
+    dnnType *rt_out[7];
+    rt_out[0] = (dnnType *)netRT->buffersRT[1];
+    rt_out[1] = (dnnType *)netRT->buffersRT[2];
+    rt_out[2] = (dnnType *)netRT->buffersRT[3]; 
+    rt_out[3] = (dnnType *)netRT->buffersRT[4]; 
+    rt_out[4] = (dnnType *)netRT->buffersRT[5]; 
+    rt_out[5] = (dnnType *)netRT->buffersRT[6]; 
+    rt_out[6] = (dnnType *)netRT->buffersRT[7]; 
+    
+    // ------------------------------------ process --------------------------------------------
+    activationSIGMOIDForward(rt_out[0], rt_out[0], dim_hm.tot());
+    checkCuda( cudaDeviceSynchronize() );
+
+    // output['dep'] = 1. / (output['dep'].sigmoid() + 1e-6) - 1.
+    activationSIGMOIDForward(rt_out[4], rt_out[4], dim_dep.tot());
+    checkCuda( cudaDeviceSynchronize() );
+    transformDep(ones, ones + dim_dep.tot(), rt_out[4], rt_out[4] + dim_dep.tot());
+    checkCuda( cudaDeviceSynchronize() );
+
+    subtractWithThreshold(rt_out[0], rt_out[0] + dim_hm.tot(), rt_out[1], rt_out[0], op);
+
+    // ----------- nms end
+    // ----------- topk  
+      
+    if(K > dim_hm.h * dim_hm.w){
+        printf ("Error topk (K is too large)\n");
+        return;
+    }
+    
+    checkCuda( cudaMemcpy(ids_d, ids_, dim_hm.c * dim_hm.h * dim_hm.w*sizeof(int), cudaMemcpyHostToDevice) );    
+
+    sort(rt_out[0],rt_out[0]+dim_hm.tot(),ids_d);
+    checkCuda( cudaDeviceSynchronize() );   
+
+    topk(rt_out[0], ids_d, K, scores_d, topk_inds_d, topk_ys_d, topk_xs_d);
+    checkCuda( cudaDeviceSynchronize() );    
+
+    checkCuda( cudaMemcpy(scores, scores_d, K *sizeof(float), cudaMemcpyDeviceToHost) );
+
+    topKxyclasses(topk_inds_d, topk_inds_d+K, K, width, dim_hm.w*dim_hm.h, clses_d, inttopk_xs_d, inttopk_ys_d);
+
+    checkCuda( cudaMemcpy(topk_xs_d, (float *)inttopk_xs_d, K*sizeof(float), cudaMemcpyDeviceToDevice) );
+    checkCuda( cudaMemcpy(topk_ys_d, (float *)inttopk_ys_d, K*sizeof(float), cudaMemcpyDeviceToDevice) );
+    
+    checkCuda( cudaMemcpy(clses, clses_d, K*sizeof(int), cudaMemcpyDeviceToHost) );
+
+    // ----------- topk end 
+    
+    topKxyAddOffset(topk_inds_d, K, dim_reg.h*dim_reg.w, inttopk_xs_d, inttopk_ys_d, topk_xs_d, topk_ys_d, rt_out[3], src_out, ids_out);
+    // checkCuda( cudaDeviceSynchronize() );
+    
+   
+    getRecordsFromTopKId(topk_inds_d, K, dim_dep.c, dim_dep.h * dim_dep.w, rt_out[4], dep_d, ids_out);
+    checkCuda( cudaMemcpy(dep, dep_d, K * dim_dep.c * sizeof(float), cudaMemcpyDeviceToHost) );
+    
+    getRecordsFromTopKId(topk_inds_d, K, dim_rot.c, dim_rot.h * dim_rot.w, rt_out[5], rot_d, ids_out);
+    checkCuda( cudaMemcpy(rot, rot_d, K * dim_rot.c * sizeof(float), cudaMemcpyDeviceToHost) );
+    
+    getRecordsFromTopKId(topk_inds_d, K, dim_dim.c, dim_dim.h * dim_dim.w, rt_out[6], dim_d, ids_out);
+    checkCuda( cudaMemcpy(dim_, dim_d, K * dim_dim.c * sizeof(float), cudaMemcpyDeviceToHost) );
+
+    getRecordsFromTopKId(topk_inds_d, K, dim_wh.c, dim_wh.h * dim_wh.w, rt_out[2], wh_d, ids_out);
+    checkCuda( cudaMemcpy(wh, wh_d, K * dim_wh.c * sizeof(float), cudaMemcpyDeviceToHost) );
+   
+    checkCuda( cudaMemcpy(xs, topk_xs_d, K * sizeof(float), cudaMemcpyDeviceToHost) ); 
+    checkCuda( cudaMemcpy(ys, topk_ys_d, K * sizeof(float), cudaMemcpyDeviceToHost) ); 
+  
+    // ---------------------------------- post-process -----------------------------------------
+    
+    // ddd_post_process_2d
+    cv::Mat new_pt1(cv::Size(1,2), CV_32F);
+    cv::Mat new_pt2(cv::Size(1,2), CV_32F); 
+
+    for(int i = 0; i<K; i++){
+        new_pt1.at<float>(0,0)=static_cast<float>(trans2.at<double>(0,0))*xs[i] +
+                                static_cast<float>(trans2.at<double>(0,1))*ys[i] +
+                                static_cast<float>(trans2.at<double>(0,2))*1.0;
+        new_pt1.at<float>(0,1)=static_cast<float>(trans2.at<double>(1,0))*xs[i] +
+                                static_cast<float>(trans2.at<double>(1,1))*ys[i] +
+                                static_cast<float>(trans2.at<double>(1,2))*1.0;
+
+        new_pt2.at<float>(0,0)=static_cast<float>(trans2.at<double>(0,0))*wh[i] +
+                                static_cast<float>(trans2.at<double>(0,1))*wh[K+i] +
+                                static_cast<float>(trans2.at<double>(0,2))*1.0;
+        new_pt2.at<float>(0,1)=static_cast<float>(trans2.at<double>(1,0))*wh[i] +
+                                static_cast<float>(trans2.at<double>(1,1))*wh[K+i] +
+                                static_cast<float>(trans2.at<double>(1,2))*1.0;
+
+        target_coords[i*4] = new_pt1.at<float>(0,0);
+        target_coords[i*4+1] = new_pt1.at<float>(0,1);
+        target_coords[i*4+2] = new_pt2.at<float>(0,0);
+        target_coords[i*4+3] = new_pt2.at<float>(0,1);
+    }
+    
+    float alpha;
+    float x, y, z, rot_y;
+    detected3D.clear();
+    for(int i = 0; i<classes; i++){      
+        for(int j=0; j<K; j++){
+            if(clses[j] == i){
+                //get alpha
+                if(rot[1*K + j] > rot[5*K + j])
+                    alpha = std::atan2(rot[2*K + j], rot[3*K + j]) -0.5 * M_PI;
+                else
+                    alpha = std::atan2(rot[6*K + j], rot[7*K + j]) +0.5 * M_PI;  
+                
+                // unproject_2d_to_3d
+                z = dep[j] - calibs.at<float>(2,3);// z = depth - P[2, 3]
+                x = (target_coords[j*4] * dep[j] - calibs.at<float>(0,3) - calibs.at<float>(0,2) * z) / calibs.at<float>(0,0);
+                y = (target_coords[j*4+1] * dep[j] - calibs.at<float>(1,3) - calibs.at<float>(1,2) * z) / calibs.at<float>(1,1) + (dim_[j] / 2);
+                // alpha2rot_y
+                rot_y = (alpha + std::atan2(target_coords[j*4] - calibs.at<float>(0,2), calibs.at<float>(0,0)));
+                if(rot_y>M_PI)
+                    rot_y -= 2*M_PI;
+                if(rot_y<M_PI)
+                    rot_y += 2*M_PI;   
+
+                // if(scores[j] > peakThreshold) {
+                if(scores[j] > centerThreshold) {
+                    if(z>0) {
+                        // compute_box_3d
+                        r.at<float>(0,0) = std::cos(rot_y);
+                        r.at<float>(0,2) = std::sin(rot_y);
+                        r.at<float>(2,0) = -std::sin(rot_y);
+                        r.at<float>(2,2) = std::cos(rot_y);
+
+                        corners.at<float>(0,0) = dim_[2*K+j]/2;
+                        corners.at<float>(0,1) = dim_[2*K+j]/2;
+                        corners.at<float>(0,2) = -dim_[2*K+j]/2;
+                        corners.at<float>(0,3) = -dim_[2*K+j]/2;
+                        corners.at<float>(0,4) = dim_[2*K+j]/2;
+                        corners.at<float>(0,5) = dim_[2*K+j]/2;
+                        corners.at<float>(0,6) = -dim_[2*K+j]/2;
+                        corners.at<float>(0,7) = -dim_[2*K+j]/2;
+
+                        corners.at<float>(1,4) = -dim_[j];
+                        corners.at<float>(1,5) = -dim_[j];
+                        corners.at<float>(1,6) = -dim_[j];
+                        corners.at<float>(1,7) = -dim_[j];
+                        
+                        corners.at<float>(2,0) = dim_[K+j]/2;
+                        corners.at<float>(2,1) = -dim_[K+j]/2;
+                        corners.at<float>(2,2) = -dim_[K+j]/2;
+                        corners.at<float>(2,3) = dim_[K+j]/2;
+                        corners.at<float>(2,4) = dim_[K+j]/2;
+                        corners.at<float>(2,5) = -dim_[K+j]/2;
+                        corners.at<float>(2,6) = -dim_[K+j]/2;
+                        corners.at<float>(2,7) = dim_[K+j]/2;
+                        cv::Mat aus = r * corners;
+
+                        for(int k=0; k<8; k++) {
+                            aus.at<float>(0,k) += x;
+                            aus.at<float>(1,k) += y;
+                            aus.at<float>(2,k) += z;
+                        }
+                        // corners.copyTo(pts3DHomo(cv::Rect(0, 0, 8, 3)));
+                        for(int k1=0; k1<3; k1++) {
+                            for(int k2=0; k2<8; k2++)
+                                pts3DHomo.at<float>(k1,k2) = aus.at<float>(k1,k2); 
+                        }
+                        aus.release();
+                        aus = calibs * pts3DHomo;
+                        
+                        tk::dnn::box3D res;
+                        for(int k=0; k<8; k++) {
+                            res.corners.push_back(aus.at<float>(0,k) / aus.at<float>(2,k));
+                            res.corners.push_back(aus.at<float>(1,k) / aus.at<float>(2,k));
+                        }
+                        res.cl = i;
+                        res.prob = scores[j];
+                        res.print();
+                        detected3D.push_back(res);  
+                    }
+                }
+            }
+        }
+    }
+}
+
+cv::Mat CenternetDetection3D::draw(cv::Mat &frame) {
+    tk::dnn::box3D b;
+    int x0, w, x1, y0, h, y1;
+    int objClass;
+    std::string det_class;
+
+    int baseline = 0;
+    float font_scale = 0.5;
+    int thickness = 2;   
+    
+    // draw dets
+    for(int i=0; i<detected3D.size(); i++) {
+        b = detected3D[i];
+
+        for(int ind_f = 3; ind_f>=0; ind_f--) {
+            for(int j=0; j<4; j++) {
+                cv::line(frame, cv::Point(b.corners.at(face_id.at(ind_f).at(j) * 2), 
+                                          b.corners.at(face_id.at(ind_f).at(j) * 2 + 1)),
+                                cv::Point(b.corners.at(face_id.at(ind_f).at((j+1)%4) * 2), 
+                                          b.corners.at(face_id.at(ind_f).at((j+1)%4) * 2 + 1)), 
+                                colors[b.cl], 2);
+                if(ind_f == 0) {
+                    cv::line(frame, cv::Point(b.corners.at(face_id.at(ind_f).at(0) * 2), 
+                                              b.corners.at(face_id.at(ind_f).at(0) * 2 + 1)),
+                                    cv::Point(b.corners.at(face_id.at(ind_f).at(2) * 2), 
+                                              b.corners.at(face_id.at(ind_f).at(2) * 2 + 1)), colors[b.cl], 2);
+                    cv::line(frame, cv::Point(b.corners.at(face_id.at(ind_f).at(1) * 2), 
+                                              b.corners.at(face_id.at(ind_f).at(1) * 2 + 1)),
+                                    cv::Point(b.corners.at(face_id.at(ind_f).at(3) * 2), 
+                                              b.corners.at(face_id.at(ind_f).at(3) * 2 + 1)), colors[b.cl], 2);
+                }
+            }
+        }
+        // draw label
+        cv::Size text_size = getTextSize(classesNames[b.cl], cv::FONT_HERSHEY_SIMPLEX, font_scale, thickness, &baseline);
+        cv::rectangle(frame,  cv::Point(b.corners.at(face_id.at(0).at(0) * 2), 
+                                        b.corners.at(face_id.at(0).at(0) * 2 + 1)), 
+                              cv::Point((b.corners.at(face_id.at(0).at(0) * 2) + text_size.width - 2), 
+                                        (b.corners.at(face_id.at(0).at(0) * 2 + 1)) - text_size.height - 2), colors[b.cl], -1);                      
+        cv::putText(frame, classesNames[b.cl], cv::Point(b.corners.at(face_id.at(0).at(0) * 2),  
+                                                         b.corners.at(face_id.at(0).at(0) * 2 + 1) - (baseline / 2)), 
+                                               cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(255, 255, 255), thickness);
+    }
+    return frame;
+}
+
+}}
+
+
diff --git a/src/kernels/postprocessing.cu b/src/kernels/postprocessing.cu
index 3510200..88dbcc1 100644
--- a/src/kernels/postprocessing.cu
+++ b/src/kernels/postprocessing.cu
@@ -1,5 +1,11 @@
 #include "kernelsThrust.h"
 
+void transformDep(float *src_begin, float *src_end, float *dst_begin, float *dst_end) {
+    int e = exp(-6);
+    thrust::transform(thrust::device, dst_begin, dst_end, thrust::make_constant_iterator(e), dst_begin, thrust::plus<float>());
+    thrust::transform(thrust::device, src_begin, src_end, dst_begin, dst_begin, thrust::divides<float>());
+    thrust::transform(thrust::device, dst_begin, dst_end, thrust::make_constant_iterator(-1.0), dst_begin, thrust::plus<float>());
+}
 
 void subtractWithThreshold(dnnType *src_begin, dnnType *src_end, dnnType *src2_begin, dnnType *src_out, struct threshold op){
     thrust::transform(thrust::device, src_begin, src_end, src2_begin, src_out, op);
@@ -51,6 +57,14 @@ void topKxyAddOffset(int * ids_begin, const int K, const int size,
     thrust::transform(thrust::device, intys_begin, intys_begin + K, src_out, ys_begin, thrust::plus<float>());
 }
 
+void getRecordsFromTopKId(int * ids_begin, const int K, const int ch, const int size, dnnType *src_begin, float *src_out, int *ids_out) {
+    for(int i=0; i<ch; i++) {
+        // thrust::gather(thrust::device, ids_begin, ids_begin + K, src_begin, src_out);
+        thrust::transform(thrust::device, ids_begin, ids_begin + K, thrust::make_constant_iterator(i*size), ids_out, thrust::plus<int>());
+        thrust::gather(thrust::device, ids_out, ids_out + K, src_begin, src_out+i*K);
+    }
+}
+
 void bboxes(int * ids_begin, const int K, const int size, float *xs_begin, float *ys_begin, 
             dnnType *src_begin, float *bbx0, float *bbx1, float *bby0, float *bby1,
             float *src_out, int *ids_out){