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@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
CLVideoStabilizerKernel(const SmartPtr<CLContext> & context,const char * name,uint32_t channel,SmartPtr<CLImageHandler> & handler)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
CLVideoStabilizer(const SmartPtr<CLContext> & context,const char * name)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>
get_warp_input_buf()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
is_ready()77 CLVideoStabilizer::is_ready ()
78 {
79 return CLImageHandler::is_ready ();
80 }
81
82 void
reset_counter()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
execute_done(SmartPtr<VideoBuffer> & output)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
prepare_parameters(SmartPtr<VideoBuffer> & input,SmartPtr<VideoBuffer> & output)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
set_sensor_calibration(CalibrationParams & params)184 CLVideoStabilizer::set_sensor_calibration (CalibrationParams ¶ms)
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
set_camera_intrinsics(double focal_x,double focal_y,double offset_x,double offset_y,double skew)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
align_coordinate_system(CoordinateSystemConv & world_to_device,CoordinateSystemConv & device_to_image)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
set_motion_filter(uint32_t radius,float stdev)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
analyze_motion(int64_t frame0_ts,DevicePoseList pose0_list,int64_t frame1_ts,DevicePoseList pose1_list)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
stabilize_motion(int32_t stab_frame_id,std::list<Mat3d> & motions)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>
create_kernel_video_stab(const SmartPtr<CLContext> & context,uint32_t channel,SmartPtr<CLImageHandler> handler)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>
create_cl_video_stab_handler(const SmartPtr<CLContext> & context)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
MotionFilter(uint32_t radius,float stdev)335 MotionFilter::MotionFilter (uint32_t radius, float stdev)
336 : _radius (radius),
337 _stdev (stdev)
338 {
339 set_filters (radius, stdev);
340 }
341
~MotionFilter()342 MotionFilter::~MotionFilter ()
343 {
344 _weight.clear ();
345 }
346
347 void
set_filters(uint32_t radius,float stdev)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
cumulate_motion(uint32_t index,uint32_t from,std::list<Mat3d> & motions)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
stabilize(int32_t index,std::list<Mat3d> & motions,int32_t max)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