Cloud-based Control and vslam through Cooperative Mapping and Localization*

Transcription

1 Cloud-based Control and vslam through Cooperative Mapping and Localization* Berat A. Erol, Satish Vaishnav, Joaquin D. Labrado, Patrick Benavidez, and Mo Jamshidi Department of Electrical and Computer Engineering The University of Texas at San Antonio, One UTSA Circle, San Antonio, TX 78249, USA Abstract Simultaneous Localization and Mapping (SLAM) is one of the most widely popular and applied methods designed for more accurate localization and navigation operations. Our experiments showed that vision based mapping helps agents navigate in unknown environments using feature based mapping and localization. Instead of using a classical monocular camera as a vision source, we have decided to use RGB-D (Red, Green, Blue, Depth) cameras for better feature detection, 3D mapping, and localization. This is due to the fact that the RGB-D camera returns depth data as well as the normal RGB data. Moreover, we have applied this method on multiple robots using the concept of cooperative SLAM. This paper illustrates our current research findings and proposes a new architecture based on gathered data from RGB-D cameras, which are the Microsoft Kinect and the ASUS Xtion Pro for 3D mapping and localization. Index Terms Cooperative SLAM, vslam, Robotics, RGB-D, Cloud Computing I. INTRODUCTION GPS-denied localization can be a computational bottleneck for autonomous robotic systems navigating an unknown environment, locating objects of interest, performing automation operations, etc. This problem requires a system to understand the work environment, be aware the obstacles, and map the environment. This is commonly referred as the chicken and egg problem in the literature, since calculating the location requires a map, and to map the area the system should know its current location. Therefore, a system needs to initialize itself before and during the mapping process as well as localize itself simultaneously. Hence, the problem defined as Simultaneous Localization and Mapping (SLAM). The ability to sense the environment is pivotal for any autonomous mobile system that intends to navigate through a known or unknown area. In the past, this was done through the aid of sensors which is returned data that allows the mobile systems to know where they were in the world [1]. Nowadays, there is a easier method to accomplish this task with devices such as Xbox Kinect. a camera provides RGB-D information to the mobile system [2] [4]. RGB-D sensors are now being used to help mobile systems navigate by creating a map of their work environment [1], [5] [8]. * This work was supported by Grant number FA from Air Force Research Laboratory and OSD through a contract with North Carolina Agricultural and Technical State University. Cooperative SLAM is one operational approach that aims to build a common map of entire work environment, and based on a shared data to calculate the locations of the system s agents at the same time. If SLAM operations by one system involves high computational complexity and high power consumption, then the level of the computational complexity and power consumption with multiple systems will be undeniably increased. Therefore, this calls for a solution that will act as a master platform which builds a general map that can be accessed by any system currently in the operation. Additionally, it will process all the sensor data provided by the multiple systems, or any system added for that operation later on. The purpose of this paper is to propose a vision based SLAM system (vslam) includes RGB-D cameras with cloud computing back-end. That is ideal for multiple unmanned ground vehicles (UGV) and aerial vehicles (UAV), and cooperative work flow among them. The algorithm is discussed here in order to better identify key features in the world that could be used in each agents own map, or one that is shared across multiple agents. The complexity of the problem depends on the system and the operation to be carried. Computation of vslam algorithms on a robot is typically not done as it comes with a relatively high cost in computational complexity which in turn affects the power consumption and the positional data update rate. Storage is also an issue as large amounts of data can be acquired for mapping tasks in vslam. Both the storage and computational limits can prohibit extended use of systems relying on vslam. The paper is organized as follows; Section II is the literature survey, which is followed by Section III with proposed architecture. Section IV covers results of the experiments that were discussed, followed by the conclusions, Section V. II. LITERATURE SURVEY In the literature one can see several implementations on cooperative SLAM that use stereo vision camera systems acquiring the data for fusing the cooperative data [5]. Information filters have suitable and convenient characteristics, especially for the multi-sensor data fusing application task. In [6], a vision based SLAM approach is presented using the feedback from RGB-D camera. The features from the

2 color images have been extracted and projected onto the point cloud which was obtained from the depth images. 3D correspondences were used to find correspondences between consecutive images which helps in finding the transformation between the two images. A matrix is used to obtain the map data and hence localization process is done using this map data. In addition to these approaches and research findings, [7] presents a method of loop closing using visual features in conjunction with the laser scan data. The algorithm uses the pose and time information when the image at any given moment matches with the previous images and the scanned patch by the laser at those two moments are compared. Using this information, a covariance is obtained which provides appropriate information for loop closing. For our experiments the implemented algorithm was a Grid Based Fast SLAM by using: a single node, two nodes, four nodes, and eight nodes of a cluster. The execution time was decreased by several orders of magnitude with increasing number of nodes. On the other hand, there were some limitations due to this approach, such as using a separate hardware on server implementations, manual implementation of nodes, as well as neglecting network delays/latencies. A. Feature Detection and Fusion into Map Data The most common method for feature detection in SLAM systems has been SIFT [9], [10], ORB [10], and SURF [1]. These algorithms are widely used for visual odometry, for the building of 3D maps, and to detect objects or points of interest. All three of these techniques are useful in their own right for object recognition. The only difficultly with these algorithms is that each of them require a large amount of images to be stored. This could be handled by the cloud computing by offloading images and running a SIFT or SURF algorithm on the cloud. This is a method that will be tested once the test bed for the system is created. For an encouraging approach, DAVinCi is a Cloud Computing Framework for Service Robots. It acts as Platform as a Service (PaaS), which is designed to perform secondary tasks like mapping, localization and others [11]. Also it works as an active interface between all the heterogeneous agents, the back end computation, and storage through the use of ROS and Hadoop Distributed File System (HDFS). Its role is to act as a master node which runs the ROS name services and maintains the list of publishers and subscribers, as well as receive messages either from the HDFS back end or from other agents/robots. On the other hand, it is important to state that DaVinCi does not provide a real-time implementation. Another method to find features in the mobile systems environment is done through the use of Point Clouds and the Iterative Closest Point (ICP) algorithm [12], [13]. Point clouds allow for systems to use depth information from the RGB-D data stream that the sensor returns. The ICP method is meant to find the smallest distance between two points in a point cloud. This can be done by generating a point cloud and then comparing the individual points. Covariance Intersection (CI) is a method of data fusion which combines two or more states estimates. In addition to this, CI gathers sensor measurements from different platforms having an unknown correlation among them. Improvements in the location determination and mapping process is achieved by using collaborative estimation which is implemented with an information filter and a CI technique. [5] shows less navigation errors by using CI, coping with the memory and computation cost that arises from the increasing size of the augmented state vector matrix, and its corresponding uncertainty, which aims to improve the robustness of Cooperative visual SLAM (vslam). B. Proposed Method for Map Merging For the aim of our experiments, the map points should be transformed from the world coordinate frame to the cameracentered coordinate frame. In other words, we project map points into the image plane. This is done by left-multiplication with a E CW matrix, which represents camera pose [14], E CW stands for a 4x4 transformation matrix from Camera frame C, to World frame W. p jc is the pose of camera in cameracentered coordinate frame, and p jw is the pose of camera in world coordinate frame. Then, p jc = E CW.p jw (1) For visual observation, after tracking a video frame successfully, by using parallel tracking and mapping (PTAM), the pose estimate is scaled by the current estimate for a factor of λ and it is transformed from the front camera coordinate system to the quadrocopters coordinate system, which leads to a direct observation of its pose given by [15], where (x t, y t, z t ) denote the position of the quadcopter in meters in the world coordinates, and (φ t, θ t, ω t ) denote the roll, pitch and yaw angles in degrees, respectively. h p (X t ) := (x t, y t, z t, φ t, θ t, ω t ) T (2) Z p,t := f(e DC.EC, t) (3) Where, E C,t is the estimated camera pose (scaled with λ), E DC is the constant transformation from the camera to the quadcopter, and f is the transformation from the roll-pitchyaw transformation. More details for this stage can be seen in [6], [7], [14] [16] For our experimental studies, two local maps, m 1 and m 2, need to be merged. For this purpose we used point cloud data processing techniques [12], [13]. The first step is to define a global map where this local maps will be placed at appropriate position with properly defined transformations. Then, since two quadrocopters create two local maps, the global map M consists of those two local maps, along with the transformation matrix that containing rotational and translational information of the initial poses of the two quadrocopters. If we construct the T 1 matrix, the transformation Matrix for the first quadcopters, X 01 = (x 01, y 01, z 01, φ 01, θ 01, ω 01 ) defines the initial pose of first agent in Global map M. (x 01, y 01, z 01 ) defines the initial position of the first quadcopter in X, Y and Z directions

3 and (φ 01, θ 01, ω 01 ) is the initial orientation of it with respect to Y, X and Z axis i.e. roll, pitch and yaw. Therefore, Where, T 1 = T x1.t y1.t z1 (4) x 01 T x1 = 0 cosθ 01 sinθ sinθ 01 cosθ cosφ 01 0 sinφ 01 0 T y1 = y 01 sinφ 01 0 cosφ cosω 01 sinω T y1 = sinω 01 cosω z Similar to equation (4), T 2 = T x2.t y2.t z2 is the transformation matrix for the second quadrocopter, and the initial pose of it in the Global map M is X 02 = (x 02, y 02, z 02, φ 02, θ 02, ω 02 ). T x2, T y2 and T z2 are transformation matrices, in similar order as previous ones with respect to x, y and z axis, respectively. On the other hand, the transformation matrix T M [17] is in the form of: [ ] R T T M = (5) 0 1 R defines the rotation about the three axis and T defines the translation with respect to the three axis. The Rotation Matrix constitutes of the following elements: R 11 R 12 R 13 R = R 21 R 22 R 23 R 31 R 32 R 33 Where, R 11 = cosθcosφ R 12 = sinωsinθcosφ cosωsinφ R 13 = cosωsinθcosφ + sinωsinφ R 21 = cosθsinφ R 22 = sinωsinθsinφ + cosωcosφ R 23 = cosωsinθsinφ sinωcosφ R 31 = sinθ R 32 = sinωcosθ R 33 = cosωcosθ The roll angle can be computed as θ=-sin 1 (R 31 ). Moreover, based on the two arguments, the yaw angle can be found by ω=tan 1 (R 32 /R 33 ) or ω=tan 1 [(R 32 /cosθ)/(r 33 /cosθ)] for cosθ < 0 or cosθ > 0, respectively. Lastly, the pitch angle can be calculated from φ=tan 1 [(R 21 /cosθ)/(r 11 /cosθ)]. Furthermore, the final equation of the global map consisting the union of the local maps along with their transformations is M = T 1.m 1 T 2.m 2 (6) III. PROPOSED ARCHITECTURE vslam method will be used in order to detect and identify features in the images that are grabbed by the RGB-D camera. We have used vslam on the cloud to help agents navigate in their environment [4], [18]. This was done through the use of RG-Chromaticity to process and find features in the environment. RG-Chromaticity was used to inspect each pixel for RGB intensity and match it to images stored in the database. This was implemented and tested on a Pioneer2 UGV. The system was guided around the UTSA BSE building 2nd floor and returned to the initial starting position. Using what we have already done we have looked into other methods of feature detection so the system is able to understand its environment efficiently. This technique has some drawbacks due to lighting not being consistent. Fig. 1, shows the general architecture of a cloud VSLAM node using different packages in ROS, such as OpenCV, or the Point Cloud Library (PCL) [12]. ROS is a very useful tool in the creation of this algorithm, as it is designed to handle all the sensor data for the system [16]. The work flow for merging the local maps and creating a global map can be explained as: Obtain local maps by using ROS package. Apply PC smoothing on maps to remove the noisy data. Define a global map M. Apply transformation matrix on local maps. Use the map forming equation (6) to get M. Apply point cloud registration for the two local maps including transformations to obtain the merged map. Apply point cloud registration technique between the maps to merge the two local maps into a global map. Fig. 1: General architecture of a cloud VSLAM node. A. Hardware The RGB-D camera selected for this research is an ASUS Xtion Pro Live sensor. An ODROID-XU4 ARM-based octacore mini computer running Ubuntu XU4 Robotics Edition is used to interface with the RGB-D camera [19].

4 Our system will consist of a UGV, Kobuki TurtleBot2, equipped with an RGB-D sensor, and a ROS ready ODROID- XU4. ROS will allow us to control the TurtleBot2 and process all RGB-D data as well as sensor data, Fig. 2. This setup will allow the UGV to use a vslam algorithm for navigation. Next, we will find features for the vslam algorithm, and pass any features detected to a program designed for localization. Thus, the system can know where it is in its work environment. Our algorithm will decide if an old feature is the same as a newly detected feature. This is important for the system, since it will have to generate a map of the environment for future use. This process will be repeated as the system navigates towards the specified goal to create a more complete map. Fig. 4: Overview of Cooperative Mapping and Localization algorithm IV. EXPERIMENTAL RESULTS This section presents experimental results from a VSLAM algorithm that uses a monocular RGB camera for vision input. Our research lab is the work space for Case 1 and is illustrated in Fig. 3. Fig. 5: The Feature matching technique used for localization Fig. 2: System integration and architecture for the Robots point of view illustrated with ROS messages. Fig. 3: Our experimental setup for quadcopters positions. Two quadcopters are placed in opposite directions. The cameras were facing towards to arrows. Geometric shapes are tables, workstations and chairs. A. Cooperative 3D Mapping Images captured by the monocular cameras of the two quadcopters, and placing them into a global map; then, the features are used to localize the robots as illustrated in Fig. 4. Then, localization technique uses feature matching between the point cloud (PC) data, specifically latest Cooperative Mapping that leads to create the Global Map, of the current view and the global map as shown in Fig. 5. The results in Fig. 6 shows the mapped areas, our research lab, by the two cameras of the quadcopters. Map1 stands for the feedback from the first quadcopter where the points colored as red, and the Map2 from the second quadcopter with white points. Please refer to Fig. 4 for comparing the image frames. In this case, one of the quadrotors initial pose coincides with the origin of Global Frame while the initial pose of the second quadrotor is (0,-1,0,0,0,180). Map1, point cloud feedback from the first quadrotor, and Map2, input from the second quadrotor, are filtered and transformed based on their initial poses and since the first cameras initial pose coincides with the origin, there is no transformation observed. Fig. 7 shows the results of the global map when they are

5 Fig. 6: Map1(red) and Map2(white) before (above) and after (below) filtering for Case 1, our research lab. Fig. 8: Global Map finalized after transformation for Case 2. Fig. 7: Map1 and Map2 are merged in Case 1. combined and then merged. For instance, in Case 2, the initial pose of the first quadcopter is (0,0,0,0,45,0) and the initial pose of the second quadcopter is (0,-0.5,0,0,0,90). For this experimental case, we have chosen a different environment with almost no obstacles around, such as desks and chairs, which has resulted with more precise mapping with lower number of points to filter and lesser noisy map as expected. In Fig. 8, the two maps shows the process of mapping followed by the transformation. Later, these two maps are combined and merged. Fig. 9: Feature Matching for localization of the quadcoptor before (above) and after filtering. Filtering applied with removal of bad matches. B. Self Localization Fig. 9 shows mapping results of the robot before and after filtering out the bad correspondences. The positioning explains the location of the robot, and the 3D pose estimates are the outputs of the Kalman Filter, when the sensor readings incorporates all the correspondences. After neglecting the bad correspondences, the localization of the robot has resulted in better approximation. As we

6 discussed previously considering the yaw angle, the good estimates for correspondences were the outputs of the Kalman Filtering, when the sensor readings incorporates only the good correspondences. Our results can be seen in Fig. 10. Fig. 10: Kalman Filter estimation for the x-coordinates of the quadcopter (above), and the yaw angle estimation after filtering out the bad correspondences. V. CONCLUSION The work done till now builds the map after gathering the data by performing the data processing operations of smoothing and registration i.e. the map is built offline. Also, the localization module estimates the location of the quadcopter in the global map built offline. Hence, one of the immediate future work involves building a framework to perform the operation of cooperative mapping and localization simultaneously. The next future work will be to test the algorithm of cooperative mapping with a RGB-D sensor like the Kinect which might signify the importance of the algorithm in a better way with faster map building operations and much better accuracy. Also, the localization operation will be much faster and more accurate. Cooperative SLAM operations involves dealing with high amount of data due to the huge number of point clouds involved in building the Global Map using the local maps obtained from the quadcopters. As the mapping area increases, using the cooperative operation becomes more sensible so as the map the area faster and also with better accuracy with data fusion.also, better sensors leads to better accuracy. This implies that a cooperative SLAM operation with better sensors will lead to tremendous amount of point cloud data which will practically involve huge amounts of computation and high processing power. This calls in for the use of Cloud Computing which takes care of the high computation and processing power requirements. REFERENCES [1] M. Du, J. Wang, J. Li, H. Cao, G. Cui, J. Fang, J. Lv, and X. Chen, Robot robust object recognition based on fast surf feature matching, in Chinese Automation Congress (CAC), IEEE, 2013, pp [2] F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard, 3-d mapping with an rgb-d camera, Robotics, IEEE Transactions on, vol. 30, no. 1, pp , [3] M. Paton and J. Kosecka, Adaptive rgb-d localization, in Computer and Robot Vision (CRV), 2012 Ninth Conference on. IEEE, 2012, pp [4] P. Benavidez, M. Muppidi, P. Rad, J. Prevost, M. Jamshidi, and L. Brown, Cloud-based realtime robotic visual slam, in Systems Conference (SysCon), th Annual IEEE International, April 2015, pp [5] X. Li and N. Aouf, Experimental research on cooperative vslam for uavs, in Computational Intelligence, Communication Systems and Networks (CICSyN), 2013 Fifth International Conference on. IEEE, 2013, pp [6] F. Endres, J. Hess, N. Engelhard, J. Sturm, D. Cremers, and W. Burgard, An evaluation of the rgb-d slam system, in Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012, pp [7] P. Newman and K. Ho, Slam-loop closing with visually salient features, in Robotics and Automation, ICRA Proceedings of the 2005 IEEE International Conference on. IEEE, 2005, pp [8] G. Bradski et al., The opencv library, Doctor Dobbs Journal, vol. 25, no. 11, pp , [9] D. Wang and D. Liu, Sift-preserving compression of mobile-captured license plate images for recognition, in Wireless Communications and Signal Processing (WCSP), 2014 Sixth International Conference on. IEEE, 2014, pp [10] Y. Qin, H. Xu, and H. Chen, Image feature points matching via improved orb, in Progress in Informatics and Computing (PIC), 2014 International Conference on. IEEE, 2014, pp [11] R. Arumugam, V. R. Enti, L. Bingbing, W. Xiaojun, K. Baskaran, F. F. Kong, K. D. Meng, G. W. Kit et al., Davinci: A cloud computing framework for service robots, in Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010, pp [12] R. B. Rusu and S. Cousins, 3D is here: Point Cloud Library (PCL), in IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, May [13] B. Bellekens, V. Spruyt, R. B. M. Weyn, and R. Berkvens, A survey of rigid 3d pointcloud registration algorithms, in The Fourth International Conference on Ambient Computing, Applications, Services and Technologies. Citeseer, [14] G. Klein and D. Murray, Parallel tracking and mapping for small ar workspaces, in Mixed and Augmented Reality, ISMAR th IEEE and ACM International Symposium on. IEEE, 2007, pp [15] J. Engel, J. Sturm, and D. Cremers, Camera-based navigation of a low-cost quadrocopter, in Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012, pp [16] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, Ros: an open-source robot operating system, in ICRA workshop on open source software, vol. 3, no. 3.2, 2009, p. 5. [17] G. G. Slabaugh, Computing euler angles from a rotation matrix. [Online]. Available: sbbh653/publications/euler.pdf [18] P. Benavidez, M. Muppidi, and M. Jamshidi, Improving visual slam algorithms for use in realtime robotic applications, in World Automation Congress (WAC), 2014, Aug 2014, pp [19] ODROID Forum View topic - Ubuntu Robotics Edition for ODROID-XU3 (ROS+OpenCV+PCL). [Online]. Available: