Hierarchical Sampling based Particle Filter for Visual-inertial Gimbal in the Wild

The gimbal platform has been widely used in photogrammetry and robot perceptual module to stabilize the camera pose, thereby improving the captured video quality. Usually a gimbal is mainly composed of sensors and actuator parts. The orientation measurements from sensor can be inputted directly to actuator to steer camera towards proper pose. But the off-the-shelf custom product is either quite expensive, or depending on highly precise IMU and Brushless DC motor with hall sensor to estimate angles, which is prone to suffer from accumulative drift over long-term operation. In this paper, a CV based new tracking and fusion algorithm dedicated for gimbal system on drones operating in nature is proposed, main contributions are listed as below: a) a light-weight Resnet -18 backbone based network model was trained from scratch, and deployed onto Jetson Nano platform to segment the image into binary parts (ground and sky). b) geometric primitives tracking of the skyline and ground plane in 3D as cues, along with orientation estimation from IMU can provide multiple guesses for orientation. c) spherical surface based adaptive particle sampling can fuse orientation from aforementioned sensor sources efficiently. The final prototyping algorithm is tested on the real-time embedded system, and with both simulation on ground and real functional tests in the air.


II. RELATED WORKS
The stabilization of video problem can be traced back to the Electronic Image Stabilization (EIS) technique [29], which relies on the handcrafted feature points in consecutive image frames, to search for correspondences for image alignment. This renders the cheap hand-held device possibility to stabilize video sequence without too much overhead in hardware or mechanics. The feature points based tracking, [26], or optical flow tracking [31], requires always good association of feature points or raw pixel intensities across video frames [22]. thus, it tends to be brittle to observation noise induced by motion blur. For that reason, humanoid robot vision system [34], or complex video [35] task compensates the rotation and shift of video images by using costly IMU only.
*This work was supported by Flemish government "Vlir-UOS" project under grant VLS-D7314-EC2020SIN278A101. 1  The review work about traditional motion modelling work applied to video stabilization, can be referred to in [30]. But these passive video stabilization work is always constrained on image frame, cannot fully utilizes more freedoms in 3D. The video stabilization problem can also be considered as image deblurring task. The main goal is to infer the pixel intensity in blurry area from transferring or interpolation of neighbouring pixels information on single or multiple images, incorporating both temporal or spatial constraints from the video. The recent deep learning technique boosts video processing performance by introducing prior from dataset at specific domains. [32] warps the selfie image patch locally by decoding the scene representation from multiple encoders, such as foreground contours, feature points on the background, even 3D facial mesh. [33] trains a sensor-driver model by using raw IMU data, to predict the visual odometry in a sliding time window, similar to the filter approach. [36] integrated the video stabilization into large mapping and localization framework via graph in self-supervised manner. To tackle the problem of video stabilization in dynamic scenes, [38] learns a dense warping fields from consecutive video frames, then the warped parts are blended to synthesize the stabilized image. [37] framework even makes use of the motion prediction model, a spatial filter for optical flow tracking, followed by inpainting process to synthesize outputs. All these models involve a lot of image processing overhead, and are quite heavy to deploy on edge device for real-time use.
Video stabilization is also widely applied in aerial surveillance [39]. By Using natural cue to estimate the attitude of UAVs or fixed-wing airplane is literally nothing new, like the work in [2] [6][1] relying all on skyline tracking. But it requires delicate tuning tricks to search for boundary, and input image with ideal condition. Other work depends on the tracking of geometric primitives on 2D image or in 3D world, either by handcrafted feature points, like [23][25] [26], or curved boundary [5], to predict the ego-pose of camera, which can further guide the stabilisation of attitude of UAVs, as work done in [3], [40] even detects the moving objects in the view through tracking filter over the "SIFT" [7] feature points. However, in natural setting, there are a lot of spurious and similar appearance features, all these tracking algorithm relying on traditional feature points cannot work at all in such setting.

III. OVERVIEW
The ultimate goal of our project is to deploy a gimbal platform carried by UAVs for volcanic eruption surveillance in a depopulated region full of mountains, thence, our work differs from prior work in that, a quite robust platform in operation over long-term and with endurance of harsh weather condition should be devised. As shown in Figure 1, the gimbal system is placed underneath airplane outfitted with camera, IMU, and barometer. To overcome the aforementioned correspondence issues of feature points, the gimbal system can take advantage of the natural cues such as skyline and ground for pose tracking. At bottom of Figure 1, there is a binary mask with skyline and points in ground region. The ray direction passing through the red dots position along with height from barometer, can determine the 3D point through trigonometry. More details about perception can be referred to at section IV.

A. Open source Hardware
The gimbal platform design is formed on open-sourced hardware completely. The main processing unit is a Jetson Nano, featured with 2GB GPU memory and Quad-core ARM A57, connected to IMU, camera, and barometer sensors. OpenCR driver board maps the driving command to control commands for two servo motors, in order to compensate roll and pitch angle accordingly.

B. ROS nodes
The software is composed mainly of three parts, The preprocessing part including network model and geometric primitive extraction, followed by tracking module to align the skyline and normal of ground plane in current frame with those at reference frame. The compensation angles from various pipelines are then fed into the proposed particle filter in section V to have fusion output, finally used as input for controller to compensate motion.

IV. PERCEPTION
The perception part is structured into preprocessing and rotation transform estimation, where he the perception part can be further separated into two pipelines, roll and pitch prediction from skyline tracking, and full Euler 3D rotation from ground plane tracking. The following clarification follows this structure.

A. Preprocessing
The general computer vision processing pipeline, like "OpenCV" library is investigated for our purpose at first. a) The feature points are extracted from two image frames to estimate relative pose, b) The boundary between sky and ground is isolated from image by Canny edge detection for alignment. As illustrated in Figure 4, regarding the feature points, the always alike appearance in grassland region and cloudy sky with dynamics all pose a challenge to correspondence match. In Figure 4a, many false inconsistent correspondences are found. As for boundary isolation, the color distribution of sky, clouds, and grass land are significantly contrasting, on top of the HSV image in Figure 4b, the Canny [8] edge detection can be applied to find the boundary between ground and sky region, but some brightness of bluish sky is cast onto grass ground. As a consequence, the final fitting curve are shifted downwards compared to reality as depicted in 4c. The Canny edge detection on raw RGB is quite noisy over whole image, unsuitable for boundary search. To tackle this challenging issue, we referred to the neural network model, and trained from scratch on Skyline dataset [4] by using Resnet-18 [19] as backbone for binary mask segmentation, the model is fine-tuned on self-collected dataset in nature with a few hundred images. Some train data samples along with ground truth masks are shared in Figure  5. The model is exported into "ONNX" and optimized by "TensorRT" to convert to "FP16" precision.

B. Estimation from Skyline Tracking
On top of binary mask results from network model, the skyline can be found along the pixel gradient change direction. The extracted skyline can be further considered as a cue to estimate the roll and pitch of camera with respect to the line in reference frame, as shown in Figure 6. The search of boundary points, as yellow dots in Figure 6c, can be implemented firstly over the whole reference image. Straight line fitting in red as illustrated in Figure 6c, including two parameters, slope m and intercept b in Equation 1. For the subsequent images, we use the constant angular velocity model from skyline tracking results of last two frames, to predict the skyline line position in current frame, like the green line presented in 6d. The assumption is that image never flips upside down, because of airplane maneuverability constraints. The boundary can always be searched along the vertical direction on predicted straight line pixels, for example, the pixel labelled as sky should be searched in downwards direction. In practice, the found points at boundary is further down-sampled to a few hundreds of points to speed up the search. Then the skyline parameters m and k in Equation 2 are calculated by least square algorithm from the sample points. The estimated skyline of current frame is the red line in Figure 6d. To guarantee the skyline has enough length for down-streaming processing, a minimal half length of image width is set as check condition. The roll α can be derived from arctan(m), and pitch β can be derived from subtraction of arctan((h1 − c)/ f y ) − arctan((h2 − c)/( f y )). c is the half size of image height, h1 and h2 is the height of center points at straight line in reference and current image frame. f y is the focus length of camera. To avoid unnecessary operation, roll and pitch have a certain tolerant range, so that processing is only triggered when the movement is out of this range. For the coupling issue of roll and pitch processing, the logic as below is utilized. There are three cases in total. From slope rate m of straight skyline, roll angle compensation can be recovered first. On top of corrected results by roll angle, pitch movement checks further by skyline height shift. In such way, pure roll, pure pitch, or both happening at the same time, are covered totally.

C. Estimation from Ground Plane Tracking
Ground plane tracking, is relying on the normal vector of ground plane, as demonstrated in Figure 1, a set of points in the ground region of binary mask are sampled evenly, followed by a back-projection to 3D camera frame corresponding to Equations 3-9. The height h is measured from barometer, and ray direction passing through the pixel position is P i at left of Equation 5. K is the intrinsic matrix obtained by calibration approach [15]. θ is derived from the dot product of gravitational vector and ray direction. Length scale l i can be obtained by trigonometry in Equation 8. Finally the current normal vector m of ground plane is shaped by cross product of non-Collinear triple points. The ground plane tracing mode is only triggered when the level height of camera is over a 300 meters level in our setting, so that the variance of uneven grass land can be approximated by a flat plane compared to height. The ground region also uses threshold condition check.
cos(θ ) = P i · N G || P i || · || N G || (7) Next, the rotation matrix to align the normal vector, m in current frame and reference normal n at start can be derived following Equations 11-14.
In the end, the 3D Euler angles are retrieved from the Rotation matrix according to  in an order of "yaw ¡-pitch ¡-roll". Only roll and pitch are used for fusion.
V. HIERARCHICAL TEMPLATE BASED PARTICLE FILTER The particle filter is easy to implement, applicable in non-linear problems, in particular for positioning like [20]. Finally, a variant of vanilla particle filter [28], sampling on the spherical surface is devised and presented in pseudo code below. In real configuration, the roll and pitch is virtually constrained by a rotation range because of mechanical kinematics, e.g., roll is -45 • to 45 • .   The general idea behind this adaptive particle filter is straightforward. It comprises of three steps, sampling from orientation measurements of IMU s I (Line 3), sampling according to observation from CV pipeline (Line 9), and resampling proportional to updated weight of each particle(Line 20). This weight is the square root distance of angle between one particle and CV observation. s C 1 , and s C 2 indicate the orientation from skyline and ground plane separately.

Algorithm 2: Particle Filter based on Manifold Template
The manifold surface is discretized into cells at multiscale resolution, where the longitudinal direction is pitch while the latitudinal direction is roll. When both observation are available, more samples will be created around those close to CV observation. In Line 3, particles are sampled from normal Gaussian distribution, with mean centered at IMU measurements plus shift by constant angular velocity propagating through interval and certain bias σ b .
Line 10 of Table 21 manifests the metric for neighbouring check of particles close to CV observation. µ f at Line 9 is coming from fusion Equation 20, a sum of mean of IMU and CV wieghted by inverse of variance.
IMU and CV variance are set according to our practical tests. µ C and δ C are merged results from two CV pipelines in same form of Equation 20 and 21. The fusion of three pipelines are recursive fusion by two times. Each particle life cycle can be represented into a four states' machine, in Figure 7, arrows between them stand for transition condition. The rasterized cells spread over the discretized manifold space of roll and pitch angles. In our setting, the CV approach has smaller variance compared to IMU according to accuracy. Three levels of resolution (Ω 1,2,3 in Table 21) are utilized. The top left is initial state, sampled from normal distribution centered at IMU measurement at time t. Bottom left is prediction based on propagation of previous state by extrapolation in time. When the orientation from single CV pipeline, either skyline or ground plane is available, the new samples in red are generated on finer resolution neighbouring the green dots corresponding to µ f in Equation 20. Each dot stands for the center of cell. If both CV pipelines are at hand, the highest resolution is employed for new samples generation. Each particle's timestamp of creation is recorded. If elapsed time is over certain interval δt, the particles will be moved back to coarser resolution, like the arrow direction from bottom right to top right. The particle set is a mixing states of this machine, each particle with different resolution over time.

VI. EXPERIMENT & RESULTS
In practice, the video is scaled down to 640×480 resolution, to achieve around 20 FPS real-time performance. Intrinsics of camera follow the calibration guide of [15]. Extrinsic calibration between IMU and camera is estbalished on an open source tool "Kalibr" [13] [14]. In Figure 8, the simulation setup at lab can be referred to. There are three parts, motor driver board, Jetson Nano, and gimbal part, the former two will be positioned inside airplane body for outdoor test, only gimbal part outfitted with sensors is exposed in the air. All 3D printed pieces have enhanced connections, also having taken the aerial dynamics into account for flying efficiency. A big natural picture from real scenario is chosen for test. The camera on gimbal is placed to be forward facing the wall. Then gimbal cases along with sensors, are attached onto a pole end, which is not in the view of picture 8, the other end is controlled by hand to simulate the random rotation and vibration. The ground truth data of roll and pitch angles are read from the fixed protractor near the each servo motor accordingly, like the one fixed behind the gimbal for roll reading. In demo video test, we use the fused estimation from our particle filter to compensate the motor rotation conversely for camera stabilization. The closed-loop PID control is leveraged for actuation. Motor is with 50-80ms latency. All the following test sequences are starting from horizontal direction on image. This configuration holds for all tests of our algorithms, we always start from the aligned ground plane orthogonal to gravitational vector, and horizontal skyline. In outdoor test, we always manually align the gimbal platform with proper pose. For comparison, the SOTA visual-inertial frameworks, "ORBSLAM3", "VINS-mono" and "MSCKF" are selected as benchmarks. The open source datasets for SLAM research were captured indoor or within the city or urban region, rarely including the depopulated wild zone, we thus recorded several custom sequences by using the aforementioned setup at lab. The outdoor test can be a bit tough to generate ground truth angle, thus we only performed the video test on real airplane outdoor for verification only. Disturbance to gimbal is simulated by hand-held pole as mentioned before. During recording, each pose configuration of gimbal is kept for a few seconds on a par with protractor readings as ground truth. In the end, the recorded rosbag of raw video and IMU sequences are utilized to run the SOTA SLAM approaches. All results of 6D pose estimation are exported, and only pitch and roll angles are taken out for comparison. There is no displacement estimation in our approach, so all postend loop closure detection and post-optimization of SLAM cannot be triggered.  It can be inferred from Table I, our approach outperforms the other algorithms consistently by a considerable margin, nearly across all the test sequence. The sequences are named after the simulation movement, sequence with pure roll or pitch rotation and both axis rotations. The roll test has bigger movement range, so we recorded more dense poses in Rosbag. The fairly good performance with least RMSE in most tests should be attributed to the good assumption, skyline and ground plane in the wild, unlike the traditional feature point based SLAM, which is struggling or failing to establish the correspondences between video frames, due to wrong match in feature points. Furthermore, our particle based filter even shows quite robust performance over long-term test, compared to other approaches on "full*" sequences. the particle filter models the non-linearity of this problem rather well, fusing the ego-body IMU measurement and CV observation r obustly to get rid of accumulated drift. "full-03" sequence lasts for more than 20 minutes, it is remarkable that drift error on other approaches in last row of Table I has one order of magnitude increase compared to other rows. Figure 9 further justifies our claim. The test keeps the same setting above by repeating 5 times per each sequence, our approach has mean value at 4.30 degrees, and variance at 0.16, which is smallest amongst the all. Thanks to the natural cue assumption we employed, and adaptive particle filter fusion. The approach can always achieve best accuracy and robustness in any configuration, even invariant over the long period operation. It is noteworthy that the ORBs [9] performs worst, which may be explained by the front-end ORB feature extraction, it looks like the optical flow is more robust compared to handcrafted ORB feature points in such scenario.

VII. CONCLUSION & OUTLOOK
A brand new gimbal system is proposed in this paper, which includes natural geometry primitives tracking at frondend, by using skyline and ground plane. The current frame rotation with respect to the manually set reference frame can be derived from the two lines and normal vectors alignment. Next, a specific particle filter with adaptive resolution based sampling strategy can fuse orientation from both CV and IMU pipelines, even jump between different states according to various lifetime phase of one particle. The final experimental results are implemented on the 3D printed gimbal platform. All simulation tests at lab are performed in realtime on a Jetson Nano. To evaluate the performance of our system, several self-collected sequences with static picture as target were recorded. The rotation disturbance to gimbal is simulated by turning a pole at the other end by hand. Three types of popular SOTA visual-inertial based SLAM methods are chosen to compare, revealing the best accuracy and robustness of our approach amongst all comparisons. Our method can even get rid of accumulative drift issue in traditional visual odometry methods, thanks to a global reference frame in use for all frames' tracking. limitations, Our approach depends on the geometric primitive assumption, for challenging wild, the flatness assumption of ground cannot hold, even the abrupt illumination change and low-level cloud occlusion, all pose great challenge to tracking of those visual cues. The skyline now is simplified into a straight line for easy match and real-time performance, on affordable edge device. In reality, the curved mountains in the view make this assumption quite brittle. The traditional iterative closest point (ICP) can be further applied on image level for match. The 360 fisheye camera with full view should be beneficial to robustness of gimbal platform. Even the gimbal motor and processing unit can be upgraded to fulfill more real-time potential for more high-level tasks.