Robot-to-Robot Relative Pose Estimation using Humans as Markers

TL;DR

This paper introduces a novel method for estimating the 3D relative pose between robots using human pose key-points as markers, leveraging visual detection, re-identification, and iterative refinement in diverse environments.

Contribution

The paper presents a new approach combining human pose detection, re-identification, and iterative optimization for robot relative pose estimation using humans as markers.

Findings

Effective in terrestrial and underwater environments

Accurate key-point correspondence improves pose estimation

Feasible for multi-robot systems in human-centric settings

Abstract

In this paper, we propose a method to determine the 3D relative pose of pairs of communicating robots by using human pose-based key-points as correspondences. We adopt a 'leader-follower' framework, where at first, the leader robot visually detects and triangulates the key-points using the state-of-the-art pose detector named OpenPose. Afterward, the follower robots match the corresponding 2D projections on their respective calibrated cameras and find their relative poses by solving the perspective-n-point (PnP) problem. In the proposed method, we design an efficient person re-identification technique for associating the mutually visible humans in the scene. Additionally, we present an iterative optimization algorithm to refine the associated key-points based on their local structural properties in the image space. We demonstrate that these refinement processes are essential to establish accurate key-point correspondences across viewpoints. Furthermore, we evaluate the performance of the proposed relative pose estimation system through several experiments conducted in terrestrial and underwater environments. Finally, we discuss the relevant operational challenges of this approach and analyze its feasibility for multi-robot cooperative systems in human-dominated social settings and feature-deprived environments such as underwater.

Tables

Table 1. TABLE I : A quantitative performance comparison for various person ReId models on standard datasets; a set 150 150 150 test images are used for comparison from each dataset.

Person ReId	Market-1501 Dataset		CUHK-03 Dataset		FPS
models	Rank-1 Acc. ($\%$)	MAP ($\%$)	Rank-1 Acc. ($\%$)	MAP ($\%$)	(Jetson TX2)
Aligned ReId	$92.90$	$90.12$	$67.15$	$68.03$	$0.67$
Deep person ReId	$85.86$	$68.24$	$62.26$	$65.15$	$0.33$
Tripled-loss ReId	$85.25$	$74.88$	$72.75$	$60.27$	$0.62$
Proposed person ReId	$75.67$	$72.26$	$57.82$	$54.91$	7.45

Equations

$$SSIM(\mathbf{x},\mathbf{y})=l(\mathbf{x},\mathbf{y})c(\mathbf{x},\mathbf{y})s(\mathbf{x},\mathbf{y})=\frac{2{\mu }_{\mathbf{x}}{\mu }_{\mathbf{y}}}{{\mu }_{\mathbf{x}}^2+{\mu }_{\mathbf{y}}^2}\times \frac{2{\sigma }_{\mathbf{xy}}}{{\sigma }_{\mathbf{x}}^2+{\sigma }_{\mathbf{y}}^2}.$$

$$SSIM(\mathbf{x},\mathbf{y})=\frac{2{\mu }_{\mathbf{x}}{\mu }_{\mathbf{y}}+c_1}{{\mu }_{\mathbf{x}}^2+{\mu }_{\mathbf{y}}^2+c_1}\times \frac{2{\sigma }_{\mathbf{xy}}+c_2}{{\sigma }_{\mathbf{x}}^2+{\sigma }_{\mathbf{y}}^2+c_2}.$$

$$L({\mathbf{p}}_l,{\mathbf{p}}_f)=1-SSIM({\varphi }_{I_{leader}}({\mathbf{p}}_l),{\varphi }_{I_{follower}}({\mathbf{p}}_f)).$$

$${\mathbf{p}}_f^{\ast }=\underset{\mathbf{p}}{\mathrm{argmin}}L({\mathbf{p}}_l^0,\mathbf{p})\text{s. t.}||\mathbf{p}-{\mathbf{p}}_f^0|{|}_{\infty }<32.$$

$${\mathbf{p}}_f^{t+1}={\mathbf{p}}_f^t-\eta \cdot \nabla L({\mathbf{p}}_l^0,{\mathbf{p}}_f^t).$$

$${\mathbf{T}}_f^l=\underset{{\mathbf{T}}_f^l}{\mathrm{argmin}}||{\mathbf{p}}_f-{\mathbf{K}}_f{\mathbf{T}}_f^l{\mathbf{x}}_l|{|}^2.$$

Full text

I Introduction

Accurate computation of relative pose is essential in multi-robot estimation problems such as cooperative tracking, localization [1], planning, mapping [2], and more. Unless global positioning information (e.g., GPS, USBL) is available, the robots need to estimate their positions and orientations relative to each other based on their exteroceptive sensory measurements and noisy odometry [3]. This process is necessary for registering their measurements to a common frame of reference in order to maintain coordination during task execution.

In a cooperative setting, robots with visual sensing capabilities solve the relative pose estimation problem by triangulating mutually visible local features and landmarks. A lack of salient features significantly affects the accuracy of this estimation [4], which eventually hampers the overall success of the operation. Such difficulties often arise in poor visibility conditions underwater due to a lower number of salient features and natural landmarks [5, 6]. Nevertheless, proximity of human divers to robots is a fairly common occurrence in applications [7] at shallow depths. Besides, humans are frequently present and visible in many social scenarios [8, 9] where natural landmarks are not reliably identifiable due to repeated textures, noisy visual conditions, etc. Hence, the problem of having limited natural landmarks can be alleviated by using mutually visible humans as markers (i.e., features correspondences), particularly in human-robot collaborative applications. Despite the potential, the feasibility of using human presence or body-pose for robot-to-robot relative pose estimation has not been explored in the literature.

In this paper, we propose a method for computing six degrees-of-freedom ($6$-DOF) robot-to-robot transformation between pairs of communicating robots by using mutually detected humans’ pose-based key-points as correspondences. As illustrated in Fig. 1, we adopt a leader-follower framework where one of the robots (equipped with a stereo camera) is assigned as a leader. First, the leader robot detects and triangulates 3D positions of the key-points in its own frame of reference. Then the follower robot matches the corresponding 2D projections on its intrinsically calibrated camera and localizes itself by solving the perspective-n-point (PnP) problem [10]. It is to be noted that this entire process of extrinsic calibration is automatic and does not require prior knowledge about the robots’ initial positions. Additionally, it is straightforward to extend the leader-follower framework for multi-robot teams from the pairwise solutions. Furthermore, if the leader robot has global positioning information, i.e., has a GPS or an USBL receiver, the follower robots can use that information to localize themselves in the global frame as well.

In addition to the conceptual design, we present an end-to-end system with efficient solutions to the practicalities involved in the proposed robot-to-robot pose estimation method. As mentioned, we use OpenPose [11] for detecting human body-poses in the image space. Although it provides reliable detection performance, the extracted 2D key-points across different views do not necessarily associate as a correspondence. We propose a twofold solution to this:

• •

First, we design an efficient person re-identification module by evaluating the hierarchical similarities of the key-point regions in the image space. It takes advantage of the consistent human pose structures across viewpoints and evaluates their pair-wise similarities for fast body-pose association. We also demonstrate that the state-of-the-art (SOTA) appearance-based person re-identification models fail to provide acceptable performance under single-board real-time constraints.

• •

Subsequently, we formulate an iterative optimization algorithm to refine the noisy key-point correspondences by further exploiting their local structural properties in respective images. We demonstrate that the pair-wise key-point refinement is crucial to ensure their validity in a perspective geometric sense.

This two-stage process facilitates efficient and robust key-point associations across viewpoints for accurate robot-to-robot relative pose estimation. In this paper, we primarily focus on these two novel modules because the rest of the computational aspects are generic to all multi-robot cooperative pose estimation systems. Nevertheless, we present a fast implementation of the proposed system and evaluate its end-to-end performance over several terrestrial and underwater field experiments. Lastly, we analyze its practical feasibility and discuss various operational considerations.

II Related Work

II-A Robot-to-robot Relative Pose Estimation

The problem of robot-to-robot relative pose estimation has been thoroughly studied for 2D planar robots, particularly for range and bearing sensors. Analytic solutions for determining $3$-DOF robot-to-robot transformation using mutual distance and/or bearing measurements involve solving an over-determined system of nonlinear equations [3, 12]. Similar solutions for the 3D case, i.e., for determining 6-DOF transformation using inter-robot distance and/or bearing measurements, has been proposed as well [13, 14]. In practice, these analytic solutions are used as an initial estimate for the relative pose, and then iteratively refined by optimization techniques (e.g. nonlinear weighted least-squares) to account for the noisy observation and uncertainty in robot motion.

Robots that rely on visual perception (i.e., use cameras as exteroceptive sensors) solve the relative pose estimation problem by triangulating mutually visible features and landmarks [15]. Therefore, it reduces to solving the PnP problem by using sets of 2D-3D correspondences between geometric features and their projections on respective image planes [10]. Although high-level geometric features (e.g., lines, conics) have been proposed, point-based features are typically used in practice for relative pose estimation [16]. Moreover, the PnP problem is solved either using iterative approaches by formulating the over-constrained system ($n$ $>3$) as a nonlinear least-squares problem, or by using sets of three non-collinear points ($n=$ $3$) in combination with Random Sample Consensus (RANSAC) to remove outliers [17]. Besides, vision-based approaches often use temporal-filtering methods, the extended Kalman-filter (EKF) in particular, to reduce the effect of noisy measurements in order to provide near-optimal pose estimates [15, 16]. On the other hand, it is also common to simplify the relative pose estimation by attaching specially designed calibration-patterns on each robot [18]. However, this requires that the robots operate at a sufficiently close range, and remain mutually visible.

II-B Human Body-Pose Detection

Visual detection of 2D human pose has made significant progress over the last decade. The SOTA methodologies can be categorized into the top-down and bottom-up approaches. The top-down approaches [19, 20] detect the humans in the image space first, and then perform localization and association of their body-parts. One major limitation of these approaches is that their run-times are proportional to the number of persons in the image. Additionally, the robustness of the pose estimation largely depends on the accuracy of their person detectors. In contrast, the bottom-up approaches [11, 21] do not suffer from these two issues. However, they require solving a more computationally challenging inference problem of learning global contextual cues for simultaneous body-part detection and association.

The classical approaches typically use pictorial structures [22, 23] to model the appearance of human body-parts. A set of densely sampled shape descriptors are used for localizing the body-parts and then classifiers such as AdaBoost, SVMs, etc., are used for detection. Associating the detected body-parts is rather challenging; a mixture of tree-based models are typically used to learn separate pairwise relationships for different body-part configurations [24]. Graph-based connectivity models are then used to formulate the inference (association) as a graph-cut problem. These pairwise connectivity models can be further generalized [25] to capture the anatomical relationships among multiple body-parts. Recently proposed approaches use Deep Neural Networks (DNNs) to learn the human pose detection from large training datasets to perform fast and accurate global inference. DeepPose [26], for instance, formulates the problem as a regression problem and uses a cascade of DNNs to learn the inference in a holistic fashion. On the other hand, OpenPose [11] jointly learns to detect and associate using pose machines [27]. In contrast to DNNs, each module of a pose machine is trained locally; the sequential predictions of these modules are then refined to perform a hierarchical joint inference. Such hierarchical structures facilitate fast inference for multi-person pose estimation in addition to achieving SOTA performance. Due to these compelling reasons, we use OpenPose in this work.

II-C Human-aware Robot Control

Human-awareness is important for autonomous mobile robots operating in social settings and human-robot collaborative applications. A large body of literature and systems exist [7, 28] which focus on the areas of understanding human motion, instructions, behaviors, etc. Additionally, tracking human pose relative to a robot is particularly common in applications such as person tracking or following [8, 29], collaborative manipulation [30], behavior imitation [31], etc. However, the feasibility of using humans’ presence or their body-poses as markers for robot-to-robot relative pose estimation has not been explored in the literature.

III System Design and Methodology

As shown in Fig. 2, the proposed robot-to-robot relative pose estimation system incorporates several computational components: detection of human body-poses in images captured from different views (by leader and follower robots), pair-wise association of the detected humans across viewpoints, geometric refinement of the key-point correspondences, and 3D pose estimation of the follower robot relative to the leader. We present their methodological details and relevant design choices in the following sections.

III-A Human Body-Pose Detection

OpenPose [11] is an open-source library for real-time multi-human 2D pose detection in images, originally developed using Caffe and OpenCV libraries111github.com/CMU-Perceptual-Computing-Lab/openpose. We use a Tensorflow implementation222github.com/ildoonet/tf-pose-estimation based on the MobileNet model that provides faster inference compared to the original model (also known as the CMU model). Specifically, it processes a $368\times 368$ image in $180$ milliseconds on the embedded computing board named Jetson TX2 [32], whereas the original model takes multiple seconds.

OpenPose generates $18$ key-points pertaining to the nose, neck, shoulders, elbows, wrists, hips, knees, ankles, eyes, and ears of a human body. As shown in Fig. 3, a subset of these 2D key-points and their pair-wise anatomical relationships are generated for each human. We represent the key-points $\mathbf{KP}(I)$ by a $N_{I}\times 18$ array where $N_{I}$ is the number of detected humans in an image $I$. If a particular key-point is occluded or not detected, then the values are left as ($-1$, $-1$). We configure $\mathbf{KP}(I)$ in a way that the first row belongs to the left-most person, the second row belongs to the next left-most person, and gradually the last row belongs to the right-most person in the image. This way of sorting the key-points helps to speed up the process of associating the rows of $\mathbf{KP}(I_{leader})$ and $\mathbf{KP}(I_{follower})$. That is, the follower robot needs to make sure that it is pairing the key-points of the same individuals. This is important because in practice they might be looking at different individuals, or the same individuals in a different spatial order. Associating multiple persons across different images is a well-studied problem known as person re-identification (ReId).

III-B Person Re-identification using Hierarchical Similarities

Although several existing deep visual models provide very good solutions for person ReId [33, 34], we design a simple and efficient model to meet the real-time single-board computational constraints. The idea is to avoid using a computationally demanding feature extractor by making use of the hierarchical anatomical structures that are already embedded in the key-points. First, we bundle the subsets of key-points in several spatial bounding boxes (BBox) as follows:

• •

Face BBox: nose, eyes, and ears;

• •

Upper-body BBox: neck, shoulders, and hips;

• •

Lower-body BBox: hips, knees, and ankles;

• •

Left-arm BBox: left shoulder, elbow, and wrist;

• •

Right-arm BBox: right shoulder, elbow, and wrist;

• •

Full-body BBox: encloses all the key-points.

Fig. 4 illustrates the spatial hierarchy of these BBoxes and their corresponding key-points. They are extracted by spanning the corresponding key-points’ coordinate values in both the $x$ and $y$ dimensions. We use an offset (of additional $10\%$ length) in each dimension to capture more spatial information around the key-points. A BBox is discarded if its area falls below an empirically chosen threshold of $600$ square pixels. We found that BBox areas below this resolution are not always informative and are prone to erroneous results. This happens when the corresponding body-part is either not detected or very far from the camera.

Once the BBox areas are selected, we exploit their pairwise structural properties as features for person ReId; specifically, we compare the structural similarities [35] between image patches pertaining to the face, upper-body, lower-body, left-arm, right-arm, and the full body of a person. Based on their aggregated similarities, we evaluate the pair-wise association between each person as seen by the leader (in $I_{leader}$) and by the follower (in $I_{follower}$). The structural similarity [35] for a particular pair of single-channel rectangular image-patches ($\mathbf{x}$, $\mathbf{y}$) is evaluated based on three properties: luminance $l(\mathbf{x},\mathbf{y})={2\mathbf{\mu}_{\mathbf{x}}\mathbf{\mu}_{\mathbf{y}}}/{(\mathbf{\mu}_{\mathbf{x}}^{2}+\mathbf{\mu}_{\mathbf{y}}^{2})}$, contrast $c(\mathbf{x},\mathbf{y})={2\mathbf{\sigma}_{\mathbf{x}}\mathbf{\sigma}_{\mathbf{y}}}/{(\mathbf{\sigma}_{\mathbf{x}}^{2}+\mathbf{\sigma}_{\mathbf{y}}^{2}})$, and structure $s(\mathbf{x},\mathbf{y})={\mathbf{\sigma}_{\mathbf{xy}}}/{\mathbf{\sigma}_{\mathbf{x}}\mathbf{\sigma}_{\mathbf{y}}}$; here, $\mathbf{\mu}_{\mathbf{x}}$ ($\mathbf{\mu}_{\mathbf{y}}$) denotes the mean of image patch $\mathbf{x}$ ($\mathbf{y}$), $\mathbf{\sigma}_{\mathbf{x}}^{2}$ ($\mathbf{\sigma}_{\mathbf{y}}^{2}$) denotes the variance of $\mathbf{x}$ ($\mathbf{y}$), and $\mathbf{\sigma}_{\mathbf{xy}}$ denotes the cross-correlation between $\mathbf{x}$ and $\mathbf{y}$. The structural similarity metric (SSIM) is then defined as:

[TABLE]

In order to ensure numeric stability, two standard constants $c_{1}=(255k_{1})^{2}$ and $c_{2}=(255k_{2})^{2}$ are added as:

[TABLE]

We use $k_{1}=0.01$, $k_{2}=0.03$, and an $8\times 8$ sliding window in our implementation. Additionally, we resize the patches extracted from $I_{leader}$ so that their corresponding pairs in $I_{follower}$ have the same dimensions. Then, we apply Eq. 1 on every channel ($R,G,B$) and use their average value as the similarity metric on a scale of [[math], $1$]. Specifically, we use this metric for person ReId as follows:

• •

We only consider the mutually visible body-parts for evaluating the pair-wise SSIM values. This choice is important to enforce meaningful comparisons; otherwise, it is equivalent to using only the full-body BBox, which we found to be highly inaccurate.

• •

Each person in $I_{follower}$ is associated with the most similar person corresponding to the maximum SSIM value in $I_{leader}$. However, the association is discarded if that value is less than a threshold $\delta_{min}=0.4$ which is chosen by an AUC (area under the curve)-based analysis (see Section IV-B). This reduces the risk of inaccurate associations, particularly when there are mutually exclusive people in the scene.

III-C Key-point Refinement

Once the specific persons are identified, i.e., the rows of $\mathbf{KP}(I_{leader})$ and $\mathbf{KP}(I_{follower})$ are associated, the mutually visible key-points are paired together to form correspondences. Although the key-points are ordered and OpenPose localizes them reasonably well, they cannot be readily used as geometric correspondences due to perspective distortions and noise. We attempt to solve this problem by designing an iterative optimization algorithm that refines the noisy correspondences based on their structural properties in a $32\times 32$ neighborhood. By denoting $\mathbf{\phi}_{I}(\mathbf{p})$ as the $32\times 32$ image-patch centered at $\mathbf{p}=[p_{x},p_{y}]^{T}$ in image $I$, we define a loss function for each correspondence $(\mathbf{p}_{l}\in I_{leader},\mathbf{p}_{f}\in I_{follower})$ as:

[TABLE]

Then, we refine each initial key-point correspondence $(\mathbf{p}_{l}^{0},\mathbf{p}_{f}^{0})$ by minimizing the following function:

[TABLE]

As Eq. 3 suggests, we fix $\mathbf{p}_{l}=\mathbf{p}_{l}^{0}$ and refine $\mathbf{p}_{f}=\mathbf{p}_{f}^{0}$ to maximize $SSIM(\mathbf{\phi}_{I_{leader}}(\mathbf{p}_{l}),\mathbf{\phi}_{I_{follower}}(\mathbf{p}_{f}))$. In our implementation, we adopt a gradient-based refinement algorithm that performs the following iterative update:

[TABLE]

We follow the procedures suggested in [36, 37] for computing the gradient of SSIM. For fast processing, we vertically stack all the key-points and their gradients to perform the optimization simultaneously with a fixed learning rate of $\eta=0.003$ for a maximum iteration of $100$. We present empirical validations for the choices of the refinement resolution and other hyper-parameters in Section IV-B.

III-D Robot-to-robot Pose Estimation

Once the mutually visible key-points are associated and refined, the follower robot uses the corresponding 3D positions (provided by the leader) to estimate its relative pose by solving a PnP problem. Thus, we require that the leader robot is equipped with a stereo camera (or an RGBD camera) so that it can triangulate the refined key-points using epipolar constraints (or use the depth sensor) to represent the key-points in 3D.

Let $\mathbf{x}_{l}$ denote the 3D locations of the key-points in the leader’s coordinate frame, and $\mathbf{p}_{f}$ denote their corresponding 2D projections on the follower’s camera. Then, assuming the cameras are synchronized, the PnP problem is formulated as follows:

[TABLE]

Here, $\mathbf{K}_{f}$ is the intrinsic matrix of the follower’s camera and $\mathbf{T}_{f}^{l}$ is its 6-DOF transformation relative to the leader. In our implementation, we follow the standard iterative solution for PnP using RANSAC [10].

IV Experimental Analysis

We conduct several experiments with 2-DOF and 3-DOF robots to evaluate the performance and feasibility of the proposed relative pose estimation method. We present these experimental details, analyze the results, and discuss various operational considerations in the following sections.

IV-A Proof of Concept: Structure from Motion

At first, we perform experiments to validate that the human pose-based key-points can be used as geometric correspondences for relative pose estimation. As illustrated in Fig. 5(a), we emulate an experimental set-up for structure from motion with humans; we use an intrinsically calibrated monocular camera to capture a group of nine (static) people from multiple views. Here, the goal is to estimate the camera poses and reconstruct the 3D structures of the humans using only their body-poses as features.

In the evaluation, we first use OpenPose to detect the human pose-based 2D key-points in the images (Fig. 5(a)). Then, we utilize the proposed person ReId and key-point refinement modules to obtain the feature correspondences across multiple views (Fig. 5(b)). Subsequently, we follow the standard procedures for structure from motion [38]: fundamental matrix computation using 8-point algorithm with RANSAC, essential matrix computation, camera pose estimation by enforcing the Cheirality constraint, and linear triangulation. Finally, the triangulated 3D points and camera poses are refined using bundle adjustment. As demonstrated in Fig. 5(c), the spatial structure of the reconstructed points on the human bodies and the camera poses are consistent with our setup. Results of another experiment for a two-view case are shown in Fig. 6, which further validate that the estimated camera poses are comparable to the ground truth, i.e., analogous SIFT feature-based estimation. In the next section, we demonstrate the effectiveness of our proposed body-pose association and key-point refinement modules in ensuring this robust pose estimation performance.

IV-B Effectiveness of the Body-pose Association and Key-point Refinement Modules

It is easy to notice that person ReId is essential for associating mutually visible persons across different views. As mentioned in Section III-B, we focus on achieving fast association by making use of the local structural properties around the anatomical key-points in the image space. In contrast, the SOTA person ReId approaches adopt deep visual feature extractors that are computationally demanding. In Table II, we quantitatively evaluate the SOTA models named Aligned ReId [39], Deep person ReId [34], and Tripled-loss ReId [40] based on accuracy and mean averaged precision (mAP) on two standard datasets. Specifically, a test-set containing $150$ instances from the Market-1501 and CUHK-03 datasets are used for the evaluation; also, their run-times on a NVIDIA™ Jetson TX2 are shown for comparison. The results indicate that although these models (once trained on similar data) perform well on standard datasets, they are computationally too expensive for single-board embedded platforms.

Moreover, as demonstrated in Table II, these off-the-shelf models do not perform that well on high-resolution real-world images. Although their performance can be improved by training on more comprehensive real-world data, the computational complexity remains a barrier. To this end, the proposed person Reid module provides significantly faster run-time and better portability as it does not require rigorous large-scale training. Its only hyper-parameter is the SSIM threshold $\delta_{min}$ (see Section III-B), which we select by standard AUC-based analysis of ROC (receiver operating characteristic) curve. As shown in Figure 7(a), we choose $\delta_{min}=0.4$, which corresponds to $83.5\%$ true-positive and $6.5\%$ false-positive rates for person ReID on the combined test set of $250$ images containing $687$ person associations. Additionally, we select the key-point refinement resolution through an ablation experiment with $8256$ key-point correspondences. We observe that the optimal key-point location is found within $25\times 25$ pixels of the initial estimate by OpenPose for over $96\%$ of the cases. As shown in Figure 7(b), we make a more conservative choice of $32\times 32$ refinement region in our implementation.

Finally, we evaluate the utility and effectiveness of the proposed key-point refinement algorithm based on re-projection errors and compare the results with traditional SIFT feature-based reconstruction. As Figure 8(a) demonstrates, the 3D reconstruction and camera pose estimation with raw key-points are inaccurate as the unrefined correspondences are invalid in a perspective geometric sense. As Figure 8(b) shows, the average re-projection error for the refined key-points reduces to $6.85e^{-5}$ pixels, which is acceptable considering the fact that there are ten times less anatomical key-points than SIFT feature-based key-points. This evaluation corresponds to the experiment presented in Figure 5, which shows that the refined key-points constitute accurate scene reconstruction and camera pose estimation. Another qualitative validation of the iterative key-point refinement algorithm and its convergence behavior can be found in Figure 6.

IV-C Robot-to-robot 3-DOF Pose Estimation

We also perform experiments for 3-DOF robot-to-robot relative pose estimation with 2D robots. In the particular scenario shown in Figure 9, we use two planar robots (one leader and one follower) and two mutually visible humans in the scene. The robot with an AR-tag on its back is used as the follower robot while the other robot is used as the leader. The AR-tag is used to obtain the follower’s ground truth relative pose for comparison. On the other hand, the leader robot is equipped with an RGBD camera; it communicates with the follower and shares the 3D locations of the mutually visible key-points. Specifically, it detects the human pose-based 2D key-points and associates the corresponding depth information to represent them in 3D. Subsequently, the follower robot uses this information to localize itself relative to the leader by following the proposed estimation method.

As demonstrated in Figure 9, we move the follower robot in a rectangular pattern and evaluate the 3-DOF pose estimates relative to the static leader robot. We present the qualitative results in Figure 10; it shows that the follower robot’s pose estimates are very close to their respective ground truth. Overall, we observe an average error of $0.0475\%$ in translation (cm) and a $0.8625^{\circ}$ average error in rotation, which is reasonably accurate. We obtain similar qualitative and quantitative performance with a dynamic leader as well. Next, we present field experimental validations of the relative pose estimation performance in feature-deprived underwater scenarios.

IV-D Robot-to-robot 6-DOF Pose Estimation in Adverse Underwater Visual Conditions

As seen in Figure 11, standard point-based feature detectors fail to generate a large pool of reliable correspondences when there are very few salient features and landmarks in the scene. Consequently, the sampling-based parameter estimation techniques (e.g., RANSAC) often generate inaccurate results in feature-deprived underwater scenarios. However, we demonstrate that human pose-based key-points can still be refined to establish reliable geometric correspondences for robot-to-robot relative pose estimation. Moreover, we get a reasonably large pool of correspondences with only one or two humans in the scene, which is fairly common in cooperative underwater missions.

We perform several field experiments in human-robot collaborative setups; Figure 12 shows the setup of a particular underwater experiment where we capture human body-poses from different perspectives to estimate the 6-DOF transformations of two follower robots relative to a leader robot. The leader robot is equipped with a stereo camera; hence, the 3D information of the human pose-based key-points is obtained by using stereo triangulation technique. Subsequently, we find the corresponding 2D projections on the follower robots’ cameras using the proposed person ReId and key-point refinement processes. Finally, we estimate the follower-to-leader relative poses from their respective PnP solutions.

We present a particular snapshot in Figure 13(a); it illustrates the leader and follower robots’ perspectives and the associated human pose-based key-points. Subsequently, Figures 13(b)-13(c) demonstrate the geometric validity of those key-point correspondences and the reconstructed 3D points are shown in Figure 13(d). As seen, the estimated 3D structure is consistent with the mutually visible humans’ body-poses. Finally, the estimated 6-DOF poses of the follower robots relative to the leader robot are shown in Figure 13(e).

Such leader-to-follower pose estimates are useful in cooperative diver following [8], convoying [42], and other interactive tasks while operating in close proximity. The robust performance and efficient implementation of the proposed modules make it suitable for use by visually-guided underwater robots in human-robot collaborative applications. However, there are a few practicalities involved which can affect the performance; next, we discuss these aspects and their possible solutions based on our experimental findings.

IV-E Discussion: Operational Challenges and Practicalities

Synchronized cooperation: A major operational requirement of multi-robot cooperative systems is the ability to register synchronized measurements in a common frame of reference, which can be quite challenging in practice. For problems such as ours, an effective solution is to maintain a buffer of time-stamped measurements and register those as a batch using a temporal sliding window. We effectively used such timestamp-based buffer schedulers [43] in our implementation with reasonable robustness. However, the challenge remains in finding instantaneous relative poses, especially when both robots are in motion. Nevertheless, these aspects are independent of the choice of features/key-points for relative pose estimation and more generic requirements to multi-robot cooperative systems.

Trade-off between robustness and efficiency: It is quite challenging to ensure a fast yet robust performance for visual feature-based body-pose estimation and person ReId on limited computational resources of embedded platforms. This trade-off between robustness and efficiency led us to design fast body-pose association and refinement modules. These efficient modules enable us to achieve an average end-to-end run-time of $375$-$420$ milliseconds for relative pose estimation on Jetson TX2. Note that the proposed person ReId and key-point refinement account for only $195$-$240$ milliseconds (up to nine humans in the scene). Hence, faster human body-pose detectors (than OpenPose) can significantly boost the end-to-end run-time of the system.

In Section IV-B, we demonstrated that the proposed person ReId model performs reasonably well in practice despite its simplistic design. One operational benefit in our application is that the humans are seen at once from every perspective; hence, both their appearances and body-poses remain consistent. We provide a demonstration of this benefit in Figure 14; it shows three queries for ReId where humans with similar suit/wearable are seen at various distances and orientations from the camera. Although the gallery images contain several humans with similar appearances, we observe that the top three results correspond to best matches both in terms of human appearance and body-pose. We postulate that computing aggregated similarity scores on local pose-based BBoxes contribute to these results. Since the general-purpose person ReId problem is significantly harder and requires more sophisticated computational pipelines [39, 34], our proposed module seems to take advantage of the body-pose consistency across viewpoints for a faster run-time.

Number of humans and relative viewing angle: We observed a couple of other practical issues during the field experiments. First, the presence of multiple humans in the scene helps to ensure reliable pose estimation performance. We found that two or more mutually visible humans are ideal for establishing a large pool of reliable correspondences. Additionally, the pose estimation performance is affected by the relative viewing angle; specifically, it often fails to find correct associations when the $\angle$leader-human-follower is larger than (approximately) $135^{\circ}$. This results in a situation where the robots are exclusively looking at opposite sides of the person without enough common key-points. Moreover, other than temporal lags, we did not observe significant deviations in pose estimation performance with an increasing number of robots within this viewing angle; note that we used up to three follower robots in our experiments.

V Conclusions and Future Work

In this paper, we explore the feasibility of using human body-poses as markers to establish reliable multi-view geometric correspondences and to eventually solve the robot-to-robot relative pose estimation problem. First, we use OpenPose for extracting the pose-based 2D key-points pertaining to the humans in the scene. Then we associate the humans seen from multiple views using an efficient person re-identification model. Subsequently, we refine the key-point correspondences using an iterative optimization algorithm based on their local structural similarities in the image space. Finally, we use the 3D locations of the key-points (triangulated by the leader robot) and their corresponding 2D projections (seen by the follower robot) to formulate a PnP problem and solve for the unknown pose of the follower robot relative to the leader. We perform extensive experiments in terrestrial and underwater environments to investigate the applicability of the proposed relative pose estimation method; the experimental results validate its effectiveness both for 2D and 3D robots. We also discuss the relevant operational challenges and propose efficient solutions to deal with them. In the future, we seek to improve the end-to-end run-time of the proposed system and plan to use it in practical applications such as multi-robot convoying and cooperative source-to-destination planning.

VI Acknowledgments

We would like to thank Hyun Soo Park (Assistant Professor, University of Minnesota) for his valuable insights which immensely enriched this paper. We are grateful to the Bellairs Research Institute of Barbados for providing us with the facilities for field experiments; we also acknowledge our colleagues at the IRVLab and the participants of the $2019$ Marine Robotics Sea Trials for their assistance in collecting data and conducting the experiments.