Home | History | Annotate | Download | only in ocl 
      1 /*
      2  * cl_video_stabilizer.cpp -  Digital Video Stabilization using IMU (Gyroscope, Accelerometer)
      3  *
      4  *  Copyright (c) 2017 Intel Corporation
      5  *
      6  * Licensed under the Apache License, Version 2.0 (the "License");
      7  * you may not use this file except in compliance with the License.
      8  * You may obtain a copy of the License at
      9  *
     10  *      http://www.apache.org/licenses/LICENSE-2.0
     11  *
     12  * Unless required by applicable law or agreed to in writing, software
     13  * distributed under the License is distributed on an "AS IS" BASIS,
     14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     15  * See the License for the specific language governing permissions and
     16  * limitations under the License.
     17  *
     18  * Author: Zong Wei <wei.zong (at) intel.com>
     19  */
     20 
     21 #include "cl_video_stabilizer.h"
     22 #include "cl_utils.h"
     23 
     24 namespace XCam {
     25 
     26 static const XCamKernelInfo kernel_video_stab_warp_info [] = {
     27     {
     28         "kernel_image_warp_8_pixel",
     29 #include "kernel_image_warp.clx"
     30         , 0,
     31     },
     32     {
     33         "kernel_image_warp_1_pixel",
     34 #include "kernel_image_warp.clx"
     35         , 0,
     36     }
     37 };
     38 
     39 CLVideoStabilizerKernel::CLVideoStabilizerKernel (
     40     const SmartPtr<CLContext> &context,
     41     const char *name,
     42     uint32_t channel,
     43     SmartPtr<CLImageHandler> &handler)
     44     : CLImageWarpKernel (context, name, channel, handler)
     45 {
     46     _handler = handler.dynamic_cast_ptr<CLVideoStabilizer> ();
     47 }
     48 
     49 CLVideoStabilizer::CLVideoStabilizer (const SmartPtr<CLContext> &context, const char *name)
     50     : CLImageWarpHandler (context, name)
     51 {
     52     _projector = new ImageProjector ();
     53     _filter_radius = 15;
     54     _motion_filter = new MotionFilter (_filter_radius, 10);
     55 
     56     CoordinateSystemConv world_to_device (AXIS_X, AXIS_MINUS_Z, AXIS_NONE);
     57     CoordinateSystemConv device_to_image (AXIS_X, AXIS_Y, AXIS_Y);
     58 
     59     align_coordinate_system (world_to_device, device_to_image);
     60 
     61     _input_frame_id = -1;
     62     _frame_ts[0] = 0;
     63     _frame_ts[1] = 0;
     64     _stabilized_frame_id = -1;
     65 }
     66 
     67 SmartPtr<VideoBuffer>
     68 CLVideoStabilizer::get_warp_input_buf ()
     69 {
     70     XCAM_ASSERT (_input_buf_list.size () >= 1);
     71 
     72     SmartPtr<VideoBuffer> buf = (*_input_buf_list.begin ());
     73     return buf;
     74 }
     75 
     76 bool
     77 CLVideoStabilizer::is_ready ()
     78 {
     79     return CLImageHandler::is_ready ();
     80 }
     81 
     82 void
     83 CLVideoStabilizer::reset_counter ()
     84 {
     85     XCAM_LOG_DEBUG ("reset video stabilizer counter");
     86 
     87     _input_frame_id = -1;
     88     _stabilized_frame_id = -1;
     89     xcam_mem_clear (_frame_ts);
     90     _device_pose[0].clear ();
     91     _device_pose[1].clear ();
     92     _input_buf_list.clear ();
     93 }
     94 
     95 XCamReturn
     96 CLVideoStabilizer::execute_done (SmartPtr<VideoBuffer> &output)
     97 {
     98     if (!_input_buf_list.empty ()) {
     99         _input_buf_list.pop_front ();
    100     }
    101 
    102     CLImageWarpHandler::execute_done (output);
    103 
    104     return XCAM_RETURN_NO_ERROR;
    105 }
    106 
    107 XCamReturn
    108 CLVideoStabilizer::prepare_parameters (SmartPtr<VideoBuffer> &input, SmartPtr<VideoBuffer> &output)
    109 {
    110     XCamReturn ret = XCAM_RETURN_NO_ERROR;
    111     XCAM_ASSERT (input.ptr () && output.ptr ());
    112     XCAM_UNUSED (output);
    113 
    114     if (_input_buf_list.size () >= 2 * _filter_radius + 1) {
    115         _input_buf_list.pop_front ();
    116     }
    117     _input_buf_list.push_back (input);
    118     _input_frame_id++;
    119 
    120     const VideoBufferInfo & video_info_in = input->get_video_info ();
    121 
    122     _frame_ts[_input_frame_id % 2] = input->get_timestamp ();
    123 
    124     SmartPtr<DevicePose> data = input->find_typed_metadata<DevicePose> ();
    125     while (data.ptr ()) {
    126         _device_pose[_input_frame_id % 2].push_back (data);
    127 
    128         input->remove_metadata (data);
    129 
    130         data = input->find_typed_metadata<DevicePose> ();
    131     }
    132 
    133     Mat3d homography;
    134     if (_input_frame_id > 0) {
    135         homography = analyze_motion (
    136                          _frame_ts[(_input_frame_id - 1) % 2],
    137                          _device_pose[(_input_frame_id - 1) % 2],
    138                          _frame_ts[_input_frame_id % 2],
    139                          _device_pose[_input_frame_id % 2]);
    140 
    141         if (_motions.size () >= 2 * _filter_radius + 1) {
    142             _motions.pop_front ();
    143         }
    144         _motions.push_back (homography);
    145 
    146         _device_pose[(_input_frame_id - 1) % 2].clear ();
    147     }
    148 
    149     Mat3d proj_mat;
    150     XCamDVSResult warp_config;
    151     if (_input_frame_id >= _filter_radius)
    152     {
    153         _stabilized_frame_id = _input_frame_id - _filter_radius;
    154         int32_t cur_stabilized_pos = XCAM_MIN (_stabilized_frame_id, _filter_radius + 1);
    155 
    156         XCAM_LOG_DEBUG ("input id(%ld), stab id(%ld), cur stab pos(%d), filter r(%d)",
    157                         _input_frame_id,
    158                         _stabilized_frame_id,
    159                         cur_stabilized_pos,
    160                         _filter_radius);
    161 
    162         proj_mat = stabilize_motion (cur_stabilized_pos, _motions);
    163 
    164         Mat3d proj_inv_mat = proj_mat.inverse ();
    165         warp_config.frame_id = _stabilized_frame_id;
    166         warp_config.frame_width = video_info_in.width;
    167         warp_config.frame_height = video_info_in.height;
    168 
    169         for( int i = 0; i < 3; i++ ) {
    170             for (int j = 0; j < 3; j++) {
    171                 warp_config.proj_mat[i * 3 + j] = proj_inv_mat(i, j);
    172             }
    173         }
    174 
    175         set_warp_config (warp_config);
    176     } else {
    177         ret = XCAM_RETURN_BYPASS;
    178     }
    179 
    180     return ret;
    181 }
    182 
    183 XCamReturn
    184 CLVideoStabilizer::set_sensor_calibration (CalibrationParams &params)
    185 {
    186     XCamReturn ret = XCAM_RETURN_NO_ERROR;
    187 
    188     if (_projector.ptr ()) {
    189         _projector->set_sensor_calibration (params);
    190     } else {
    191         ret = XCAM_RETURN_ERROR_PARAM;
    192     }
    193 
    194     return ret;
    195 }
    196 
    197 XCamReturn
    198 CLVideoStabilizer::set_camera_intrinsics (
    199     double focal_x,
    200     double focal_y,
    201     double offset_x,
    202     double offset_y,
    203     double skew)
    204 {
    205     XCamReturn ret = XCAM_RETURN_NO_ERROR;
    206 
    207     if (_projector.ptr ()) {
    208         _projector->set_camera_intrinsics(
    209             focal_x,
    210             focal_y,
    211             offset_x,
    212             offset_y,
    213             skew);
    214     } else {
    215         ret = XCAM_RETURN_ERROR_PARAM;
    216     }
    217 
    218     return ret;
    219 }
    220 
    221 XCamReturn
    222 CLVideoStabilizer::align_coordinate_system (
    223     CoordinateSystemConv &world_to_device,
    224     CoordinateSystemConv &device_to_image)
    225 {
    226     XCamReturn ret = XCAM_RETURN_NO_ERROR;
    227 
    228     _world_to_device = world_to_device;
    229     _device_to_image = device_to_image;
    230 
    231     return ret;
    232 }
    233 
    234 XCamReturn
    235 CLVideoStabilizer::set_motion_filter (uint32_t radius, float stdev)
    236 {
    237     XCamReturn ret = XCAM_RETURN_NO_ERROR;
    238 
    239     _filter_radius = radius;
    240 
    241     if (_motion_filter.ptr ()) {
    242         _motion_filter->set_filters (radius, stdev);
    243     } else {
    244         ret = XCAM_RETURN_ERROR_PARAM;
    245     }
    246 
    247     return ret;
    248 }
    249 
    250 Mat3d
    251 CLVideoStabilizer::analyze_motion (
    252     int64_t frame0_ts,
    253     DevicePoseList pose0_list,
    254     int64_t frame1_ts,
    255     DevicePoseList pose1_list)
    256 {
    257     if (pose0_list.empty () || pose1_list.empty () || !_projector.ptr ()) {
    258         return Mat3d ();
    259     }
    260     XCAM_ASSERT (frame0_ts < frame1_ts);
    261 
    262     Mat3d ext0 = _projector->calc_camera_extrinsics (frame0_ts, pose0_list);
    263 
    264     Mat3d ext1 = _projector->calc_camera_extrinsics (frame1_ts, pose1_list);
    265 
    266     Mat3d extrinsic0 = _projector->align_coordinate_system (
    267                            _world_to_device,
    268                            ext0,
    269                            _device_to_image);
    270 
    271     Mat3d extrinsic1 = _projector->align_coordinate_system (
    272                            _world_to_device,
    273                            ext1,
    274                            _device_to_image);
    275 
    276     return _projector->calc_projective (extrinsic0, extrinsic1);
    277 }
    278 
    279 Mat3d
    280 CLVideoStabilizer::stabilize_motion (int32_t stab_frame_id, std::list<Mat3d> &motions)
    281 {
    282     if (_motion_filter.ptr ()) {
    283         return _motion_filter->stabilize (stab_frame_id, motions, _input_frame_id);
    284     } else {
    285         return Mat3d ();
    286     }
    287 }
    288 
    289 static SmartPtr<CLVideoStabilizerKernel>
    290 create_kernel_video_stab (
    291     const SmartPtr<CLContext> &context,
    292     uint32_t channel,
    293     SmartPtr<CLImageHandler> handler)
    294 {
    295     SmartPtr<CLVideoStabilizerKernel> stab_kernel;
    296 
    297     const char *name = (channel == CL_IMAGE_CHANNEL_Y ? "kernel_image_warp_y" : "kernel_image_warp_uv");
    298     char build_options[1024];
    299     xcam_mem_clear (build_options);
    300 
    301     snprintf (build_options, sizeof (build_options),
    302               " -DWARP_Y=%d ",
    303               (channel == CL_IMAGE_CHANNEL_Y ? 1 : 0));
    304 
    305     stab_kernel = new CLVideoStabilizerKernel (context, name, channel, handler);
    306     XCAM_ASSERT (stab_kernel.ptr ());
    307     XCAM_FAIL_RETURN (
    308         ERROR, stab_kernel->build_kernel (kernel_video_stab_warp_info[KernelImageWarp], build_options) == XCAM_RETURN_NO_ERROR,
    309         NULL, "build video stab kernel failed");
    310     XCAM_ASSERT (stab_kernel->is_valid ());
    311 
    312     return stab_kernel;
    313 }
    314 
    315 SmartPtr<CLImageHandler>
    316 create_cl_video_stab_handler (const SmartPtr<CLContext> &context)
    317 {
    318     SmartPtr<CLImageHandler> video_stab;
    319     SmartPtr<CLImageKernel> stab_kernel;
    320 
    321     video_stab = new CLVideoStabilizer (context);
    322     XCAM_ASSERT (video_stab.ptr ());
    323 
    324     stab_kernel = create_kernel_video_stab (context, CL_IMAGE_CHANNEL_Y, video_stab);
    325     XCAM_ASSERT (stab_kernel.ptr ());
    326     video_stab->add_kernel (stab_kernel);
    327 
    328     stab_kernel = create_kernel_video_stab (context, CL_IMAGE_CHANNEL_UV, video_stab);
    329     XCAM_ASSERT (stab_kernel.ptr ());
    330     video_stab->add_kernel (stab_kernel);
    331 
    332     return video_stab;
    333 }
    334 
    335 MotionFilter::MotionFilter (uint32_t radius, float stdev)
    336     : _radius (radius),
    337       _stdev (stdev)
    338 {
    339     set_filters (radius, stdev);
    340 }
    341 
    342 MotionFilter::~MotionFilter ()
    343 {
    344     _weight.clear ();
    345 }
    346 
    347 void
    348 MotionFilter::set_filters (uint32_t radius, float stdev)
    349 {
    350     _radius = radius;
    351     _stdev = stdev > 0.f ? stdev : std::sqrt (static_cast<float>(radius));
    352 
    353     int scale = 2 * _radius + 1;
    354     float dis = 0.0f;
    355     float sum = 0.0f;
    356 
    357     _weight.resize (2 * _radius + 1);
    358 
    359     for (int i = 0; i < scale; i++) {
    360         dis = ((float)i - radius) * ((float)i - radius);
    361         _weight[i] = exp(-dis / (_stdev * _stdev));
    362         sum += _weight[i];
    363     }
    364 
    365     for (int i = 0; i <= scale; i++) {
    366         _weight[i] /= sum;
    367     }
    368 
    369 }
    370 
    371 Mat3d
    372 MotionFilter::cumulate_motion (uint32_t index, uint32_t from, std::list<Mat3d> &motions)
    373 {
    374     Mat3d motion;
    375     motion.eye ();
    376 
    377     uint32_t id = 0;
    378     std::list<Mat3d>::iterator it;
    379 
    380     if (from < index) {
    381         for (id = 0, it = motions.begin (); it != motions.end (); id++, ++it) {
    382             if (from <= id && id < index) {
    383                 motion = (*it) * motion;
    384             }
    385         }
    386         motion = motion.inverse ();
    387     } else if (from > index) {
    388         for (id = 0, it = motions.begin (); it != motions.end (); id++, ++it) {
    389             if (index <= id && id < from) {
    390                 motion = (*it) * motion;
    391             }
    392         }
    393     }
    394 
    395     return motion;
    396 }
    397 
    398 Mat3d
    399 MotionFilter::stabilize (int32_t index,
    400                          std::list<Mat3d> &motions,
    401                          int32_t max)
    402 {
    403     Mat3d res;
    404     res.zeros ();
    405 
    406     double sum = 0.0f;
    407     int32_t idx_min = XCAM_MAX ((index - _radius), 0);
    408     int32_t idx_max = XCAM_MIN ((index + _radius), max);
    409 
    410     for (int32_t i = idx_min; i <= idx_max; ++i)
    411     {
    412         res = res + cumulate_motion (index, i, motions) * _weight[i];
    413         sum += _weight[i];
    414     }
    415     if (sum > 0.0f) {
    416         return res * (1 / sum);
    417     }
    418     else {
    419         return Mat3d ();
    420     }
    421 }
    422 
    423 }
    424