Multi-view object pose distribution tracking for pre-grasp planning on mobile robots

TL;DR

This paper introduces a method for tracking object poses from multiple camera views to help mobile robots plan grasps more efficiently and accurately.

Contribution

A particle filter-based multi-view 6D pose tracking framework that fuses data from multiple cameras for improved accuracy and uncertainty estimation.

Findings

The proposed framework outperforms previous methods in quantifying object pose and uncertainty.

The method is effective for pre-grasp planning on mobile robots in real-world scenarios.

A real-world benchmark dataset was created to evaluate the framework.

Abstract

The ability to track the 6D pose distribution of an object while a mobile manipulator is still approaching it can enable the robot to pre-plan grasps, thereby improving both the time efficiency and robustness of mobile manipulation. However, tracking a 6D object pose distribution on approach can be challenging due to the limited view of the robot camera. In this study, we present a particle filter-based multi-view 6D pose distribution tracking framework that compensates for the limited view of the moving robot camera while it approaches the object by fusing observations from external stationary cameras in the environment. We extend the single-view pose distribution tracking framework (PoseRBPF) to fuse observations from external cameras. We model the object pose posterior as a multi-modal distribution and introduce techniques for fusion, re-sampling, and pose estimation from the tracked distribution to effectively handle noisy and conflicting observations from different cameras. To evaluate our framework, we also contribute a real-world benchmark dataset. Our experiments demonstrate that the proposed framework yields a more accurate quantification of object pose and associated uncertainty than previous research. Finally, we apply our framework for pre-grasp planning on mobile robots, demonstrating its practical utility.

Figures

Multi-camera setup for pre-grasp planning. (a) Determining whether the robot should navigate to the selected base pose for grasping. (b) Determining whether the robot should attempt grasp using the selected grasp pose. a-i) Object pose uncertainty from distance. a-ii) Object pose uncertainty projected onto the base pose space. b-iii) Object pose uncertainty after navigating to the base pose for grasping. b-iv) Object pose uncertainty projected onto the grasp pose space.

Rao–Blackwellized particle filter proposed by Deng et al. (2021) extended for multi-view 6D pose distribution tracking.

Illustration for determining supporting samples. (a) Set of rotation hypothesis at time t−1. (b) Posterior rotation distribution P(Rt|Z1:t). (c) Rotation samples in P(Rt|Z1:t) with probabilities greater than or equal to ϵmin. (d) Transformed rotation hypothesis from Rt−1 after applying motion from t−1 to t. (e) Supporting samples Sh,t at time t for rotation hypothesis rh,t−1 in Rt−1.

Subset of ten YCB objects used in the MY-MM dataset.

External and robot camera views at different time steps for four different sequences in the MY-MM dataset.

Setup for initially recording the transformation between the object and the robot TCP.

Simulating robot camera views while navigating toward the target object. Ground truth object pose at each time step is annotated by using the robot’s TCP pose at each time step and initially recorded transformation between the object and the robot’s TCP (Figure 6).

Comparison of object pose distribution tracking using Ours and PoseRBPF (Deng et al., 2021). First row: Robot views at different time frames, external camera views, tracked object (cracker box). Second row: (a) translational error, (b) rotational error, (c) log-probability of ground truth translation and (d) log-probability of ground truth rotation for ours. Third row: (e) translational error, (f) rotational error, (g) log-probability of ground truth translation and (h) log-probability of ground truth rotation for PoseRBPF.

Tables

TABLE 1. Pose estimation results.

	AUC of ADD					AUC of ADD-S
	PoseRBPF			CosyPose	Ours	PoseRBPF			CosyPose	Ours
Object	Robot cam	External camera 1	External camera 2	Robot + external cameras	Robot + external cameras	Robot camera	External camera 1	External camera 2	Robot + external cameras	Robot + external cameras
002_master_chef_can	22.48	40.73	20.10	24.94	46.93	69.68	85.02	81.59	77.91	84.48
003_cracker_box	59.22	65.07	19.79	65.04	62.29	79.00	87.20	72.94	82.77	82.76
004_sugar_box	45.76	08.30	21.05	36.88	47.69	64.32	58.22	39.54	52.67	77.55
006_mustard_bottle	43.42	29.78	50.27	48.59	43.04	74.56	85.05	67.04	75.02	80.84
010_potted_meat_can	09.22	28.34	19.39	10.96	33.13	24.53	65.28	36.82	25.59	60.97
011_banana	30.11	05.64	20.15	43.20	22.46	50.87	18.98	42.18	60.99	40.30
021_bleach_cleanser	39.55	09.82	04.69	39.63	55.73	64.85	46.46	33.94	60.05	77.51
024_bowl	19.16	23.68	24.59	25.14	37.46	35.17	62.86	65.27	39.73	64.53
025_mug	20.25	49.54	33.18	26.96	64.77	53.75	80.96	83.50	54.04	87.11
052_extra_large_clamp	34.35	11.42	10.61	37.91	27.35	53.04	28.54	37.40	55.03	48.57
All	32.35	27.23	22.38	35.92	44.08	56.97	61.85	56.02	58.38	70.46

TABLE 2. Pose uncertainty quantification results.

Object	Log probability of ground truth translation		Log probability of ground truth rotation
	PoseRBPF	Ours	PoseRBPF	Ours
	Robot camera	Robot + external cameras	Robot camera	Robot + external cameras
002_master_chef_can	−3.05	−0.46	−5.88	−5.98
003_cracker_box	−3.06	−1.56	−3.04	−2.98
004_sugar_box	−4.37	−2.04	−6.04	−6.35
006_mustard_bottle	−3.53	−1.95	−4.23	−3.63
010_potted_meat_can	−10.57	−4.18	−8.63	−6.54
011_banana	−5.08	−4.70	−5.49	−4.27
021_bleach_cleanser	−4.90	−2.01	−5.41	−4.82
024_bowl	−5.42	−0.97	−4.42	−3.96
025_mug	−5.46	−0.31	−7.33	−4.35
052_extra_large_clamp	−3.03	−0.95	−7.19	−5.81
All	−5.02	−1.91	−5.76	−4.86

TABLE 3. Ablation study on multi-view fusion.

Object	Log probability of ground truth rotation
	Fusion without additional filtering	Fusion with additional filtering	Ours (Section 2.3.4)
002_master_chef_can	−14.26	−7.32	−7.08
003_cracker_box	−14.74	−5.45	−4.74
004_sugar_box	−16.53	−8.06	−5.48
006_mustard_bottle	−13.34	−6.05	−3.46
010_potted_meat_can	−16.41	−6.17	−3.59
011_banana	−16.60	−5.61	−3.70
021_bleach_cleanser	−16.60	−7.25	−4.03
024_bowl	−15.70	−7.13	−6.93
025_mug	−15.05	−5.60	−4.93
052_extra_large_clamp	−16.60	−7.71	−6.07
All	−15.52	−6.63	−5.00

TABLE 4. Ablation study on re-sampling.

Object	Log probability of ground truth translation
	Re-sampling all particles simultaneously	Separately re-sampling particles originating from different frames	Ours (Section 2.3.5)
002_master_chef_can	−0.80	−1.35	−1.67
003_cracker_box	−2.08	−2.71	−2.63
004_sugar_box	−2.81	−4.60	−3.74
006_mustard_bottle	−1.32	−2.38	−2.62
010_potted_meat_can	−6.67	−6.13	−0.42
011_banana	−5.10	−6.92	−3.40
021_bleach_cleanser	−5.79	−6.47	−2.68
024_bowl	−1.30	−1.16	−0.54
025_mug	−3.91	−1.94	−0.52
052_extra_large_clamp	−2.92	−5.11	−2.18
All	−3.27	−3.87	−2.04

TABLE 5. Ablation study on pose estimation from tracked distribution.

Object	AUC of ADD		AUC of ADD-S
	PoseRBPF	Ours (Section 2.3.6)	PoseRBPF	Ours (Section 2.3.6)
002_master_chef_can	27.65	29.19	82.11	80.38
003_cracker_box	41.67	55.80	74.23	81.14
004_sugar_box	35.09	42.95	71.40	73.67
006_mustard_bottle	47.79	50.01	78.71	82.88
010_potted_meat_can	29.32	35.66	74.00	73.19
011_banana	19.39	32.03	52.97	54.40
021_bleach_cleanser	35.72	49.67	71.41	76.83
024_bowl	25.12	37.63	70.86	74.03
025_mug	42.18	62.18	86.66	86.69
052_extra_large_clamp	09.81	11.34	35.10	35.54
All	31.37	40.64	69.74	71.87

TABLE 6. Different camera combinations for fusion.

Object	Pose estimation						Uncertainty quantification
	AUC of ADD			AUC of ADD-S			Log probability of ground truth translation			Log probability of ground truth rotation
	R-E1	R-E2	R-E1-E2	R-E1	R-E2	R-E1-E2	R-E1	R-E2	R-E1-E2	R-E1	R-E2	R-E1-E2
002_master_chef_can	47.61	25.74	46.93	83.93	77.49	84.48	−0.79	−1.17	−0.46	−6.03	−5.57	−5.98
003_cracker_box	55.57	47.19	62.29	80.32	79.63	82.76	−2.10	−2.18	−1.56	−3.46	−3.44	−2.98
004_sugar_box	38.98	42.76	47.69	77.89	70.95	77.55	−1.58	−4.71	−2.04	−6.11	−7.84	−6.35
006_mustard_bottle	35.03	46.23	43.04	83.52	77.06	80.84	−1.26	−3.11	−1.95	−4.11	−3.48	−3.63
010_potted_meat_can	27.02	08.05	33.13	47.90	19.28	60.97	−6.42	−9.85	−4.18	−6.93	−7.17	−6.54
011_banana	12.92	27.90	22.46	25.46	47.39	40.30	−6.18	−3.64	−4.70	−4.67	−3.46	−4.27
021_bleach_cleanser	50.68	47.44	55.73	73.26	63.61	77.51	−3.19	−5.00	−2.01	−5.11	−5.04	−4.82
024_bowl	34.56	26.79	37.46	61.69	46.65	64.53	−1.42	−3.84	−0.97	−3.99	−3.87	−3.96
025_mug	59.22	18.96	64.77	79.08	37.96	87.11	−1.25	−6.84	−0.31	−3.70	−6.66	−4.35
052_extra_large_clamp	28.28	23.62	27.35	46.50	39.51	48.57	−3.05	−5.06	−0.95	−7.16	−7.85	−5.81
All	38.98	31.46	44.08	65.95	55.95	70.46	−2.72	−4.54	−1.91	−5.12	−5.43	−4.86

TABLE 7. Grasp success rate with and without using visual object pose uncertainty during decision-making.

Object	FBOE		OTBE		OTBM		OTBGM
	SG	AG	SG	AG	SG	AG	SG	AG
003_cracker_box	8	10	7	10	10	10	10	10
006_mustard_bottle	1	10	3	10	5	10	4	5
021_bleach_cleanser	3	10	3	10	5	8	4	7
024_bowl	4	10	3	10	7	9	7	9
025_mug	6	10	4	10	10	10	10	10
All	22	50	20	50	37	47	35	41
Success rate	44%		40%		78.72%		85.36%

Full text

Introduction

1

Inspired by the success of mobile logistic robots in warehouses, mobile manipulation (MM) is also making strides toward solving various day-to-day tasks, like assisting cafeteria staff in cleaning cafeteria tables (Garcia-Haro et al., 2020) and handling and transporting supermarket products (Zia Ur Rahman et al., 2022). The success of such robots largely depends on two factors: time efficiency and robustness (Hvilshøj et al., 2012; Thakar et al., 2023; Sereinig et al., 2020). For example, in the case of a cafeteria cleaning robot, tables must be cleaned quickly so that they can be made available for new customers. Failure to grasp can have serious consequences, such as spilling leftover coffee from a mug, damaging the object, or even disturbing other objects in the vicinity, thereby making it difficult for the robot to complete the task.

Pre-grasp planning can improve both the time efficiency and robustness of the mobile manipulation (Naik, 2024). For example, i) by determining the optimal base poses for grasping objects (Naik et al., 2024b; Naik et al., 2024a; Jauhri et al., 2022a; Makhal and Goins, 2018; Vahrenkamp et al., 2013), grasping time can be optimized, or ii) by evaluating whether it is safe to attempt a grasp, grasping failures can be reduced (Naik et al., 2025; Shi et al., 2021). Like grasp planning for rigid objects, pre-grasp planning often requires the 6D pose estimate of the objects. Pose estimates are usually obtained using the robot’s onboard camera; if the robot can track the object poses while navigating to the manipulation scene, it can already start pre-grasp planning. However, it is not always possible to have a good view of the objects while approaching, which can make it difficult to track the poses.

Modern indoor environments are often equipped with cameras for various purposes, such as occupancy tracking, surveillance, smart office solutions, and more. These cameras often have an unoccluded overview of the objects. However, due to their greater distance from the objects, they also capture limited pixel information, making it difficult to rely solely on them for accurate 6D pose estimation. Furthermore, for pre-grasp and grasp planning, poses are required in the robot base frame. Transforming poses estimated in the external camera frames to the robot base frame introduces additional uncertainty due to the inherent uncertainties associated with mobile base localization. Nevertheless, these external cameras have the potential to enable better 6D object pose tracking when used in combination with the robot camera.

Relying on a single best guess of the object pose is always risky, as the accuracy of estimated object poses is influenced by factors such as occlusions, object symmetry, self-occlusions, and lighting conditions (Xiang et al., 2017; Tremblay et al., 2018; Shi et al., 2021). This risk can be reduced by also maintaining and exploiting the uncertainty in the estimate, which is usually expressed as a 6D object pose distribution (Manhardt et al., 2019; Deng et al., 2021). For example, uncertainty can be used to assess the probability of the availability of inverse kinematics (IK) solutions at the estimated base pose for grasping by projecting the pose distribution onto the base pose space (Figures 1a-i and ii). Similarly, uncertainty can be utilized to evaluate the probability of a successful grasp by mapping the pose distribution onto the grasp pose space (Figure 1b-iii and iv).

Multi-camera setup for pre-grasp planning. (a) Determining whether the robot should navigate to the selected base pose for grasping. (b) Determining whether the robot should attempt grasp using the selected grasp pose. a-i) Object pose uncertainty from distance. a-ii) Object pose uncertainty projected onto the base pose space. b-iii) Object pose uncertainty after navigating to the base pose for grasping. b-iv) Object pose uncertainty projected onto the grasp pose space.

We propose a multi-view 6D object pose distribution tracking framework that, in addition to the robot camera views, also fuses views from the external cameras in the environment for pose distribution tracking. Specifically, we extend the PoseRBPF (Rao–Blackwellized Particle Filter) (Deng et al., 2021) to utilize information from the external cameras.

Ideally, fusing observations from external cameras should improve the accuracy of the estimated pose distributions. However, multi-camera fusion also introduces several challenges. For example, how do we ensure that during fusion, good observations from some cameras are not corrupted by poor observations from others? and, how do we determine which observations to trust when cameras have contradictory observations? Moreover, a single object pose is required for robotics tasks. However, deriving a single pose from multimodal pose distributions is not trivial. Finally, there are no benchmarks for evaluation that reflect a multi-camera setup for pre-grasp planning (Figure 1a), where the external cameras are positioned far away from the objects while the robot camera views are expected to improve as the robot moves closer to the objects.

We extend the multi-view object pose distribution tracking framework for pre-grasp planning initially introduced in Naik et al. (2022) to address these challenges. In Naik et al. (2022), multi-view fusion was performed by taking the product of observation likelihoods from all camera views without considering potentially unreliable observations from certain cameras. In addition, the standard re-sampling and pose estimation techniques proposed in Deng et al. (2021) were adopted, which did not consider the additional complexities introduced by the multi-camera setup for pre-grasp approach in mobile manipulation. Finally, the framework was evaluated only on a simulated dataset without any real-world evaluation. In this study, we address these limitations through the following contributions.

• To effectively deal with poor observations and conflicting evidence, we propose:

1. multi-view exclusion—a failure model to determine which camera views should be considered during fusion;
2. adaptive re-sampling—a re-sampling strategy to avoid premature convergence to a particular hypothesis before sufficient evidence is obtained from all camera observations;
3. pose estimation from tracked distribution that maintains multiple pose hypotheses when sufficient temporal and spatial evidence is not available to confidently converge on a single pose estimate;

• Real-world benchmark dataset and evaluation. We contribute a novel “multi-view YCB object pose tracking dataset for mobile manipulation (MY-MM)” and conduct an extensive evaluation on this real-world dataset.
• Application. We exemplify the importance of maintaining object pose uncertainties during pre-grasp planning on mobile robots for the tasks shown in Figure 1, evaluating:

1. the availability of valid IK solutions for grasping the object using the estimate of the selected base pose;
2. grasp success using the estimate of the selected grasp pose.

Related research

1.1

In this section, we summarize related research in 6D object pose estimation, pose distribution estimation, and multi-view pose estimation, and then we discuss our contribution.

6D object pose estimation

1.1.1

Before the advances in deep learning (Krizhevsky et al., 2012), 6D object pose estimation was often addressed using template-based (Choi and Christensen, 2012b; Lim et al., 2014; Hinterstoisser et al., 2011) or feature-based methods (Drost et al., 2010; Choi and Christensen, 2012a; Collet et al., 2011). In the past decade, research focus has shifted to using data-driven learning-based approaches. Initial work focused on end-to-end regression from image to 6D pose (Kendall et al., 2015). Later studies have tried to better deal with object symmetry and occlusions using novel architectures, loss-function (Xiang et al., 2017), solving orientation as classification (Kehl et al., 2017; Corona et al., 2018), or using keypoints (Pavlakos et al., 2017; Rad and Lepetit, 2017; Peng et al., 2019; Haugaard and Buch, 2022). Furthermore, since robotics tasks such as grasping require very high accuracy, iterative methods have been also developed (Li Y. et al., 2018; Manhardt et al., 2018; Wang et al., 2019; Zakharov et al., 2019) for refining the 6D object poses. However, all these methods only provide best-guess pose estimates without any information regarding associated uncertainty.

Object pose distribution estimation

1.1.2

Pose uncertainties are traditionally modeled using unimodal distributions, such as Gaussians. However, given that pose distributions are often multimodal, it is preferable to employ multimodal distributions, such as a mixture of Gaussians or histograms.

Most existing research has focused solely on estimating rotational distribution (Okorn et al., 2020; Murphy et al., 2021; Iversen et al., 2022), assuming that good translation estimates are available. Research involving the estimation of a full 6D object pose distribution typically either allow multiple pose predictions from the network (Manhardt et al., 2019) or separately model translation and rotational distributions (Glover et al., 2012; Deng et al., 2019; Deng et al., 2021). Translational uncertainty is usually modeled by a simple Gaussian, while rotational uncertainty is modeled using a Bingham mixture model (Glover et al., 2012; Okorn et al., 2020) or a multi-modal histogram distribution (Corona et al., 2018; Deng et al., 2019).

Moreover, most such studies do not directly predict the object pose. They infer the pose either by comparing the object image against the rendered images of the object (Corona et al., 2018), using a learned comparison function against image dictionaries (Okorn et al., 2020), or by codebook matching and training a de-noising auto-encoder for generating codes (Sundermeyer et al., 2018; Deng et al., 2021). Others have represented the distributions implicitly, using a neural network that estimates the probability of a candidate pose given the input image (Murphy et al., 2021; Haugaard et al., 2023).

In this study, we separately model translational and rotational distributions and use codebook matching to determine the likelihood of different rotations.

Multi-view object pose estimation

1.1.3

Multi-view pose estimation involves estimating the 6D pose of an object by considering multiple views of it (Collet and Srinivasa, 2010; Susanto et al., 2012; Kanezaki et al., 2018; Li C. et al., 2018; Xie et al., 2019; Labbé et al., 2020). Sequentially integrating additional views of an object over time for pose estimation is referred to as “object pose tracking.” This includes object-based SLAM methods (Salas-Moreno et al., 2013; Mendes et al., 2016) or active vision methods (Zeng et al., 2017; Kaba et al., 2017; Deng et al., 2020; An et al., 2024) that capture multiple views of an object and improve the pose estimate over time. Some recent research has also concentrated on 6D object pose tracking in a manipulation context (Deng et al., 2019; Wang et al., 2020; Wen et al., 2020).

Other approaches concentrate on simultaneously using multiple views of an object from various camera viewpoints. Some of these methods jointly estimate the 3D pose from multiple object views (Helmer et al., 2010; Su et al., 2015; Kanezaki et al., 2018), others first generate pose estimates in each view and then employ various techniques to fuse multiple pose estimates into one (Collet and Srinivasa, 2010; Susanto et al., 2012; Li C. et al., 2018; Xie et al., 2019; Labbé et al., 2020), while recent work (Haugaard and Iversen, 2023) has used learned 2D–3D correspondence distributions for multi-view pose estimation.

However, not much prior research exists in multi-view full-pose distribution estimation. Erkent et al. (2016) have introduced an approach for integrating the 6D pose estimates from different views by assuming Gaussian uncertainty in each view. As discussed above, a unimodal distribution is not sufficient for modeling symmetric or occluded objects. They also assume only stationary cameras and do not incorporate any motion model for tracking the estimated pose distribution, which is essential for manipulation tasks. To our knowledge, there is no existing research that explores multiple simultaneous views along with sequential temporal integration to estimate multimodal object pose distributions.

Materials and methods

2

Problem definition

2.1

We define the problem as 6D object pose distribution tracking using the robot’s onboard camera and external cameras in the environment for pre-grasp planning on mobile robots. Let $[eqn]$ , $[eqn]$ , $[eqn]$ , and $[eqn]$ be the robot camera, external camera $[eqn]$ , object, and world frame at time $[eqn]$ , respectively. $[eqn]$ refers to the transformation from frame $[eqn]$ to $[eqn]$ . Then, given:

• the transformation of each camera w.r.t the world frame $[eqn]$ ,
• the motion of the robot camera $[eqn]$ ,
• a textured CAD model of the object,
• an initial (at $[eqn]$ ) object detection (2D bounding box),
• observations (RGB-D images) from external cameras $[eqn]$ and the robot camera $[eqn]$ at time $[eqn]$ .

Our objective is to estimate and track the object pose distribution $[eqn]$ (where $[eqn]$ ) by fusing observations from external cameras $[eqn]$ in addition to the robot camera $[eqn]$ and subsequently infer pose estimate $[eqn]$ from the tracked distribution.

Our approach is inspired by the single-camera 6D pose distribution tracking framework PoseRBPF (Deng et al., 2021). We extend PoseRBPF to the pre-grasp planning multi-camera setup (Figure 1a). In the subsequent sections, we briefly introduce PoseRBPF, including relevant mathematical details for multi-view extensions (Section 2.2), and then present the proposed multi-view extensions (Section 2.3).

PoseRBPF

2.2

The posterior distribution of the 6D object pose given the observations up to time $[eqn]$ can be expressed as $[eqn]$ , where $[eqn]$ is a 6D state variable and $[eqn]$ refers to observations from time 1 to $[eqn]$ . The 6D state variable $[eqn]$ consists of rotational $[eqn]$ and translational $[eqn]$ components. Since dense sampling in $[eqn]$ is computationally intractable, PoseRBPF models the rotational component $[eqn]$ as conditioned on the translational component $[eqn]$ as described in Equation 1.

$$[eqn]$$

In addition, particles are sampled only over the translation space marginalized over the rotation (Rao–Blackwellization) (Murphy and Russell, 2001; Doucet et al., 2001). This approximation allows efficient sampling in the 6D space.

To effectively capture the object’s symmetry and visual ambiguity arising from occlusions, the rotation conditioned on the translation $[eqn]$ is modeled as a 3D histogram distribution. Each bin in the histogram $[eqn]$ represents the discretized rotational component of the object pose at time $[eqn]$ , with $[eqn]$ , $[eqn]$ , and $[eqn]$ denoting the elevation, azimuth, and bank indices, respectively. Discretization is performed with a resolution of 5°. As a result, the histogram distribution consists of 37 × 72 × 72 (elevation, azimuth, bank) bins. The probability of $[eqn]$ is $[eqn]$ , where $[eqn]$ , $[eqn]$ , and $[eqn]$ .

Thus, the $[eqn]$ th particle $[eqn]$ at time $[eqn]$ is represented by a translation $[eqn]$ and a discretized rotation distribution conditioned over the translation $[eqn]$ :

$$[eqn]$$

The posterior distribution at time $[eqn]$ is approximated by a set of $[eqn]$ particles $[eqn]$ expressed as

$$[eqn]$$

During initialization, the translational component $[eqn]$ of the particles are sampled around the available translation estimate of the object, assuming Gaussian uncertainty. The discretized rotation distribution conditional on the translation $[eqn]$ for each particle is initialized as a uniform distribution.

At each time-step $[eqn]$ , the set of particles from the previous time-step $[eqn]$ are propagated to the current time-step using a motion model. The motion propagation involves sequentially applying transformations: $[eqn]$ (inverse of the motion of the robot camera frame), $[eqn]$ (transformation from the previous robot camera frame to the previous object frame), and $[eqn]$ (motion of the object frame).

$$[eqn]$$

The observation likelihood $[eqn]$ is measured using the degree of agreement between observation $[eqn]$ and the propagated particles $[eqn]$ . A codebook of vector embeddings is pre-computed for the discretized rotation space of the object using a de-noising auto-encoder (Sundermeyer et al., 2018). The pre-computed encodings from the codebook for different rotations of the object are then compared with the encodings obtained from the current observation $[eqn]$ .

As each particle $[eqn]$ consists of a translation $[eqn]$ and a discretized rotation distribution conditioned on the translation $[eqn]$ (Equation 2), the observation likelihood $[eqn]$ for each particle $[eqn]$ is a histogram, with each bin containing the likelihood of the discretized rotation.

The importance factor (weight) of an individual particle is directly proportional to its observation likelihood (Thrun, 2002).

$$[eqn]$$

The weight $[eqn]$ for each particle $[eqn]$ is calculated by marginalizing over the observation likelihood for rotation samples, as shown in Equation 5:

$$[eqn]$$

Re-sampling involves increasing the number of relevant particles—that is, the particles with higher weights are more likely to be duplicated, and those with lower weights are more likely to be discarded (Thrun, 2002). After the re-sampling step, a multi-set of important particles $[eqn]$ is obtained.

The translational component $[eqn]$ of these particles represents the posterior translation distribution $[eqn]$ at time $[eqn]$ . To obtain the posterior rotation distribution $[eqn]$ , the individual observation likelihoods $[eqn]$ of each particle $[eqn]$ are marginalized. The posterior rotational distribution $[eqn]$ with temporal fusion up to time $[eqn]$ is obtained using Bayes’ rule.

Finally, the 6D pose estimate of the object from the distribution is computed. The translation estimate $[eqn]$ of the object pose $[eqn]$ at time $[eqn]$ is obtained by taking an average of the posterior translation distribution. The rotation estimate $[eqn]$ is obtained by taking the weighted average of the samples in the neighborhood of a rotation estimate at the previous time step $[eqn]$ after applying the motion from times $[eqn]$ to $[eqn]$ .

Proposed approach

2.3

In this section, we first provide a brief overview of the proposed multi-view pose distribution tracking framework using Figure 2 and then present our contributions in detail.

Rao–Blackwellized particle filter proposed by Deng et al. (2021) extended for multi-view 6D pose distribution tracking.

Overview

2.3.1

Figure 2 describes the architecture of the proposed multi-view 6D object pose distribution tracking framework for a three-camera setup comprising a robot camera and two external cameras. The posterior distribution is modeled as a multi-modal distribution (Section 2.3.2). The particles approximating the posterior distribution at time $[eqn]$ (Figure 2a) are propagated to time $[eqn]$ using the motion model (Figure 2b; Section 2.3.3). Observation likelihoods for these propagated particles are then computed for each camera (Figures 2c,d), as proposed in PoseRBPF. Next, the observations from different cameras are fused (Figure 2e; Section 2.3.4). Each particle is then weighed based on the fused observations, followed by adaptive re-sampling (Section 2.3.5). The posterior distributions at time $[eqn]$ are obtained using temporal fusion. The 6D pose is then estimated using the pose distribution obtained (Section 2.3.6), and the re-sampled particles are advanced to the next time-step.

It should be noted that the robot camera frame $[eqn]$ is used as a reference frame, and both object pose distribution $[eqn]$ and pose estimate $[eqn]$ are estimated in this frame.

Multi-modal posterior distribution

2.3.2

As each camera can have different translation and rotation beliefs, we model both translation and rotation posterior distributions as multi-modal distributions. We assume that the uncertainty in the translation $[eqn]$ follows a Gaussian distribution in each camera. By combining the Gaussian distributions from all camera frames, the resultant posterior translation distribution is modeled as a mixture of Gaussians, as shown in Equation 6:

$$[eqn]$$

where $[eqn]$ is the number of camera frames, $[eqn]$ is the normal distribution with the parameters $[eqn]$ (mean) and $[eqn]$ (co-variance matrix), and $[eqn]$ is a weight for the particular camera.

During the initialization of the tracking (at $[eqn]$ ), the translation component $[eqn]$ of the particles are sampled around the translation estimate $[eqn]$ in each camera $[eqn]$ obtained either from an object detector (Wu et al., 2019) or an alternate pose estimator (Xiang et al., 2017; Wang et al., 2019). In the case of an object detector, the bounding box center and corresponding depth (in the camera depth image) is used as a translation estimate. The particles are then sampled around these estimates assuming Gaussian noise with standard deviation $[eqn]$ in each camera $[eqn]$ . If more than one camera provides an initial translational estimate $[eqn]$ , then the resultant posterior translation distribution is a mixture of Gaussians. For the three-camera setup in Figure 2, the posterior translation distribution is represented using three Gaussians (Figure 2a). The discretized rotation distribution conditional on the translation $[eqn]$ for each particle is initialized as a uniform distribution similar to PoseRBPF.

As the camera from which a particle $[eqn]$ originated at $[eqn]$ is required during the re-sampling step (Section 2.3.5), we represent the particles in $[eqn]$ (Equation 3) originating from camera $[eqn]$ using the set $[eqn]$ . For $[eqn]$ cameras, there exists $[eqn]$ such particle sets $[eqn]$ , where each $[eqn]$ is a subset of $[eqn]$ $[eqn]$ with size $[eqn]$ and $[eqn]$ . The total number of particles $[eqn]$ in particle set $[eqn]$ is kept constant, so that at any time $[eqn]$ , $[eqn]$ . This is ensured during the re-sampling step.

Motion propagation

2.3.3

The particles originating from the robot camera frame are propagated as described in Equation 4 (same as PoseRBPF). The motion propagation for particles originating from the external camera frames (as described in Equation 7) involves sequentially applying transformations: $[eqn]$ (transformation from the current robot camera frame to the current external camera frame), $[eqn]$ (inverse of the motion of the external camera frame), $[eqn]$ (transformation from the previous external camera frame to the previous robot camera frame), $[eqn]$ (transformation from the previous robot camera frame to the previous object frame), and $[eqn]$ (motion of the object frame):

$$[eqn]$$

External cameras and objects are assumed stationary, and hence their motion from times $[eqn]$ to $[eqn]$ is 0 (i.e., $[eqn]$ and $[eqn]$ are identity matrices). The uncertainty of the transformation from the external to the robot camera $[eqn]$ is modeled as Gaussian noise. The uncertainty of the relative motion of the robot camera $[eqn]$ is modeled as Gaussian noise with dispersion proportional to the relative motion (Thrun, 2002).

At the end of the motion propagation step, a new particle set $[eqn]$ is obtained, which contains the updated particles from the previous time step $[eqn]$ after applying the motion. Each particle $[eqn]$ in $[eqn]$ with its translation $[eqn]$ and rotation distribution conditioned on the translational component $[eqn]$ is propagated from times $[eqn]$ to $[eqn]$ , resulting in $[eqn]$ and $[eqn]$ .

NOTE: In the remaining sections, frames are represented by integers ranging from 1 to $[eqn]$ for simplification. Specifically, the robot camera frame $[eqn]$ is denoted by 1, while the external camera frames $[eqn]$ are represented by integers from 2 to $[eqn]$ . Additionally, when a translation or rotation variable is not explicitly associated with a specific camera frame, it should be assumed to be in the robot camera frame, such that both $[eqn]$ $[eqn]$ represent the translation estimate in the robot camera frame $[eqn]$ .

Multi-view fusion

2.3.4

As the observation likelihoods $[eqn]$ 1 obtained for each particle $[eqn]$ in camera $[eqn]$ are assumed to be conditionally independent of each other, the joint observation likelihood can be obtained by fusing the individual observation likelihoods (Durrant-Whyte and Henderson, 2016) as described in Equation 8:

$$[eqn]$$

The observation likelihoods from different cameras are first transformed into a common reference frame. The likelihoods of corresponding bins across views are then fused to obtain the resultant likelihood. While this ensures consistency across views and mitigates potential scale bias, such a fusion is unable to handle erroneous observations (Khaleghi et al., 2013). For instance, if the object is not visible in one of the cameras (i.e., empty histogram), the fused observation likelihood will also be an empty histogram.

To address this issue, additive/Laplace smoothing (Schütze et al., 2008) can be performed on the individual observation likelihoods before the fusion. However, if camera $[eqn]$ has a poor view of the object (for example, due to occlusions), the observation likelihood $[eqn]$ obtained for particle $[eqn]$ in camera frame $[eqn]$ will most likely be inaccurate. When combined with good observations from other cameras, such an observation can corrupt the overall fusion result.

We address this by defining a failure model to assess whether an observation from a particular camera should be included in the fusion process (Khaleghi et al., 2013). Let $[eqn]$ represent the value of maximum likelihood for the observation $[eqn]$ over all possible translations and rotations in the particle set.

$$[eqn]$$

It is shown in Deng et al. (2021) that $[eqn]$ is correlated with the degree of object occlusion. If $[eqn]$ falls below 0.6, which corresponds to approximately 65% of the object being occluded, then the object can no longer be reliably tracked. We use this insight to define the failure model to determine whether observation likelihoods for particles in camera $[eqn]$ (Figure 2d) should be included in the fusion (Figure 2e). The observation likelihood $[eqn]$ for particle $[eqn]$ in camera frame $[eqn]$ is included during fusion only if it is $[eqn]$ greater than pre-defined threshold value $[eqn]$ .

Observation likelihoods in different frames are fused together (Algorithm 1). Prior to fusion, observation likelihoods from different frames are transformed into a robot camera frame, and additive/Laplace smoothing is applied by adding a small minimum likelihood $[eqn]$ to all rotations in the discretized rotation space. The observation likelihood from frame $[eqn]$ is included in the fusion if $[eqn]$ exceeds $[eqn]$ .

Algorithm 1Multi-view fusion.

• 1:  $[eqn]$
• 2:  $[eqn]$
• 3:
• 4:  $[eqn]$
• 5: for $[eqn]$ in $[eqn]$ do
• 6:  if $[eqn]$ then
• 7:   transform $[eqn]$ to $[eqn]$
• 8:    $[eqn]$
• 9:  end if
• 10: end for
• 11:  $[eqn]$

At the end of this step, the joint observation likelihood is obtained for each particle in the particle set $[eqn]$ (Figure 2e).

Note that in the rest of this study, for brevity, $[eqn]$ are represented as $[eqn]$ .

Adaptive re-sampling

2.3.5

As described in Section 2.3.4, the decision to include observation likelihood $[eqn]$ of particle $[eqn]$ in camera $[eqn]$ during the fusion is based on the $[eqn]$ (Equation 9). Moreover, as explained in Section 2.3.2, at $[eqn]$ , for each camera $[eqn]$ , with the translation estimate of the object $[eqn]$ , $[eqn]$ number of particles are sampled around this translation estimate assuming Gaussian distributed noise. These particles, denoted by the set $[eqn]$ , represent the observation of camera $[eqn]$ at $[eqn]$ . Prior to re-sampling at time $[eqn]$ , these particles are denoted using the set $[eqn]$ . Consequently, before the filter converges, the particles in the set $[eqn]$ are more likely to have the maximum likelihood $[eqn]$ for the camera’s $[eqn]$ th observation $[eqn]$ compared to other sets of particles that represent other camera observations.

Conventional re-sampling techniques do not distinguish between particles representing different camera observations. Thus, if camera $[eqn]$ has a poor observation at time $[eqn]$ (before convergence) due to motion blur, partial occlusion, and so on, particles $[eqn]$ representing the observations of camera $[eqn]$ will be more affected as they will have smaller weights than other particles. As a result, there is a possibility that these particles may be eliminated during re-sampling.

Consequently, there is an increased possibility that observations $[eqn]$ from camera $[eqn]$ may not have a sufficiently high $[eqn]$ during subsequent time-steps to be included during the fusion, as all the particles representing its observation were eliminated. As a result, the particle filter may prematurely converge to an inaccurate hypothesis by solely considering observations from other cameras and disregarding observations from camera $[eqn]$ .

Separately re-sampling the particles in each $[eqn]$ in Equation 3 can ensure that all particles from each of the $[eqn]$ sets of particles $[eqn]$ are retained throughout the tracking process. However, this can lead to a situation where the set of particles $[eqn]$ with a very poor initial translation estimate $[eqn]$ are also retained forever. Such uninformative particles would only waste computational resources without contributing to improved tracking.

To address this issue, we dynamically adjust the size of the particle sets $[eqn]$ based on the average aggregate weight of the particles within these sets. We increase the size of the particle sets with the highest average aggregate weight and reduce the size of the other particle sets to maintain a constant number of particles $[eqn]$ at time $[eqn]$ . Particles are then individually re-sampled from each of these sets. Thus, we prevent the elimination of particles originating from certain cameras due to a few poor observations in that camera; however, if a camera has consistently poor observations, then these particles are eventually eliminated.

In the rest of this section, we provide technical details of our approach. Readers can optionally proceed directly to Section 2.3.6 without these details.

We refer to our approach as “adaptive re-sampling” (Algorithm 2). It takes $[eqn]$ (the particle set obtained at the end of motion propagation step—Section 2.3.3) and $[eqn]$ (the set of weights associated with $[eqn]$ particles in $[eqn]$ ) as inputs and returns the new particle set $[eqn]$ containing $[eqn]$ particles (Lines 1–2). $[eqn]$ contains the re-sampled indices of the particles in set $[eqn]$ that are used for creating new particle set $[eqn]$ .

We let $[eqn]$ return the indices of the particles in the particle set $[eqn]$ that originated from the camera $[eqn]$ observation at $[eqn]$ = 0 and $[eqn]$ return the weights of these particles

$$[eqn]$$

Furthermore, “bpsi” (best particle set index) is the index of the $[eqn]$ with the highest average aggregate particle weights (Line 4), while “bpibps” (best particle index in the best particle set) is the index of the particle with maximum weight in the best particle set $[eqn]$ (Line 5).

The size of each of the $[eqn]$ particle sets $[eqn]$ is dynamically adjusted based on the average aggregate weight of the particles in these sets (Lines 6–12). The size of the particle sets $[eqn]$ with lowly weighted particles is reduced, while the size of the particle set with the highest average aggregate weight of particles is increased. The particle with the maximum weight from $[eqn]$ is duplicated $[eqn]$ −1 times. Particles with the lowest weights from other particle sets (“wpipsn”—worst particle index in particle set $[eqn]$ ) are eliminated to ensure that $[eqn]$ remains constant. Finally, particles from each $[eqn]$ are separately re-sampled (Lines 13–18). This incremental process of replicating the “best” particle followed by re-sampling at each time-step prevents sudden particle impoverishment and helps maintain multiple hypotheses, thereby reducing the risk of mode collapse.

Algorithm 2Adaptive re-sampling.

• 1:  $[eqn]$
• 2:  $[eqn]$
• 3:  $[eqn]$       ⊳ Multi-set (can contain duplicates)
• 4:  $[eqn]$ = $[eqn]$
• 5:  $[eqn]$ = $[eqn]$
• 6: for $[eqn]$ do
• 7:  if $[eqn]$ not equal to $[eqn]$ then
• 8:    $[eqn]$
• 9:    $[eqn]$
• 10:    $[eqn]$
• 11:  end if
• 12: end for         ⊳ Re-sampling
• 13: for $[eqn]$ do
• 14:  for $[eqn]$ do
• 15:   draw $[eqn]$ from $[eqn]$ with prob. $[eqn]$
• 16:    $[eqn]$
• 17:  end for
• 18: end for
• 19:  $[eqn]$
• 20:  $[eqn]$

By adopting this strategy, we prevent premature convergence and allow the maintenance of a diverse set of particles for a specific duration so that sufficient temporal evidence is obtained from all cameras before convergence. This duration depends on the number of particles in each of the $[eqn]$ particle sets $[eqn]$ .

Pose estimation from tracked distribution

2.3.6

For the multi-camera setup considered here, initially at $[eqn]$ , only uncertain pose estimates are available due to the large distance between the cameras and the objects. Hence, the assumptions such as having a good initial object rotation estimate (at $[eqn]$ ) in Deng et al. (2021) are not always valid. To address this, we maintain multiple pose hypotheses when sufficient evidence is not available in the tracked distribution to converge to a single pose estimate. In the following sections, we describe our approach for estimating the translation $[eqn]$ and rotation $[eqn]$ components of $[eqn]$ using the distribution $[eqn]$ .

Translation estimate

T^t

2.3.6.1

The posterior translation distribution $[eqn]$ is modeled as a mixture of Gaussians with $[eqn]$ (number of particles) samples. This distribution in general reduces to a unimodal Gaussian distribution as the filter starts to converge. The translation estimate $[eqn]$ of the object pose $[eqn]$ at time $[eqn]$ is obtained by taking a weighted average of the particles of the dominant Gaussian (i.e., the Gaussian with the highest associated weight) in the posterior translation distribution.

Rotation estimate

R^t

2.3.6.2

The posterior rotation distribution $[eqn]$ is modeled as a potentially multimodal histogram distribution. While weighted average can be employed to compute the rotation estimate $[eqn]$ when the histogram distribution is unimodal, it can yield an incorrect estimate when it is multimodal.

We maintain multiple rotation estimates when sufficient evidence is not present in the posterior rotation distribution $[eqn]$ to converge to a single pose estimate. Thus, we maintain and track a set of $[eqn]$ rotation hypotheses as described in Equation 10,

$$[eqn]$$

where $[eqn]$ is the rotation hypothesis and $[eqn]$ is the associated confidence.

In the subsequent sections, we present our approach. In Section 2.3.6.2, we introduce the notion of supporting samples $[eqn]$ , and we outline the algorithm for maintaining a set of rotation hypotheses $[eqn]$ in Section 2.3.6.2.

Supporting samples

Sh,t

2.3.6.2.1

Supporting samples $[eqn]$ are the discretized rotational components $[eqn]$ in the posterior rotational distribution $[eqn]$ that can be associated with the rotation hypothesis $[eqn]$ . A discretized rotational component $[eqn]$ is considered to be part of the supporting samples $[eqn]$ if it lies in the neighborhood of $[eqn]$ (the rotation hypothesis $[eqn]$ after applying the camera motion from $[eqn]$ −1 to $[eqn]$ ) and has a probability greater than $[eqn]$ :

$$[eqn]$$

where $[eqn]$ is a threshold angular difference to determine whether the discretized rotational component $[eqn]$ is in the neighborhood of the $[eqn]$ , and $[eqn]$ is the minimum probability that a discretized rotational component $[eqn]$ must be part of $[eqn]$ .

Figure 3 illustrates the computation of supporting samples for three different rotation hypotheses in a set (Figure 3a). Initially, the posterior rotation distribution $[eqn]$ is obtained at time $[eqn]$ (Figure 3b), and the rotations with probabilities greater than $[eqn]$ are extracted (Figure 3c). This is followed by applying the motion from time $[eqn]$ to $[eqn]$ to all the rotation hypotheses to obtain $[eqn]$ (Figure 3d). Finally, the supporting samples $[eqn]$ are obtained for each rotation hypothesis, as in Equation 11 (Figure 3e).

Illustration for determining supporting samples. (a) Set of rotation hypothesis at time t−1. (b) Posterior rotation distribution P(Rt|Z1:t). (c) Rotation samples in P(Rt|Z1:t) with probabilities greater than or equal to ϵmin. (d) Transformed rotation hypothesis from Rt−1 after applying motion from t−1 to t. (e) Supporting samples Sh,t at time t for rotation hypothesis rh,t−1 in Rt−1.

It is assumed that the rotation hypothesis $[eqn]$ can be associated with the posterior rotation distribution $[eqn]$ $[eqn]$ if more than $[eqn]$ % samples with probabilities greater or equal to $[eqn]$ in $[eqn]$ are in $[eqn]$ . For example, in Figure 3, most of the samples with probabilities greater than $[eqn]$ in the posterior rotation distribution $[eqn]$ are in the neighborhood of $[eqn]$ . Hence, if $[eqn]$ is set to 0.8 (80%), only $[eqn]$ can be associated with the samples in the posterior rotation distribution $[eqn]$ at time $[eqn]$ .

Maintaining a set of rotation hypotheses

2.3.6.2.2

If the samples in the posterior rotation distribution $[eqn]$ can be associated with the rotation hypothesis $[eqn]$ , then $[eqn]$ is updated using the expectation of the supporting samples. However, if the samples in the posterior rotation distribution $[eqn]$ cannot be reliably associated with the rotation hypothesis $[eqn]$ , then $[eqn]$ is split into two hypotheses at time $[eqn]$ —one suggested by the data that could be associated with $[eqn]$ and the other suggested by the data that could not be associated with $[eqn]$ (Thrun, 2002). The algorithm for maintaining a set of rotation hypotheses is described in Algorithm 3.

Initially, the rotation hypothesis $[eqn]$ under consideration is updated using the camera motion from times $[eqn]$ to $[eqn]$ , resulting in $[eqn]$ (Line 6). Subsequently, the supporting samples are determined, and if the rotation hypothesis $[eqn]$ can be associated with the posterior rotation distribution $[eqn]$ , $[eqn]$ is updated by taking the expectation of the supporting samples (Lines 6–10).

If $[eqn]$ cannot be associated with the $[eqn]$ , a new rotation hypothesis $[eqn]$ is introduced (Lines 12). The confidence in this hypothesis is computed relative to the confidence in the parent hypothesis $[eqn]$ , similar to multi-hypothesis tracking in Thrun (2002). Finally, the confidences $[eqn]$ in all the rotation hypotheses $[eqn]$ are normalized (by dividing by the sum of all confidences). Only the rotation hypotheses $[eqn]$ with confidence $[eqn]$ greater than $[eqn]$ are retained for the subsequent time-step (Line 18). The maximum number of hypotheses retained at any time step is given by $[eqn]$ .

Algorithm 3Set of rotation hypotheses.

• 1:  $[eqn]$
• 2:  $[eqn]$
• 3:  $[eqn]$    ⊳ For simplification purpose
• 4:  $[eqn]$
• 5: for $[eqn]$ do
• 6:   $[eqn]$    ⊳ Using motion from $[eqn]$ to $[eqn]$
• 7:   $[eqn]$    ⊳ Equation 11
• 8:   $[eqn]$    ⊳ Using $[eqn]$
• 9:  if $[eqn]$ then
• 10:    $[eqn]$
• 11:  else
• 12:    $[eqn]$
• 13:    $[eqn]$
• 14:    $[eqn]$
• 15:  end if
• 16: end for
• 17:  $[eqn]$
• 18:  $[eqn]$

Algorithm 3 thus covers two possible failure cases. The association could not be established between the rotation hypothesis $[eqn]$ and the posterior rotation distribution $[eqn]$ because

1. $[eqn]$ is a noisy distribution and the rotation hypothesis $[eqn]$ (transformed $[eqn]$ after applying the camera motion) is a valid estimate;
2. $[eqn]$ (transformed $[eqn]$ after applying the camera motion) is an invalid estimate but $[eqn]$ is a valid distribution.

In the first case, the rotation hypothesis $[eqn]$ remains in the set $[eqn]$ , and confidence in it increases, while confidence in the rotation hypotheses added based on the noisy samples in $[eqn]$ (that could not be associated with $[eqn]$ ) decreases as more valid distributions are acquired in subsequent time steps.

In the second case, confidences in the rotation hypotheses added based on the samples in the posterior rotation distribution $[eqn]$ which could not be associated with hypothesis $[eqn]$ increases, and simultaneously, confidence in hypothesis $[eqn]$ decreases as more similar $[eqn]$ samples are obtained in the subsequent time-steps.

At time $[eqn]$ , the rotational hypothesis $[eqn]$ with the highest associated confidence $[eqn]$ is used as the rotational estimate $[eqn]$ .

Results

3

Experimental evaluation

3.1

Experiment objectives

3.1.1

The experiments are designed to verify that the proposed multi-view object pose distribution tracking using a robot and external cameras results in more accurate

• object pose estimation $[eqn]$ than existing multi-view pose estimation methods;
• uncertainty quantification $[eqn]$ than existing single-view uncertainty quantification methods;
• object pose estimation $[eqn]$ and uncertainty quantification $[eqn]$ compared to using individual robot or external cameras alone.

Since our focus is on household objects, we use YCB benchmark (Calli et al., 2015) objects for evaluation. We consider the following baselines.

• PoseRBPF (Deng et al., 2021): a single-camera object pose distribution tracking framework that provides both pose estimates and pose distributions at each time-step.
• CosyPose (Labbé et al., 2020): a multi-camera pose estimation method. It first estimates the object poses in each camera and then matches individual pose hypotheses across different input images to jointly estimate camera viewpoints and 6D object poses in a single consistent scene.

We also conducted three ablation studies to validate our design choices for multi-view fusion, re-sampling, and pose estimation from a tracked distribution. Finally, we present an ablation study to compare the performance with different camera combinations for fusion.

Experiment setup

3.1.2

As the mobile robot approaches the object for grasping using its base and arm motion, the robot camera will initially have a very limited view of the object, which will improve as it moves closer to the object. On the other hand, static external cameras will always have the same view of the object. Additionally, there is a possibility that as the robot approaches the object, it may be occluded by other objects in the scene. In such cases, the robot is expected to utilize its motion to obtain a better view of the object.

Existing real-world object pose estimation benchmark datasets, such as the YCB Video dataset (Xiang et al., 2017), primarily consist of short single-camera video sequences without significant changes in the object’s view. The camera is positioned very close to the objects, simulating scenarios where the robot is already in close proximity to the object for grasping (i.e., assuming sequential navigation and manipulation). Furthermore, in multi-view datasets such as T-LESS (Hodaň et al., 2017), the object appears at an equal distance across all the views. Consequently, these datasets are not well-suited for evaluating this research. To address this gap, we contribute a “multi-view YCB object pose tracking dataset for mobile manipulation” (MY-MM).

MY-MM dataset

3.1.2.1

The dataset comprises 100 sequences featuring ten different YCB objects with distinct geometries and textures (Figure 4). For each video sequence (Figure 5), we provide the 6D pose of the object of interest in the robot camera frame, views of the scene from two external cameras, the transformations between all cameras for each time step, and intrinsic calibration for all the cameras. For each object, we provide ten sequences with ground truth poses. In each scene, along with the object with ground truth poses, several distractor objects are also present (Figure 5). Full robot camera videos along with the recorded ground truths for the sequences in Figure 5 can be viewed here2.

Subset of ten YCB objects used in the MY-MM dataset.

External and robot camera views at different time steps for four different sequences in the MY-MM dataset.

The external cameras are positioned approximately 3 m from the objects (Figure 1). The robot camera moves toward the object, starting from 2.5 m away and approaching to approximately 0.5 m from the object, recording approximately 250 frames in the process. The robot camera views were recorded using the Intel RealSense D435 RGB-D camera at a resolution of 640 × 480. External camera views were captured using Intel RealSense D455 RGB-D cameras at full HD resolution. However, only the center crop of the captured image, sized 640 × 480, containing a view of the table has been included in the dataset. We thus also reduce possible calibration errors caused by fish-eye effects in the periphery of the images.

Ground truth annotation

3.1.2.2

Computing the ground truth 6D pose from a distance is challenging. We employed the setup depicted in Figure 6 to mimic the robot camera views while approaching the object for grasping and to annotate the object’s ground truth pose. First, the relative transformation between the YCB object origin frame $[eqn]$ and a marker origin frame $[eqn]$ was manually measured. To minimize the errors, markers were printed with object outlines, and the objects were carefully placed inside. This was followed by estimating the marker pose in the robot camera frame $[eqn]$ to estimate the relative transformation between these two frames. Since the transformations between the object frame $[eqn]$ and marker frame $[eqn]$ and a robot camera frame $[eqn]$ and robot base frame $[eqn]$ are already known, they can be used to compute object poses in the robot base frame $[eqn]$ . Moreover, the transformation between the robot TCP frame and the robot camera frame is also known. The estimated object ground truth is then manually validated by overlaying the object CAD model on the image using the estimated ground truth.

Setup for initially recording the transformation between the object and the robot TCP.

Each sequence is recorded as follows. Initially, the robot camera is positioned at a considerable distance from the object (Figure 7) at $[eqn]$ . At each subsequent time-step, the robot camera is moved closer to the object using only the manipulator motion. To ensure an accurate estimation of the object pose, the manipulator is completely stopped during each step to avoid any synchronization issues that may arise due to the motion. The transformation from the robot TCP to the robot base is recorded at each time-step. This recorded transformation and the pre-computed transformation from the robot base to the object are used to estimate the ground truth object pose. By following this procedure, the object pose with respect to the robot camera is progressively estimated as the camera approaches the object.

Simulating robot camera views while navigating toward the target object. Ground truth object pose at each time step is annotated by using the robot’s TCP pose at each time step and initially recorded transformation between the object and the robot’s TCP (Figure 6).

External camera views are obtained using cameras mounted on the wall (Figure 1). The extrinsic calibration of all the cameras, both robot and external, is initially performed with respect to a ChArUco marker.

Evaluation metrics

3.1.3

We evaluate pose estimation performance using ADD and ADD-S metrics as previously proposed (Hinterstoisser et al., 2013; Xiang et al., 2017; Deng et al., 2021). Given the ground truth rotation $[eqn]$ and translation $[eqn]$ , the estimated rotation $[eqn]$ and translation $[eqn]$ , the set of 3D model points $[eqn]$ and the number of points $[eqn]$ , we define the following.

• ADD: The average distance computes the mean of the pairwise distances between the 3D model points transformed according to the ground truth and estimated pose as described in Equation 12.

$$[eqn]$$

• ADD-S: For symmetric and partially symmetric objects such as a bowl, mug, and banana, the match between points is ambiguous for some views. Therefore, the average distance is computed using the closest point distance as described in Equation 13.

$$[eqn]$$

The 6D pose is considered correct if the average distance is smaller than a pre-defined threshold. We report area under the curve (AUC) of ADD and ADD-S (Labbé et al., 2020) for threshold ranging from 0 to 10 cm in steps of 1 cm.

To evaluate the object pose uncertainty quantification, we use the following metrics.

• Log probability of ground truth translation: the log of the probability assigned to the ground truth translation in the estimate of the posterior translation distribution3 $[eqn]$ .
• Log probability of ground truth rotation: the log of the probability assigned to the ground truth in the estimate of the posterior rotation distribution $[eqn]$ .

These metrics assess whether the distribution accurately assigns probability mass to the ground truth translation and rotation. As the log is undefined for zero probability, the minimum probability values were clipped to 1e−5. Thus, the minimum value for the log probability of ground truth translation and ground truth rotation is $[eqn]$ .

Hyperparameters and implementation details

3.1.4

We used a three-camera setup (two external cameras and one robot: $[eqn]$ ) for evaluation (Figure 1). As noted in Deng et al. (2021), approximately 100 particles result in optimal performance. Hence, during initialization, 40 particles were sampled around object detection in each camera frame. Thus, the total number of particles $[eqn]$ was set to 120.

To initialize the particles, the translation estimate $[eqn]$ was determined using the center of the 2D bounding box of the object obtained using the Detectron2 (Wu et al., 2019) object detector4 and the corresponding depth in the depth image.

The particles were then sampled around these estimates, assuming normal distributed noise with a standard deviation $[eqn]$ of 20 pixels in the $[eqn]$ and $[eqn]$ estimates and 10 cm in the $[eqn]$ estimate (Equation 6). The same strategy was also employed to initialize the particles for the baseline. The standard deviations $[eqn]$ can be increased if more noise is expected in the translation estimates used for initialization. However, the larger the noise, the more particles are required to uniformly cover the translation space.

During the multi-view fusion, $[eqn]$ was set to 0.6, as it was observed that pose distributions in the particular camera frame were unreliable when $[eqn]$ fell below 0.65 (Deng et al., 2021). $[eqn]$ was set to 1e−5. $[eqn]$ is used for additive filtering and hence should be a small value. For computing observation likelihoods, de-noising auto-encoders were trained for each object as detailed in Deng et al. (2021). To identify supporting samples, $[eqn]$ was set to 30° and $[eqn]$ was set to 1e−5. $[eqn]$ and $[eqn]$ were used for associating the rotational hypothesis from the previous time-step $[eqn]$ to rotation distribution at time $[eqn]$ . Thus, for strict association, $[eqn]$ should be decreased and $[eqn]$ increased. To decide whether the rotational hypothesis $[eqn]$ should be carried over to the next time-step, $[eqn]$ was set to 0.1. The multiplicative inverse of $[eqn]$ implicitly defines the maximum number of rotation hypotheses at a given time step. Therefore, the smaller the value of $[eqn]$ , the larger the maximum number of rotation hypotheses that can be maintained. The unimodal or multi-modal nature of the posterior distributions was assessed using the Henze−Zirkler multivariate normality test (Henze and Zirkler, 1990).

All experiments were conducted on a workstation equipped with an Intel i9 processor, 32 GB RAM, and an NVIDIA GeForce RTX 3090 graphics card. The execution speed was approximately 4 Hz for the three-camera setup.

Results

3.2

In this section, we present the results of our experiments. We first provide a qualitative example in Section 3.2.1. Quantitative results that validate the objectives discussed in Section 3.1.1 are presented in Section 3.2.2.

Qualitative results

3.2.1

Figure 8 demonstrates the role of external cameras in preventing the particle filter from diverging, even when the object becomes severely occluded in the robot camera view. Initially $[eqn]$ , and the object is visible in the robot camera view. However, translation errors in the estimated object pose are notably high when using both the robot and external cameras (Figure 8a) and when using only the robot camera (Figure 8e). This is attributed to noisy depth images due to the considerable distance (approximately 3 m) between the camera and the object. However, rotational errors remain low in both scenarios (Figures 8b,f), since a unique view of the object is available in the robot camera during this period.

Comparison of object pose distribution tracking using Ours and PoseRBPF (Deng et al., 2021). First row: Robot views at different time frames, external camera views, tracked object (cracker box). Second row: (a) translational error, (b) rotational error, (c) log-probability of ground truth translation and (d) log-probability of ground truth rotation for ours. Third row: (e) translational error, (f) rotational error, (g) log-probability of ground truth translation and (h) log-probability of ground truth rotation for PoseRBPF.

As the robot approaches the object, it becomes increasingly occluded in the robot camera view $[eqn]$ , resulting in an increase in errors in both scenarios. However, there is a noteworthy difference in how these errors evolve with and without the use of external cameras. In the case of multi-view (Figure 8b), the rotational error increases as the filter starts to rely on observations from external cameras which are more uncertain than robot camera views without occlusion. However, evidence provided by the external cameras during this period prevents the particle filter from diverging (Figure 8c), and as a result, it quickly converges once the object starts to re-appear in the robot camera view. Quick convergence and accurate quantification of uncertainty can enable the robot to make early and reliable decisions as it approaches the object for grasping. Conversely, while just using a robot camera, the filter starts to diverge (Figure 8g) due to noisy evidence provided by the robot camera during this period. As a result, there is a large increase in translational (Figure 8e) and rotational (Figure 8f) errors. Eventually, the filter converges at the end of the sequence (Figures 8e,f).

More qualitative results can be found here5.

Quantitative results

3.2.2

Table 1 presents pose estimation results using two baselines—PoseRBPF (Deng et al., 2021) and CosyPose (Labbé et al., 2020) —and the proposed framework. As PoseRBPF is a single-view pose estimation method, we present pose estimates in a robot and two external cameras. These single-view pose estimates were then used as an input to CosyPose for refinement6. For large objects with distinct features, external cameras result in better performance as they have an unoccluded view of the object as well as sufficient pixel information for pose estimation. The performance also depends on the view of the object, as the external cameras are stationary.

CosyPose is able to slightly improve the pose estimates obtained using PoseRBPF in the robot camera by refining them using the pose estimates obtained using PoseRBPF in the external cameras. However, the difference is not significant, since the estimates obtained in the external cameras have large errors, especially for small objects.

Our framework achieves significantly better performance on objects such as the “006_mustard_bottle,” “010_potted_meat_can,” “021_bleach_cleanser,” “024_bowl,” and “025_mug.” This is because these objects are partially symmetric and have smaller sizes. As a result, during occlusion in the robot camera, they are either very severely occluded or have ambiguous views. The additional views from external cameras prevent the particle filter from diverging during occlusions when the robot camera has a poor view of the object. As a result, the particle filter quickly converges to the accurate pose estimate when the object once again has a good view in the robot camera. Without the additional views from the external cameras, the particle filter often needs to be re-initialized after occlusions (Deng et al., 2021). In the case of objects like “011_banana” and “052_extra_large_clamp,” the robot and external cameras often have contradictory observations that cannot be reliably captured by the failure model (Section 2.3.4); this leads to inaccurate distributions after fusion.

In Table 2, we present pose uncertainty quantification results using PoseRBPF (Deng et al., 2021) and the proposed framework. The proposed framework results in more accurate translation distributions, as indicated by the high log probability of ground truth translation. This is because, when sufficient evidence is not available, it prevents premature convergence and maintains multimodal translation distributions, while PoseRBPF incorrectly converges to inaccurate translation distributions. Preventing premature convergence is crucial for preventing the particle filter from diverging when the robot camera has poor views of the object.

Additionally, in the case of objects with distinct features and larger sizes such as the “003_cracker_box,” PoseRBPF has a slightly higher probability mass in the ground truth rotation. This is because if a robot camera has a good view of the object and a well-estimated translation, external cameras due to their large distance to the object can introduce noise during fusion. However, the use of external cameras results in much better rotation distributions for other objects that are partially symmetric and have smaller sizes.

Ablation studies

3.2.3

In this section, we first present three ablation studies to validate our design choices, followed by an ablation study on pose estimation and uncertainty quantification using different camera combinations for fusion.

Multi-view fusion

3.2.3.1

In this study, we validate the fusion approach outlined in Section 2.3.4. We compare our approach against two baselines. In the first, we directly fuse all the observations (similar to Naik et al. (2022)), while in the second baseline we perform additive filtering before fusion. Our approach (“Ours”), in addition to additive filtering, uses a failure model described in Section 2.3.4 to determine whether a particular observation should be included during the fusion. In the experiments, all particles at each time-step were injected using ground truth translations to ensure that design choices during the other steps in the particle filter did not affect the fusion. We measure the performance using the log of probability assigned to the ground truth rotation in the fused distribution. Results are presented in Table 3.

Fusion without additive filtering results in poor performance (Table 3) because if the object is not visible in one of the cameras, observations from other cameras are also lost. Fusion with additive filtering addresses this and significantly improves performance as it retains good observations from other cameras before fusion. However, it does not effectively handle situations in which one of the cameras provides a poor view of the object, resulting in noisy observations. Fusing such noisy observations with additive filtering has the potential to corrupt good observations from other cameras. To mitigate this, we employ the failure model described in Section 2.3.4 to determine whether a particular observation should be included during fusion, which results in improved performance—as indicated by the high log probability of ground truth rotation in the right column of Table 3.

Re-sampling

3.2.3.2

In this study, we validate the re-sampling approach outlined in Section 2.3.5. We compare our approach against two baselines. In the first, we re-sample all particles simultaneously using low-variance re-sampling (Thrun, 2002; Deng et al., 2021; Naik et al., 2022). In the second baseline, we separately re-sample particles originating from different camera observations at $[eqn]$ , so that particles from all camera observations are retained throughout tracking. Our approach (“Ours”) dynamically adjusts the number of particles originating from different camera observations at $[eqn]$ based on the particles’ aggregate weight. Subsequently, these particles are separately re-sampled, as described in Section 2.3.5. The performance of all three approaches is measured by comparing the log probability of the ground truth translation in the resultant translation distribution.

Re-sampling all the particles simultaneously (first baseline) leads to improved performance for large objects with distinct features, as demonstrated in Table 4, compared to our approach and the second baseline. This is because the robot camera often provides high-quality observations of these objects since they are rarely fully occluded due to their large size, while external cameras tend to have comparatively noisy observations due to their greater distance from the object. For these objects, simultaneously re-sampling all particles quickly eliminates noisy particles from the external cameras, resulting in more compact translation distributions than our approach, where particles from all cameras are retained until sufficient temporal evidence is available or the second baseline, where particles originating from different frames are retained at all times.

However, the advantage of our approach is clearly evident in the case of challenging objects where the robot camera does not always provide a good observation due to partial symmetry or smaller object sizes. Our approach enables particles from different cameras to refine their estimates instead of quickly converging to specific estimates, as observed in the first baseline. Furthermore, it does not retain noisy particles all the time, as in the second baseline. Overall, then, our approach outperforms both baselines, particularly in the case of challenging objects (Table 4).

Pose estimation

3.2.3.3

In this study, we validate the pose estimation from the tracked distribution. We use the approach in Deng et al. (2021) and Naik et al. (2022) as the baseline, where the pose is estimated using the mean of the samples in the current distribution that could be associated with the pose estimate at the previous time-step $[eqn]$ . Our approach (“Ours”), in addition to the samples in the current distribution that could be associated with the pose estimate at the previous time-step $[eqn]$ , also considers the samples that could not be associated. As a result, it maintains a multiple set of hypotheses when sufficient evidence is not available to converge to a single pose estimate (Section 2.3.6).

The same tracked distribution was used to evaluate both methods, with performance measured using the AUC of ADD and ADD-S metrics.

It can be observed from Table 5 that Deng et al. (2021) generally perform poorly, as they assume that a good initial object pose estimate is available. This assumption is often not valid when tracking from a distance. Our approach maintains a multiple pose hypothesis when sufficient temporal evidence is not then available $[eqn]$ in support of the then pose estimate $[eqn]$ . It is thus able to recover from poor pose estimates and hence results in better performance.

Different camera combinations

3.2.4

In this ablation study, we compare the performance of pose estimation and uncertainty quantification with fusion using different camera combinations. We consider three cases: i) robot camera and external camera 1 (EI); ii) robot camera and external camera 2 (E2); iii) robot camera, E1, and E2 (refer to Figure 1 for the positioning of the cameras). Results are presented in Table 6. The combination of the robot camera and E1 yields better performance than the combination of the robot camera and E2. The performance using the combination of the robot camera and E2 is very similar to the performance of just using the robot camera (Table 1). This is because E1 provides more informative views as it is positioned at a lower elevation than E2 (relative to the object frame). Consequently, E1 frequently has better views of the object’s front, back, and sides, which typically contain more unique features. In contrast, E2, due to its higher elevation, often has a better view of the object’s top compared to the views of its sides, front, and back.

However, when all the cameras are used together, there is a significant improvement in performance compared to using only the robot camera and E1. This is because E1 and E2 provide complementary views of the object (both are placed on opposite sides of the object; see Figure 1). These complementary views help resolve uncertainties and lead to improved pose estimates and uncertainty quantification.

Application: pre-grasp planning

3.3

We applied the proposed multi-view object pose distribution tracking framework for making informed decisions at various stages of mobile manipulation (MM). As described in Section 1, in addition to selecting the optimal grasp pose for grasping the object (Newbury et al., 2022), MM also requires selecting the optimal base pose from where the object can be grasped (Vahrenkamp et al., 2013; Makhal and Goins, 2018; Jauhri et al., 2022b).

An estimate of object pose $[eqn]$ is required to select the optimal base poses and grasp poses for grasping the objects. Additionally, estimates of the selected optimal grasp poses and base poses are also derived from the estimated object pose. Therefore, any error in estimated object pose $[eqn]$ can result in failures. An error in the base pose can potentially result in a situation where there are no inverse kinematics (IK) solutions for grasping the object after navigating to the estimated base pose, while an error in the estimated grasp pose might lead to a failed grasp.

As the proposed framework estimates the object pose $[eqn]$ and the associated uncertainty $[eqn]$ from a distance, the robot can make early decisions regarding the optimal base pose and grasp pose for grasping the object. Moreover, instead of directly performing the actions—navigating to the estimated base pose or attempting to grasp using the estimated grasp pose—the robot can project the object pose uncertainty $[eqn]$ onto the action space and assess whether the estimated uncertainty is acceptable for the successful execution of the action.

For the base pose, the robot can evaluate if deviations according to the uncertainty associated with the estimate of the selected base pose would still allow for IK solutions for grasping the object. Similarly, for the grasp pose, it can validate in the simulation (Kim et al., 2012) whether the deviations according to the uncertainty associated to the estimate of the selected optimal grasp pose would enable a successful grasp, as a gripper can compensate for the pose uncertainty. For mathematical details, we refer the reader to Naik et al. (2025).

In our experiments, we assumed that the top-down grasp pose was selected as the optimal grasp pose for grasping the objects. We examined four cases, each involving the use of $[eqn]$ for decision-making at an increased number of stages than the previous case.

• Fixed base pose and object pose estimation (FBOE): the robot goes to a fixed base pose near the table, estimates object pose, and attempts to grasp the object using the estimate of the selected grasp pose. This represents how MM is usually solved.
• Object pose tracking and base pose estimation (OTBE): the robot starts tracking the object pose by means of our method, obtains an estimate of the selected base pose, and navigates to the estimated base pose. It then obtains the estimate of the selected grasp pose and attempts to grasp the object.
• Object pose tracking and base IK monitoring (OTBM): OTBE plus the robot monitors the probability of the availability IK solution using the estimate of the selected base pose and the uncertainty in the object pose used for estimating the base pose. The robot decides to navigate to the estimated base pose only if the probability of success exceeds a pre-defined threshold.
• Object pose tracking and base IK and grasp success monitoring (OTBGM): OTBM plus the robot monitors the probability of grasp success using the estimate of the selected grasp pose and the uncertainty in the object pose used for estimating the grasp pose. The robot decides to attempt to grasp using the estimated grasp pose only if the probability of success exceeds a pre-defined threshold.

In OTBM and OTBGM, the pre-defined threshold for both base and grasp poses was set to 60%—that is, if more than 60% of the errors could be compensated for, the action was executed. In practice, aside from errors in visual object pose estimates, several other factors can contribute to failures during action execution. These may include poor calibration between the external camera and the robot camera due to errors in robot localization, and inability to plan a trajectory to the intended grasp pose due to occlusion from other objects. To isolate these failures and only quantify failures due to errors in the visual object pose estimate, we created digital twins for the sequences in the MY-MM dataset using the recorded ground truths and NVIDIA Isaac simulator7. The decisions were based on the real-world data from the MY-MM dataset sequences, while actions were executed in the digital twin.

We evaluated five objects (“003_cracker_box,” “006_mustard_bottle,” “021_bleach_cleanser,” “024_bowl,” and “025_mug”) with diverse shapes, colors, and textures, conducting ten trials for each object and each case. FBOE and OTBE do not consider uncertainty in the object pose, resulting in a large number of failures (Table 7).

On the other hand, OTBM and OTBGM monitor uncertainties in the tracked object pose before making MM decisions. Consequently, they exhibit a much higher success rate and fewer failures. Most of the failures in OTBGM occurred due to the absence of a valid IK solution at the estimated base pose. To mitigate these failures, one option is to raise the threshold for decision-making, requiring more than 60% of estimated errors to be compensated for before executing the action. However, this strategy comes with the trade-off of delaying decision-making, which results in longer execution times.

In summary, the early pose estimates from the proposed object pose distribution tracking framework not only empower the mobile robot to make timely decisions regarding the optimal base pose and grasp pose for object manipulation but also the associated pose uncertainty, provided by the proposed framework, helping to prevent failures. In this way, the proposed framework contributes to advances in both the time efficiency and robustness of mobile manipulation.

Discussion

4

In this study, we have presented a particle filter-based framework for multi-view 6D object pose distribution tracking using the robot’s onboard camera and external cameras in the environment. We demonstrated that the proposed framework yields more accurate 6D object pose estimates than existing multi- and single-camera pose estimation methods and distributions compared to existing single-camera pose distribution estimation methods as the mobile robot approaches the objects for grasping. The external cameras not only help resolve ambiguities when the robot camera has ambiguous views of the object but also prevent the particle filter from diverging. Consequently, the particle filter quickly converges once the robot camera views start to improve, unlike when using only the robot camera, where the particle filter needs to be re-initialized. Finally, we contributed a multi-view YCB object pose tracking dataset for mobile manipulation that can serve as a benchmark for future research in this direction.

Our results are encouraging, as they indicate that the use of external cameras can significantly improve the accuracy of the estimated object pose and distributions in the robot camera. This can enable the mobile robot to make early and informed decisions, thereby improving both the time efficiency and robustness of mobile manipulation.

We here used the PoseRBPF method (Deng et al., 2021) for computing a discretized rotational distribution, which uses Euler angle representation. However, this introduces a risk of Gimbal lock during multi-view fusion. Future research should investigate the use of quaternions for discretizing SO(3), as proposed by Murphy et al. (2021) and Haugaard et al. (2023). Furthermore, the single-view observation likelihood model from PoseRBPF (Deng et al., 2021) used in this work exhibits reduced performance on YCB objects such as “011 banana” and “052 extra-large clamp” under occlusions. This highlights the need for more robust single-view observation likelihood models capable of handling such challenging cases. Finally, our framework can be combined with a partially observable Markov decision framework (PO-MDP), as applied in An et al. (2024), to determine actions that will reduce uncertainty and provide better pose estimates and distributions.