Recent robotic manipulation competitions have highlighted that sophisticated robots still struggle to achieve fast and reliable perception of task-relevant objects in complex, realistic scenarios. To improve these systems' perceptive speed and robustness, we present SegICP, a novel integrated solution to object recognition and pose estimation. SegICP couples convolutional neural networks and multi-hypothesis point cloud registration to achieve both robust pixel-wise semantic segmentation as well as accurate and real-time 6-DOF pose estimation for relevant objects. Our architecture achieves 1 cm position error and <; 5° angle error in real time without an initial seed. We evaluate and benchmark SegICP against an annotated dataset generated by motion capture.

Colorectal cancer is one of the leading causes of cancer-related deaths worldwide, although it can be effectively treated if detected early. Teleoperated flexible endoscopes are an emerging technology to promote participation in these preventive screenings. Real-time pose estimation is therefore essential to enable feedback to the robotic endoscope's control system. Vision-based endoscope localization approaches are a promising avenue, since they do not require extra sensors on board the endoscopes. In this work, we compare several state-of-the-art algorithms for computing the image motion (optical flow), which is then used with a supervised learning strategy to provide an accurate estimate of the 6 degree of freedom endoscope motion. The method is validated using a robotically actuated endoscope in a human colon simulator, and represents a preliminary effort towards testing with clinical video data.

Augmented-Reality (AR) displays increase surgeon's visual awareness of high-risk surgical targets (e.g., the location of a tumor) by accurately overlaying pre-operative radiological 3-D model onto the intra-operative laparoscopic video. Existing AR systems lack in accuracy and robustness against frequent illumination changes, camera motions, and organ occlusions, which rapidly cause the loss of image (anchor) points, and thus the loss of the AR display after a few seconds. In this paper, we present a new AR system, which represents the first step toward long term and accurate augmented surgical display. Our system leverages feature matching to automatically recover the overlay by predicting the image locations of a high number of anchor points that were lost after a sudden image change. Additionally, a weighted sliding-window least-squares approach is also used to increase the accuracy of the AR display over time. The effectiveness of the proposed system in maintaining a long term, stable, and accurate augmentation has been tested over a set of real partial-nephrectomy laparascopic monocular videos from a DaVinci surgical robot.

This paper proposes a novel active target tracking strategy for a team of cooperating quadrotors equipped with 3-D range-finding sensors. The work builds upon previous research of the authors, and adopts a realistic nonlinear dynamic model for the quadrotors. A hierarchical controller is designed for the generation and tracking of the desired optimal trajectories of the aerial vehicles, and a discrete-time Kalman filter is used for fusing their local estimates of the target position. Under suitable conditions, it is shown that the cost function for the D-optimality criterion that the quadrotors aim at collaboratively reduce, possesses a single global minimum and no local minima. Numerical simulations and real-world experiments show the effectiveness of the proposed control strategy.

The ability to find image similarities between two distinct endoscopic views is known as feature matching, and is essential in many robotic-assisted minimally-invasive surgery (MIS) applications. Differently from feature-tracking methods, feature matching does not make any restrictive assumption about the chronological order between the two images or about the organ motion, but first obtains a set of appearance-based image matches, and subsequently removes possible outliers based on geometric constraints. As a consequence, feature-matching algorithms can be used to recover the position of any image feature after unexpected camera events, such as complete occlusions, sudden endoscopic-camera retraction, or strong illumination changes. We introduce the hierarchical multi-affine (HMA) algorithm, which improves over existing feature-matching methods because of the larger number of image correspondences, the increased speed, and the higher accuracy and robustness. We tested HMA over a large (and annotated) dataset with more than 100 MIS image pairs obtained from real interventions, and containing many of the aforementioned sudden events. In all of these cases, HMA outperforms the existing state-of-the-art methods in terms of speed, accuracy, and robustness. In addition, HMA and the image database are made freely available on the internet.

The ability to find similar features between two distinct views of the same scene (feature matching) is essential in robotics and computer vision. We are here interested in the robotic-assisted minimally-invasive surgical scenario, for which feature matching can be used to recover tracked features after prolonged occlusions, strong illumination changes, image clutter, or fast camera motion. In this paper we introduce the Hierarchical Multi-Affine (HMA) feature-matching algorithm, which improves over the existing methods by recovering a larger number of image correspondences, at an increased speed and with a higher accuracy and robustness. Extensive experimental results are presented that compare HMA against existing methods, over a large surgical-image dataset and over several types of detected features.

We present a new strategy for active vision-based localization and navigation of a mobile robot in a visual memory, i.e., within a previously-visited area represented as a large collection of images. Vision-based localization in such a large and dynamic visual map is intrinsically ambiguous, since more than one map-locations can exhibit the same visual appearance as the current image observed by the robot. Most existing approaches are passive, i.e., they do not devise any strategy to resolve this ambiguity. In this work, we present an active vision-based localization and navigation strategy that can disambiguate the true initial location among possible hypotheses by controlling the mobile observer across a sequence of highly distinctive images, while concurrently navigating towards the target image. The performance of our active localization and navigation algorithm is demonstrated experimentally on a robot moving within a large outdoor environment.

This paper presents a new cooperative active target-tracking strategy for a team of double-integrator aerial vehicles equipped with 3-D range-finding sensors. Our strategy is active because it moves the vehicles along paths that minimize the combined uncertainty about the target's position. We propose a gradient-based control approach that encompasses the three major optimum experimental-design criteria and relies on the Kalman filter for estimation fusion. We derive analytical lower and upper bounds on the target's position uncertainty by exploiting the monotonicity property of the Riccati differential equation arising from the Kalman-Bucy filter. These bounds allow us to study the impact of sensors' accuracy and target's dynamics on the steady-state performance of our coordination algorithm. Finally, in the case that the position of the vehicles is not perfectly known, we introduce a more challenging problem, termed Active Cooperative Localization and Multi-target Tracking (ACLMT). In this problem, the vehicles move in the 3-D space in order to maximize the accuracy of their own position estimate and that of multiple moving targets.

This paper proposes a new cooperative active target-tracking strategy for a team of heterogeneous robots equipped with 3-D range-finding sensors. Our strategy is active, in the sense that the robots will track one or multiple moving targets while minimizing the combined uncertainty about the targets' position. We introduce a gradient-based control approach that encompasses the three major optimum experimental design criteria and relies only on robots' relative position measurements. The Kalman-Bucy filter is used for estimation fusion. Applications of the proposed strategy are shown to an experimental scenario featuring a team of double-integrator aerial vehicles and nonholonomic ground robots cooperatively tracking the motion of a human subject for a gait-monitoring task.

We present our novel Adaptive Multi-Affine (AMA) feature-matching algorithm that finds correspondences between two views of the same non-planar object. The proposed method only uses monocular images to robustly match clusters of 2-D features according to their relative position on the object surface; finally, AMA adaptively finds the best number of clusters that maximizes the number of matching features. We use AMA to recover a feature tracker from failure (e.g., loss of points due to occlusions or deformations), by robustly matching the features in the images before and after such events. This is paramount in Augmented-Reality (AR) systems for Minimally-Invasive Surgery (MIS) to cope for frequent occlusions and organ deformations that can cause the tracked image-points to drastically reduce (or even disappear) in the current video. We validated our approach on a large set of MIS videos of partial-nephrectomy surgery; AMA achieves an increased number of matches, as well as a reduced feature-matching error when compared to state-of-the-art method.

We aim to perform robust and fast vision-based localization using a pre-existing large map of the scene. A key step in localization is associating the features extracted from the image with the map elements at the current location. Although the problem of data association has greatly benefited from recent advances in appearance-based matching methods, less attention has been paid to the effective use of the geometric relations between the 3D map and the camera in the matching process. In this paper we propose to exploit the geometric relationship between the 3D map and the camera pose to determine the visibility of the features. In our approach, we model the visibility of every map feature with respect to the camera pose using a non-parametric distribution model. We learn these non-parametric distributions during the 3D reconstruction process, and develop efficient algorithms to predict the visibility of features during localization. With this approach, the matching process only uses those map features with the highest visibility score, yielding a much faster algorithm and superior localization results. We demonstrate an integrated system based on the proposed idea and highlight its potential benefits for the localization in large and cluttered environments.

This paper presents a novel 3D indoor Laser-aided Inertial Navigation System (L-INS) for the visually impaired. An Extended Kalman Filter (EKF) fuses information from an Inertial Measurement Unit (IMU) and a 2D laser scanner, to concurrently estimate the six degree-of-freedom (d.o.f.) position and orientation (pose) of the person and a 3D map of the environment. The IMU measurements are integrated to obtain pose estimates, which are subsequently corrected using line-to-plane correspondences between linear segments in the laser-scan data and orthogonal structural planes of the building. Exploiting the orthogonal building planes ensures fast and efficient initialization and estimation of the map features while providing human-interpretable layout of the environment. The L-INS is experimentally validated by a person traversing a multistory building, and the results demonstrate the reliability and accuracy of the proposed method for indoor localization and mapping.

This paper presents a strategy for descending-stair detection, approach, and traversal using inertial sensing and a monocular camera mounted on an autonomous tracked vehicle. At the core of our algorithm are vision modules that exploit texture energy, optical flow, and scene geometry (lines) in order to robustly detect descending stairwells during both far- and near-approaches. As the robot navigates down the stairs, it estimates its three-degrees-of-freedom (d.o.f.) attitude by fusing rotational velocity measurements from an on-board tri-axial gyroscope with line observations of the stair edges detected by its camera. We employ a centering controller, derived based on a linearized dynamical model of our system, in order to steer the robot along safe trajectories. A real-time implementation of the described algorithm was developed for an iRobot Packbot, and results from real-world experiments are presented.

Vision-based control of a robot formation is challenging because the on-board sensor (camera) only provides the view-angle to the other moving robots, but not the distance that must be estimated. In order to guarantee a consistent estimate of the distance by knowing the control inputs and the sensor outputs in a given interval, the nonlinear multi-robot system must preserve its observability. Recent theoretical studies on leader-follower robot formation exploit the interesting influence that the control actions have on observability. Based on these results, in this paper we present a switching active control strategy for formation control. Our control strategy is active in the sense that, while asymptotically achieving the formation control tasks, it also guarantees the system observability in those cases in which all the robots tend to move along non-observable paths. As a result, both estimation and formation performances are improved. Extensive simulation results show the effectiveness of the proposed design.

Planar catadioptric stereo vision sensors (PCS) combine a pinhole camera with two or more planar mirrors. PCS have recently received an increasing attention since a stereo view can be easily obtained without the need of exact multi-camera synchronization and calibration. In this paper we present a rigorous analytical treatment of the imaging geometry of PCS, propose new mirror calibration algorithms and introduce new multi-view properties that can be used for eye-in-hand camera localization. The effectiveness of the algorithms is shown via extensive simulation and real-data experiments on a robotic manipulator.

This paper presents an indoor localization system for the visually impaired. The basis of our system is an Extended Kalman Filter (EKF) for six degree-of-freedom (d.o.f.) position and orientation (pose) estimation. The sensing platform consists of an Inertial Measurement Unit (IMU) and a 2D laser scanner. The IMU measurements are integrated to obtain pose estimates which are subsequently corrected using line-to-plane correspondences between linear segments in the laser-scan data and known 3D structural planes of the building. Furthermore, we utilize Lie derivatives to show that the system is observable when at least three planes are detected by the laser scanner. Experimental results are presented that demonstrate the reliability of the proposed method for accurate and realtime indoor localization.

This paper deals with vision-based localization for leader–follower formation control. Each unicycle robot is equipped with a panoramic camera that only provides the view angle to the other robots. The localization problem is studied using a new observability condition valid for general nonlinear systems and based on the extended output Jacobian. This allows us to identify those robot motions that preserve the system observability and those that render it nonobservable. The state of the leader–follower system is estimated via the extended Kalman filter, and an input-state feedback control law is designed to stabilize the formation. Simulations and real-data experiments confirm the theoretical results and show the effectiveness of the proposed formation control.

The paper introduces a new vision-based range estimator based upon the Immersion and Invariance (I&I) methodology, for leader-follower formation control. The proposed reduced-order nonlinear observer achieves global exponential convergence of the observation error to zero and it is extremely simple to implement and to tune. A Lyapunov analysis is provided to show the stability of the closed-loop system arising from the combination of the range estimator and an input-state feedback controller. Simulation experiments illustrate the theory and show the effectiveness of the proposed design.

This paper focuses on leader-follower formations of mobile robots equipped with panoramic cameras and extend earlier works in the literature addressing both the vision-based localization and control problems. First, a new sufficient analytical condition for localizability is proved and used to shed light on the geometrical meaning of formation localization using uncalibrated vision sensors, here performed with the unscented Kalman filter. Second, we design a feedback control law based on dynamic extension in order to extend the applicability of our control scheme also to the case of distant robots.

Due to their panoramic field of view, catadioptric cameras are becoming ubiquitous in many robotics and computer vision applications. Catadioptric cameras with a unique viewpoint are of primary interest, among these is the case where the reflective surface is a parabolic mirror and the camera satisfies the orthographic projection model and which we call paracatadioptric. We here present a new geometrical property (called disparity-circles property) for paracatadioptric cameras and use it to design a video compass algorithm that can compute the 1-axis rotation angle between two views of at least two parallel lines, without any need of internal camera calibration parameters. We present a linear closed form solution suitable for real-time implementation. We then study the performances of our algorithm, its sensitivity to image noise and propose also experimental results with a paracatadioptric camera mounted on a mobile robotic platform.

For a human-robot interface it is important to have a good model of how the human subject operates. However, since such a model is difficult to obtain, then the robotics interface must observe accurately the subject's behaviour when interacting with him. We present here a new human-robot interface for active interaction with the cognitive and emotional human domains. Since eye movements convey a lot of information about one subject's cognitive and emotive status, we have designed a new human-robot interface which uses a video-based Eye-Tracker (ET) to observe the subject's line of gaze. Since we are also interested in using our interface for studying and treating depression, our interface can send stimulating inputs to the subject using both a Transcranial Magnetic Stimulator (TMS) and a visual stimulus. The latter elicits the subject's emotions and consists of a set of pictures of facial expressions, which have been shown according to a novel visualization protocol, called Memory-Guided Filtering (MGF). Its effectiveness has been verified by means of many experimental results. We also present the application of our human-robot interface for preliminary studies concerning new cognitive rehabilitation strategies in depression.

We present an image-based visual servoing strategy for driving a nonholonomic mobile robot equipped with a pinhole camera toward a desired configuration. The proposed approach, which exploits the epipolar geometry defined by the current and desired camera views, does not need any knowledge of the 3-D scene geometry. The control scheme is divided into two steps. In the first, using an approximate input-output linearizing feedback, the epipoles are zeroed so as to align the robot with the goal. Feature points are then used in the second translational step to reach the desired configuration. Asymptotic convergence to the desired configuration is proven, both in the calibrated and partially calibrated case. Simulation and experimental results show the effectiveness of the proposed control scheme

We present an image-based visual servoing strategy for nonholonomic mobile robot equipped with a central catadioptric camera. This kind of vision sensor combines lens and mirrors to enlarge the field of view. The proposed approach, which exploits the epipolar geometry defined by the current and the desired camera views, does not need any knowledge of the 3-D scene geometry. The control scheme is divided in two steps. In the first one, the epipoles are used together with an approximate input-output linearizing feedback to align the robot with the goal. Feature points are then used in the second translation step to reach the desired configuration. Global asymptotic convergence is proven. Simulation and experimental results show the effectiveness of the proposed control scheme

This paper presents an epipolar based visual servoing for mobile robots equipped with a panoramic camera. The proposed visual servoing is based on the epipolar geometry and exploits the auto-epipolar property, a special configuration for the epipoles which occurs when the desired and the current views undergo a pure translation. This occurrence is detectable observing when the bi-osculating mirror conics co-intersect at the two epipoles. The auto epipolar condition enables our controller to retrieve the equal orientation between target and current camera. Translation is performed by exploiting the epipoles. Simulated experiments and Lyapunov-based stability analysis demonstrate the parametric robustness of the proposed method.

A new image-based visual servoing algorithm is presented for nonholonomic mobile robots. The algorithm, based on epipolar geometry, consists of three independent and sequential steps making use of both the estimated epipoles and the image features. In particular, due to the nonlinear dynamics of the camera-robot system, an input-output feedback linearizing control law is used during the second step. Simulations results are presented to validate the proposed visual servoing technique.