undistort-revise

configs/tasks/multi/multi_yolov5.yaml

@@ -12,11 +12,8 @@ units:
         name: Undistort
         inputNames: [uint8Image]
         outputNames: [undistortVisImage]
-        imageWidth: 1280
-        imageHeight: 720
-        # new_camera_matrix: [1, 0, 1, 
-        #                     0, 1, 1, 
-        #                     0, 0, 1]
+        imageWidth: 960
+        imageHeight: 540
         IntrinsicPath: configs/tasks/multi/rgb_intrinsic.yaml
     -
         name: ResizeNorm
@@ -45,7 +42,7 @@ units:
         maxBatchSize: 1 # used when build engine
     -
         name: MultiPostProcess
-        inputNames: [detections, seg_protos, depths, semantics, uint8Image]
+        inputNames: [detections, seg_protos, depths, semantics, undistortVisImage]
         outputNames: [out_instances, out_semantics, out_depths]
         inputWidth: 640
         inputHeight: 640

configs/tasks/multi/rgb_intrinsic.yaml

@@ -20,3 +20,12 @@ distortion_coefficients:
           -55.27210998535156,
           417.6490173339844]
 
+new_image_width: 960
+new_image_height: 540
+new_camera_matrix:
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 779.9261475, 0.,  478.369263, 
+           0., 779.9261475,  275.66448975, 
+           0.,   0.,        1.]

src/libs/preprocessors/undistort.cpp

@@ -20,18 +20,6 @@ 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({dst_height_, dst_width_});
     d_mapy_.Reshape({dst_height_, dst_width_});
@@ -41,7 +29,7 @@ bool Undistort::Init(YAML::Node& node) {
 
     InitUndistortRectifyMap(camera_intrinsic,
                             distortion_coefficients, I,
-                            camera_intrinsic, dst_width_,
+                            new_camera_intrinsic, dst_width_,
                             dst_height_, &d_mapx_, &d_mapy_);
 
     dst_img = std::make_shared<base::Image8U>(dst_height_, dst_width_, base::Color::BGR);
@@ -79,6 +67,22 @@ bool Undistort::loadIntrinsic(std::string& yaml_file){
                 distortion_coefficients(i) = 0;
             }
         }
+
+        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"]["data"][i].as<float>();
+            }
+            dst_width_ = node["new_image_width"].as<int>();
+            dst_height_ = node["new_image_height"].as<int>();
+        }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_;
+            }
+        }
+
         if(node["rectification_matrix"].IsDefined() && node["rectification_matrix"]["data"].IsDefined() &&
             !node["rectification_matrix"]["data"].IsNull()
         ){