stitch: add viewpoint to improve quality

Signed-off-by: Wind Yuan <feng.yuan@intel.com>

6 files changed, 58 insertions, 12 deletions

diff --git a/tests/test-soft-image.cpp b/tests/test-soft-image.cpp
index 978424c..444bcfe 100644
--- a/tests/test-soft-image.cpp
+++ b/tests/test-soft-image.cpp
@@ -351,17 +351,28 @@ int main (int argc, char *argv[])
         if (!fisheye_config_path)
             fisheye_config_path = FISHEYE_CONFIG_PATH;
 
-        stitcher->set_camera_num (4);
         for (uint32_t i = 0; i < 4; ++i) {
             if (parse_camera_info (fisheye_config_path, i, output_width, output_height, cam_info[i]) != 0) {
                 XCAM_LOG_ERROR ("parse fisheye dewarp info(idx:%d) failed.", i);
                 return -1;
             }
+        }
+
+        PointFloat3 bowl_coord_offset;
+        centralize_bowl_coord_from_cameras (
+            cam_info[0].calibration.extrinsic, cam_info[1].calibration.extrinsic,
+            cam_info[2].calibration.extrinsic, cam_info[3].calibration.extrinsic,
+            bowl_coord_offset);
+
+        stitcher->set_camera_num (4);
+        for (uint32_t i = 0; i < 4; ++i) {
             stitcher->set_camera_info (i, cam_info[i]);
         }
+
         BowlDataConfig bowl;
         bowl.wall_height = 3000.0f;
         bowl.ground_length = 2000.0f;
+        bowl.viewpoint_z = 1300.0f;
         //bowl.a = 5000.0f;
         //bowl.b = 3600.0f;
         //bowl.c = 3000.0f;
diff --git a/xcore/interface/data_types.h b/xcore/interface/data_types.h
index 71743c6..d2047e0 100644
--- a/xcore/interface/data_types.h
+++ b/xcore/interface/data_types.h
@@ -123,6 +123,13 @@ typedef Point3DT<float> PointFloat3;
 /*
  * Ellipsoid model
  *  x^2 / a^2 + y^2 / b^2 + (z-center_z)^2 / c^2 = 1
+ * ground : z = 0
+ * x_axis : front direction
+ * y_axis : left direction
+ * z_axis : up direction
+ * viewpoint_z : viewpoint in z_axis
+ * wall_height : bowl height inside of view
+ * ground_length: left direction distance from ellipsoid bottom edge to nearest side of the car in the view
  */
 struct BowlDataConfig {
     float a, b, c;
@@ -130,14 +137,15 @@ struct BowlDataConfig {
 
     // unit mm
     float center_z;
+    float viewpoint_z;
     float wall_height;
     float ground_length;
 
     BowlDataConfig ()
         : a (6060.0f), b (4388.0f), c (3003.4f)
         , angle_start (90.0f), angle_end (270.0f)
-        , center_z (1500.0f), wall_height (3000.0f)
-        , ground_length (2801.0f) // (2168.0f)
+        , center_z (1500.0f), viewpoint_z (1200.0f)
+        , wall_height (3000.0f), ground_length (2801.0f)
     {
         XCAM_ASSERT (fabs(center_z) <= c);
         XCAM_ASSERT (a > 0.0f && b > 0.0f && c > 0.0f);
diff --git a/xcore/surview_fisheye_dewarp.cpp b/xcore/surview_fisheye_dewarp.cpp
index 6c0830c..3913846 100644
--- a/xcore/surview_fisheye_dewarp.cpp
+++ b/xcore/surview_fisheye_dewarp.cpp
@@ -65,6 +65,7 @@ SurViewFisheyeDewarp::fisheye_dewarp(MapTable &map_table, uint32_t table_w, uint
     PointFloat3 world_coord;
     PointFloat3 cam_coord;
     PointFloat3 cam_world_coord;
+    PointFloat3 view_point (0.0f, 0.0f, bowl_config.viewpoint_z);
     PointFloat2 image_coord;
 
     XCAM_LOG_DEBUG ("fisheye-dewarp:\n table(%dx%d), out_size(%dx%d)"
@@ -81,7 +82,7 @@ SurViewFisheyeDewarp::fisheye_dewarp(MapTable &map_table, uint32_t table_w, uint
         for(uint32_t col = 0; col < table_w; col++) {
             PointFloat2 out_pos (col * scale_factor_w, row * scale_factor_h);
             world_coord = bowl_view_image_to_world (bowl_config, image_w, image_h, out_pos);
-            cal_cam_world_coord(world_coord, cam_world_coord);
+            cal_cam_world_coord(view_point, world_coord, cam_world_coord);
             world_coord2cam(cam_world_coord, cam_coord);
             cal_image_coord(cam_coord, image_coord);
 
@@ -91,19 +92,19 @@ SurViewFisheyeDewarp::fisheye_dewarp(MapTable &map_table, uint32_t table_w, uint
 }
 
 void
-SurViewFisheyeDewarp::cal_cam_world_coord(const PointFloat3 &world_coord, PointFloat3 &cam_world_coord)
+SurViewFisheyeDewarp::cal_cam_world_coord(const PointFloat3 &view_point, const PointFloat3 &world_coord, PointFloat3 &cam_world_coord)
 {
     Mat4f rotation_mat = generate_rotation_matrix( degree2radian (_extrinsic_param.roll),
                          degree2radian (_extrinsic_param.pitch),
                          degree2radian (_extrinsic_param.yaw));
     Mat4f rotation_tran_mat = rotation_mat;
-    rotation_tran_mat(0, 3) = _extrinsic_param.trans_x;
-    rotation_tran_mat(1, 3) = _extrinsic_param.trans_y;
-    rotation_tran_mat(2, 3) = _extrinsic_param.trans_z;
+    rotation_tran_mat(0, 3) = _extrinsic_param.trans_x - view_point.x;
+    rotation_tran_mat(1, 3) = _extrinsic_param.trans_y - view_point.y;
+    rotation_tran_mat(2, 3) = _extrinsic_param.trans_z - view_point.z;
 
-    Mat4f world_coord_mat(Vec4f(1.0f, 0.0f, 0.0f, world_coord.x),
-                          Vec4f(0.0f, 1.0f, 0.0f, world_coord.y),
-                          Vec4f(0.0f, 0.0f, 1.0f, world_coord.z),
+    Mat4f world_coord_mat(Vec4f(1.0f, 0.0f, 0.0f, world_coord.x - view_point.x),
+                          Vec4f(0.0f, 1.0f, 0.0f, world_coord.y - view_point.y),
+                          Vec4f(0.0f, 0.0f, 1.0f, world_coord.z - view_point.z),
                           Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
 
     Mat4f cam_world_coord_mat = rotation_tran_mat.inverse() * world_coord_mat;
diff --git a/xcore/surview_fisheye_dewarp.h b/xcore/surview_fisheye_dewarp.h
index 571b02a..abd7c1b 100644
--- a/xcore/surview_fisheye_dewarp.h
+++ b/xcore/surview_fisheye_dewarp.h
@@ -49,7 +49,7 @@ private:
 
     virtual void cal_image_coord (const PointFloat3 &cam_coord, PointFloat2 &image_coord);
 
-    void cal_cam_world_coord (const PointFloat3 &world_coord, PointFloat3 &cam_world_coord);
+    void cal_cam_world_coord (const PointFloat3 &view_point, const PointFloat3 &world_coord, PointFloat3 &cam_world_coord);
     void world_coord2cam (const PointFloat3 &cam_world_coord, PointFloat3 &cam_coord);
 
     Mat4f generate_rotation_matrix(float roll, float pitch, float yaw);
diff --git a/xcore/xcam_utils.cpp b/xcore/xcam_utils.cpp
index 0ea579b..4393961 100644
--- a/xcore/xcam_utils.cpp
+++ b/xcore/xcam_utils.cpp
@@ -151,6 +151,28 @@ PointFloat3 bowl_view_image_to_world (
     return world;
 }
 
+void centralize_bowl_coord_from_cameras (
+    ExtrinsicParameter &front_cam, ExtrinsicParameter &right_cam,
+    ExtrinsicParameter &rear_cam, ExtrinsicParameter &left_cam,
+    PointFloat3 &bowl_coord_offset)
+{
+    bowl_coord_offset.x = (front_cam.trans_x + rear_cam.trans_x) / 2.0f;
+    bowl_coord_offset.y = (right_cam.trans_y + left_cam.trans_y) / 2.0f;
+    bowl_coord_offset.z = 0.0f;
+
+    front_cam.trans_x -= bowl_coord_offset.x;
+    front_cam.trans_y -= bowl_coord_offset.y;
+
+    right_cam.trans_x -= bowl_coord_offset.x;
+    right_cam.trans_y -= bowl_coord_offset.y;
+
+    rear_cam.trans_x -= bowl_coord_offset.x;
+    rear_cam.trans_y -= bowl_coord_offset.y;
+
+    left_cam.trans_x -= bowl_coord_offset.x;
+    left_cam.trans_y -= bowl_coord_offset.y;
+}
+
 double
 linear_interpolate_p2 (
     double value_start, double value_end,
diff --git a/xcore/xcam_utils.h b/xcore/xcam_utils.h
index 64f59d2..492744a 100644
--- a/xcore/xcam_utils.h
+++ b/xcore/xcam_utils.h
@@ -36,6 +36,10 @@ PointFloat3 bowl_view_image_to_world (
     const uint32_t img_width, const uint32_t img_height,
     const PointFloat2 &img_pos);
 
+void centralize_bowl_coord_from_cameras (
+    ExtrinsicParameter &front_cam, ExtrinsicParameter &right_cam,
+    ExtrinsicParameter &rear_cam, ExtrinsicParameter &left_cam,
+    PointFloat3 &bowl_coord_offset);
 
 double
 linear_interpolate_p2 (