undistort

CMakeLists.txt

@@ -38,8 +38,7 @@ link_directories(/root/TensorRT/TensorRT-8.2.3.0/lib)
     # include_directories(/usr/include/aarch64-linux-gnu)
 
 # yaml
-include_directories(/usr/local/include/yaml-cpp)
-# include_directories(/usr/include/yaml-cpp)
+find_package(yaml-cpp REQUIRED)
 
 # glog gflags
 # sudo apt-get install libgoogle-glog*

configs/tasks/multi/multi_yolov5.yaml

 
 name: CameraModel
 inputNames: [cvImage]
-outputNames: [out_instances, out_semantics, out_depths]
-# outputNames: [detections, segProtos, rawDepths, rawSemantics]
+outputNames: [out_instances, out_semantics, out_depths, undistortVisImage]
 
 units:
     -
@@ -10,8 +9,18 @@ units:
         inputNames: [cvImage]
         outputNames: [uint8Image]
     -
-        name: ResizeNorm
+        name: Undistort
         inputNames: [uint8Image]
+        outputNames: [undistortVisImage]
+        imageWidth: 1280
+        imageHeight: 720
+        # new_camera_matrix: [1, 0, 1, 
+        #                     0, 1, 1, 
+        #                     0, 0, 1]
+        IntrinsicPath: configs/tasks/multi/rgb_intrinsic.yaml
+    -
+        name: ResizeNorm
+        inputNames: [undistortVisImage]
         outputNames: [resizeNormImages]
         inputMean: [0, 0, 0]
         inputStd: [1, 1, 1]

configs/tasks/multi/rgb_intrinsic.yaml

+image_width: 1280
+image_height: 720
+camera_name: narrow_stereo/left
+camera_matrix:
+    rows: 3
+    cols: 3
+    data: [ 1.03990153e+03,    0.     ,  6.37825684e+02,
+              0.     ,  1.03990153e+03,  3.67552653e+02,
+              0.     ,    0.     ,    1.     ]
+camera_model: plumb_bob
+distortion_coefficients:
+    rows: 1
+    cols: 8
+    data: [-2.3540384769439697,
+          -57.614105224609375,
+          -0.0002989917411468923,
+          -0.0006685509579256177,
+          427.8574523925781,
+          -2.5440633296966553,
+          -55.27210998535156,
+          417.6490173339844]
+

src/libs/preprocessors/undistort.cpp

@@ -11,8 +11,8 @@ bool Undistort::Init(YAML::Node& node) {
         return false;
     };
 
-    width_ = node["imageWidth"].as<int>();
-    height_ = node["imageHeight"].as<int>();
+    dst_width_ = node["imageWidth"].as<int>();
+    dst_height_ = node["imageHeight"].as<int>();
     std::string IntrinsicPath,  ExtrinsicPath = "";
 
     IntrinsicPath = node["IntrinsicPath"].as<std::string>();
@@ -20,19 +20,31 @@ bool Undistort::Init(YAML::Node& node) {
         LOG_WARN << "Load intrinsic error: " << IntrinsicPath;
         return false;
     }
+    if(node["new_camera_matrix"].IsDefined() && ! node["new_camera_matrix"].IsNull()){
+        for(int i=0; i<9; i++){
+            new_camera_intrinsic(i) = node["new_camera_matrix"][i].as<float>();
+        }
+    }else{
+        new_camera_intrinsic = camera_intrinsic;
+        if(dst_height_!=src_height_ || dst_width_ != src_width_ ){
+            LOG_INFO<< "use same camera intrinsic to undistort image, but with different img_size. use the same size";
+            dst_height_ = src_height_;
+            dst_width_ = src_width_;
+        }
+    }
 
-    d_mapx_.Reshape({height_, width_});
-    d_mapy_.Reshape({height_, width_});
+    d_mapx_.Reshape({dst_height_, dst_width_});
+    d_mapy_.Reshape({dst_height_, dst_width_});
 
     Eigen::Matrix3f I;
     I << 1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f;
 
     InitUndistortRectifyMap(camera_intrinsic,
                             distortion_coefficients, I,
-                            camera_intrinsic, width_,
-                            height_, &d_mapx_, &d_mapy_);
+                            camera_intrinsic, dst_width_,
+                            dst_height_, &d_mapx_, &d_mapy_);
 
-    dst_img = std::make_shared<base::Image8U>(height_, width_, base::Color::BGR);
+    dst_img = std::make_shared<base::Image8U>(dst_height_, dst_width_, base::Color::BGR);
     auto output = std::make_shared<ios::CameraSrcOut>(dst_img);
     interfaces::SetIOPtr(outputNames[0], output);
     inited_ = true;
@@ -53,25 +65,19 @@ bool Undistort::loadIntrinsic(std::string& yaml_file){
         return false;
     }
     try{
-        if(node["image_width"].IsDefined() && !node["image_width"].IsNull()){
-            int width = node["image_width"].as<int>();
-            if(width_ != width){
-                LOG_INFO << "image width from cameraInfoConfig (" << width_<< ") and intrinsic (" << width << ") does not match, set with the intrinsic width. ";
-                width_ = width;
-            }
-        }
-        if(node["image_height"].IsDefined() && !node["image_height"].IsNull()){
-            int height = node["image_height"].as<int>();
-            if(height_ != height){
-                LOG_INFO << "image height from cameraInfoConfig (" << height_<< ") and intrinsic (" << height << ") does not match, set with the intrinsic height. ";
-                height_ = height;
-            }
-        }
+        src_width_ = node["image_width"].as<int>();
+        src_height_ = node["image_height"].as<int>();
+
         for(int i=0; i<9; i++){
             camera_intrinsic(i) = node["camera_matrix"]["data"][i].as<float>();
         }
-        for(int i=0; i<5; i++){
-            distortion_coefficients(i) = node["distortion_coefficients"]["data"][i].as<float>();
+        for(int i=0; i<14; i++){
+            if(i < node["distortion_coefficients"]["data"].size()){
+                distortion_coefficients(i) = node["distortion_coefficients"]["data"][i].as<float>();
+                // std::cout<<distortion_coefficients(i) <<std::endl;
+            }else{
+                distortion_coefficients(i) = 0;
+            }
         }
         if(node["rectification_matrix"].IsDefined() && node["rectification_matrix"]["data"].IsDefined() &&
             !node["rectification_matrix"]["data"].IsNull()
@@ -98,7 +104,7 @@ bool Undistort::loadIntrinsic(std::string& yaml_file){
 
 void Undistort::InitUndistortRectifyMap(
     const Eigen::Matrix3f &camera_model,
-    const Eigen::Matrix<float, 1, 5>& distortion, const Eigen::Matrix3f &R,
+    const Eigen::Matrix<float, 1, 14>& distortion, const Eigen::Matrix3f &R,
     const Eigen::Matrix3f &new_camera_model, int width, int height,
     base::Blob<float> *d_mapx, base::Blob<float> *d_mapy) {
     float fx = camera_model(0, 0);
@@ -114,26 +120,38 @@ void Undistort::InitUndistortRectifyMap(
     float p1 = distortion(0, 2);
     float p2 = distortion(0, 3);
     float k3 = distortion(0, 4);
+    float k4 = distortion(0, 5);
+    float k5 = distortion(0, 6);
+    float k6 = distortion(0, 7);
+    float s1 = distortion(0, 8);
+    float s2 = distortion(0, 9);
+    float s3 = distortion(0, 10);
+    float s4 = distortion(0, 11);
+
     Eigen::Matrix3f Rinv = R.inverse();
 
-    for (int v = 0; v < height_; ++v) {
+    for (int v = 0; v < height; ++v) {
         float *x_ptr = d_mapx->mutable_cpu_data() + d_mapx->offset(v);
         float *y_ptr = d_mapy->mutable_cpu_data() + d_mapy->offset(v);
-        for (int u = 0; u < width_; ++u) {
-        Eigen::Matrix<float, 3, 1> xy1;
-        xy1 << (static_cast<float>(u) - ncx) / nfx,
-            (static_cast<float>(v) - ncy) / nfy, 1;
-        auto XYW = Rinv * xy1;
-        double nx = XYW(0, 0) / XYW(2, 0);
-        double ny = XYW(1, 0) / XYW(2, 0);
-        double r_square = nx * nx + ny * ny;
-        double scale = (1 + r_square * (k1 + r_square * (k2 + r_square * k3)));
-        double nnx =
-            nx * scale + 2 * p1 * nx * ny + p2 * (r_square + 2 * nx * nx);
-        double nny =
-            ny * scale + p1 * (r_square + 2 * ny * ny) + 2 * p2 * nx * ny;
-        x_ptr[u] = static_cast<float>(nnx * fx + cx);
-        y_ptr[u] = static_cast<float>(nny * fy + cy);
+        for (int u = 0; u < width; ++u) {
+            Eigen::Matrix<float, 3, 1> xy1;
+            xy1 << (static_cast<float>(u) - ncx) / nfx,
+                (static_cast<float>(v) - ncy) / nfy, 1;
+            auto XYW = Rinv * xy1;
+            double nx = XYW(0, 0) / XYW(2, 0);
+            double ny = XYW(1, 0) / XYW(2, 0);
+            double r_square = nx * nx + ny * ny;
+            // double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
+            // double xd = (x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2);
+            // double yd = (y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2);
+            double scale = (1 + r_square * (k1 + r_square * (k2 + r_square * k3)));
+            scale = scale / (1 + r_square * (k4 + r_square * (k5 + r_square * k6)));
+            double nnx =
+                nx * scale + 2 * p1 * nx * ny + p2 * (r_square + 2 * nx * nx) + s1 * r_square + s2 * r_square * r_square;
+            double nny =
+                ny * scale + p1 * (r_square + 2 * ny * ny) + 2 * p2 * nx * ny + s3 * r_square + s4 * r_square * r_square;
+            x_ptr[u] = static_cast<float>(nnx * fx + cx);
+            y_ptr[u] = static_cast<float>(nny * fy + cy);
         }
     }
 }
@@ -153,26 +171,28 @@ bool Undistort::Exec(){
     auto src_img = iptr->img_ptr_;
 
     NppiInterpolationMode remap_mode = NPPI_INTER_LINEAR;
-    NppiSize image_size;
-    image_size.width = width_;
-    image_size.height = height_;
-    NppiRect remap_roi = {0, 0, width_, height_};
+    NppiSize src_image_size, dst_image_size;
+    src_image_size.width = src_width_;
+    src_image_size.height = src_height_;
+    dst_image_size.width = dst_width_;
+    dst_image_size.height = dst_height_;
+    NppiRect remap_roi = {0, 0, src_width_, src_height_};
 
     NppStatus status;
     int d_map_step = static_cast<int>(d_mapx_.shape(1) * sizeof(float));
     switch (src_img->channels()) {
         case 1:
             status = nppiRemap_8u_C1R(
-                src_img->gpu_data(), image_size, src_img->width_step(), remap_roi,
+                src_img->gpu_data(), src_image_size, src_img->width_step(), remap_roi,
                 d_mapx_.gpu_data(), d_map_step, d_mapy_.gpu_data(), d_map_step,
-                dst_img->mutable_gpu_data(), dst_img->width_step(), image_size,
+                dst_img->mutable_gpu_data(), dst_img->width_step(), dst_image_size,
                 remap_mode);
             break;
         case 3:
             status = nppiRemap_8u_C3R(
-                src_img->gpu_data(), image_size, src_img->width_step(), remap_roi,
+                src_img->gpu_data(), src_image_size, src_img->width_step(), remap_roi,
                 d_mapx_.gpu_data(), d_map_step, d_mapy_.gpu_data(), d_map_step,
-                dst_img->mutable_gpu_data(), dst_img->width_step(), image_size,
+                dst_img->mutable_gpu_data(), dst_img->width_step(), dst_image_size,
                 remap_mode);
             break;
         default:

src/libs/preprocessors/undistort.h

@@ -30,7 +30,7 @@ public:
 
     bool loadIntrinsic(std::string& yaml_file);
     void InitUndistortRectifyMap(const Eigen::Matrix3f &camera_model,
-                               const Eigen::Matrix<float, 1, 5> &distortion,
+                               const Eigen::Matrix<float, 1, 14> &distortion,
                                const Eigen::Matrix3f &R,
                                const Eigen::Matrix3f &new_camera_model,
                                int width, int height, base::Blob<float> *d_mapx,
@@ -41,12 +41,15 @@ public:
     base::Blob<float> d_mapy_;
     base::Image8UPtr dst_img;
     Eigen::Matrix<float, 3, 3, Eigen::RowMajor> camera_intrinsic;
+    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> new_camera_intrinsic;
     Eigen::Matrix<float, 3, 3, Eigen::RowMajor> camera_rectification;
     Eigen::Matrix<float, 3, 4, Eigen::RowMajor> camera_projection;
-    Eigen::Matrix<float, 1, 5, Eigen::RowMajor> distortion_coefficients;
+    Eigen::Matrix<float, 1, 14, Eigen::RowMajor> distortion_coefficients;
 
-    int width_ = 0;     // image cols
-    int height_ = 0;    // image rows
+    int src_width_ = 0;
+    int src_height_ = 0;
+    int dst_width_ = 0;     // image cols
+    int dst_height_ = 0;    // image rows
     bool inited_ = 0;
 
 };

src/tasks/task_multi.cpp

@@ -37,6 +37,9 @@ void TaskMulti::Visualize(cv::Mat* image, interfaces::BaseIOPtr outs){
 
 void TaskMulti::Visualize(cv::Mat* image, std::vector<interfaces::BaseIOPtr>& outputs){
     auto detections = std::dynamic_pointer_cast<ios::Detection2Ds>(outputs[0])->detections;
+    auto undistort_image = std::dynamic_pointer_cast<ios::CameraSrcOut>(outputs[outputs.size()-1])->img_ptr_->toCVMat();
+    undistort_image.copyTo(*image);
+    // image->data = undistort_image.clone().data;
     for(auto& obj: detections){
         cv::Scalar color = get_color(obj->class_label * 100 + obj->track_id);
         cv::putText(*image, std::to_string(obj->class_label) + ":" + common::formatValue(obj->confidence, 2),