DenseFusion: 6D Object Pose Estimation by Iterative Dense Fusion Chen Wang 2 Danfei Xu 1 Yuke Zhu 1 Roberto Mart´ ın-Mart´ ın 1 Cewu Lu 2 Li Fei-Fei 1 Silvio Savarese 1 1 Department of Computer Science, Stanford University 2 Department of Computer Science, Shanghai Jiao Tong University Abstract A key technical challenge in performing 6D object pose estimation from RGB-D image is to fully leverage the two complementary data sources. Prior works either extract in- formation from the RGB image and depth separately or use costly post-processing steps, limiting their performances in highly cluttered scenes and real-time applications. In this work, we present DenseFusion, a generic framework for estimating 6D pose of a set of known objects from RGB- D images. DenseFusion is a heterogeneous architecture that processes the two data sources individually and uses a novel dense fusion network to extract pixel-wise dense fea- ture embedding, from which the pose is estimated. Further- more, we integrate an end-to-end iterative pose refinement procedure that further improves the pose estimation while achieving near real-time inference. Our experiments show that our method outperforms state-of-the-art approaches in two datasets, YCB-Video and LineMOD. We also deploy our proposed method to a real robot to grasp and manipulate objects based on the estimated pose. Our code and video are available at https://sites.google.com/view/densefusion/. 1. Introduction 6D object pose estimation is the crux to many important real-world applications, such as robotic grasping and ma- nipulation [ 7 , 35 , 44 ], autonomous navigation [ 6 , 11 , 42 ], and augmented reality [ 19 , 20 ]. Ideally, a solution should deal with objects of varying shape and texture, show robust- ness towards heavy occlusion, sensor noise, and changing lighting conditions, while achieving the speed requirement of real-time tasks. The advent of cheap RGB-D sensors has enabled methods that infer poses of low-textured ob- jects even in poorly-lighted environments more accurately than RGB-only methods. Nonetheless, it is difficult for ex- isting methods to satisfy the requirements of accurate pose estimation and fast inference simultaneously. Classical approaches first extract features from RGB-D data and perform correspondence grouping and hypothesis verification [ 3 , 12 , 13 , 15 , 26 , 33 , 38 ]. However, the re- RGB-D DenseFusion Figure 1. We develop an end-to-end deep network model for 6D pose estimation from RGB-D data, which performs fast and accu- rate predictions for real-time applications such as robot grasping and manipulation. liance on handcrafted features and fixed matching proce- dures have limited their empirical performances in presence of heavy occlusion and lighting variation. Recent success in visual recognition has inspired a family of data-driven methods that use deep networks for pose estimation from RGB-D inputs, such as PoseCNN [ 41 ] and MCN [ 16 ]. However, these methods require elaborate post-hoc re- finement steps to fully utilize the 3D information, such as a highly customized Iterative Closest Point (ICP) [ 2 ] procedure in PoseCNN and a multi-view hypothesis ver- ification scheme in MCN. These refinement steps cannot be optimized jointly with the final objective and are pro- hibitively slow for real-time applications. In the context of autonomous driving, a third family of solutions has been proposed to better exploit the complementary nature of color and depth information from RGB-D data with end- to-end deep models, such as Frustrum PointNet [ 23 ] and PointFusion [ 42 ]. These models have achieved good per- formances in driving scenes and the capacity of real-time inference. However, as we demonstrate empirically, these methods fall short under heavy occlusion, which is common 3343in manipulation domains. In this work, we propose an end-to-end deep learning ap- proach for estimating 6-DoF poses of known objects from RGB-D inputs. The core of our approach is to embed and fuse RGB values and point clouds at a per-pixel level, as opposed to prior work which uses image crops to compute global features [ 42 ] or 2D bounding boxes [ 23 ]. This per- pixel fusion scheme enables our model to explicitly rea- son about the local appearance and geometry information, which is essential to handle heavy occlusion. Furthermore, we propose an iterative method which performs pose re- finement within the end-to-end learning framework. This greatly enhances model performance while keeping infer- ence speed real-time. We evaluate our method in two popular benchmarks for 6D pose estimation, YCB-Video [ 41 ] and LineMOD [ 12 ]. We show that our method outperforms the state-of-the-art PoseCNN after ICP refinement [ 41 ] by 3.5% in pose ac- curacy while being 200x faster in inference time. In par- ticular, we demonstrate its robustness in highly cluttered scenes thanks to our novel dense fusion method. Last, we also showcase its utility in a real robot task, where the robot estimates the poses of objects and grasp them to clear up a table. In summary, the contributions of this work are two-fold: First, we present a principled way to combine color and depth information from the RGB-D input. We augment the information of each 3D point with 2D information from an embedding space learned for the task and use this new color-depth space to estimate the 6D pose. Second, we in- tegrate an iterative refinement procedure within the neural network architecture, removing the dependency of previous methods of a post-processing ICP step. 2. Related Work Pose from RGB images. Classical methods rely on detect- ing and matching keypoints with known object models [ 1 , 7 , 9 , 27 , 44 ]. Newer methods address the challenge by learn- ing to predict the 2D keypoints [ 3 , 22 , 32 , 34 , 35 ] and solve the poses by P n P [ 10 ]. Though prevail in speed-demanding tasks, these methods become unreliable given low-texture or low-resolution inputs. Other methods propose to directly estimate objects pose from images using CNN-based archi- tectures [ 28 , 36 ]. Many such methods focus on orientation estimation: Xiang et al. [ 39 , 40 ] learns a viewpoint-aware pose estimator by clustering 3D features from object mod- els. Mousavian et al. [ 21 ] predicts 3D object parameters and recovers poses by single-view geometry constraints. Sun- dermeyer et al. [ 31 ] implicitly encode orientation in a latent space and in test time find the best match in a codebook as the orientation prediction. However, pose estimation in 3D remains a challenge for the lack of depth information. Our method leverages both image and 3D data to estimate object poses in 3D in an end-to-end architecture. Pose from depth / point cloud. Recent studies have pro- posed to directly tackle the 3D object detection problem in discretized 3D voxel spaces. For example, Song et al. [ 29 , 30 ] generate 3D bounding box proposals and estimate the poses by featuring the voxelized input with 3D ConvNets. Although the voxel representation effectively encodes ge- ometric information, these methods are often prohibitively expensive: [ 30 ] takes nearly 20 seconds for each frame. More recent 3D deep learning architectures have en- abled methods that directly performs 6D pose estimation on 3D point cloud data. As an example, both Frustrum PointNets [ 23 ] and VoxelNet [ 43 ] use a PointNet-like [ 24 ] structure and achieved state-of-the-art performances on the KITTI benchmark [ 11 ]. Our method also makes use of sim- ilar architecture. However, unlike urban driving applica- tions for which point cloud alone provides enough informa- tion, generic object pose estimation tasks such as the YCB- Video dataset [ 41 ] demands reasoning over both geometric and appearance information. We address such a challenge by proposing a novel 2D-3D sensor fusion architecture. Pose from RGB-D data. Classical approaches extract 3D features from the input RGB-D data and perform corre- spondence grouping and hypothesis verification [ 3 , 12 , 13 , 15 , 26 , 33 , 38 ]. However, these features are either hard- coded [ 12 , 13 , 26 ] or learned by optimizing surrogate ob- jectives [ 3 , 33 , 38 ] such as reconstruction [ 15 ] instead of the true objective of 6D pose estimation. Newer methods such as PoseCNN [ 41 ] directly estimates 6D poses from im- age data. Li et al. [ 16 ] further fuses the depth input as an additional channel to a CNN-based architecture. However, these approaches rely on expensive post-processing steps to make full use of 3D input. In comparison, our method fuses 3D data to 2D appearance feature while retaining the geo- metric structure of the input space, and we show that it out- performs [ 41 ] on the YCB-Video dataset [ 41 ] without the post-processing step. Our method is most related to PointFusion [ 42 ], in which geometric and appearance information are fused in a het- erogeneous architecture. We show that our novel local fea- ture fusion scheme significantly outperforms PointFusion’s naive fusion-by-concatenation method. In addition, we use a novel iterative refinement method to further improve the pose estimation. 3. Model Our goal is to estimate the 6D pose of a set of known objects present in an RGB-D image of a cluttered scene. Without loss of generality, we represent 6D poses as a ho- mogeneous transformation matrix, p ∈ SE (3) . In other words, a 6D pose is composed by a rotation R ∈ SO (3) and a translation t ∈ R 3 , p = [ R | t ] . Since we estimate the 3344object segmentation PointNet image crop color embeddings geometry embeddings pixel-wise dense fusion matching point ... pixel-wise feature average pooling global feature ... (x 1 ,y 1 ) (x 1 ,y 1 ) (x N ,y N ) (x 2 ,y 2 ) (x N ,y N ) rotation translation confidence pixel (x i ,y i ) i = 1...N R i pose predictor c i prediction per pixel argmax( c ) 6D pose estimation per-pixel feature masked point cloud CNN t i MLP Figure 2. Overview of our 6D pose estimation model. Our model generates object segmentation masks and bounding boxes from RGB images. The RGB colors and point cloud from the depth map are encoded into embeddings and fused at each corresponding pixel. The pose predictor produces a pose estimate for each pixel and the predictions are voted to generate the final 6D pose prediction of the object. (The iterative procedure of our approach is not depicted here for simplicity) 6D pose of the objects from camera images, the poses are defined with respect to the camera coordinate frame. Estimating the pose of a known object in adversarial conditions (e.g. heavy occlusion, poor lighting, . . . ) is only possible by combining the information contained in the color and depth image channels. However, the two data sources reside in different spaces. Extracting features from heterogeneous data sources and fusing them appropriately is the key technical challenge in this domain. We address this challenge with (1) a heterogeneous ar- chitecture that processes color and depth information dif- ferently, retaining the native structure of each data source (Sec. 3.3 ), and (2) a dense pixel-wise fusion network that performs color-depth fusion by exploiting the intrinsic map- ping between the data sources (Sec. 3.4 ). Finally, the pose estimation is further refined with a differentiable iterative refinement module (Sec. 3.6 ). In contrast to the expensive post-hoc refinement steps used in [ 16 , 41 ], our refinement module can be trained jointly with the main architecture and only takes a small fraction of the total inference time. 3.1. Architecture Overview Fig. 2 illustrates the overall proposed architecture. The architecture contains two main stages. The first stage takes color image as input and performs semantic segmentation for each known object category. Then, for each segmented object, we feed the masked depth pixels (converted to 3D point cloud) as well as an image patch cropped by the bounding box of the mask to the second stage. The second stage processes the results of the segmenta- tion and estimates the object’s 6D pose. It comprises four components: a) a fully convolutional network that processes the color information and maps each pixel in the image crop to a color feature embedding, b) a PointNet-based [ 24 ] net- work that processes each point in the masked 3D point cloud to a geometric feature embedding, c) a pixel-wise fusion network that combines both embeddings and outputs the es- timation of the 6D pose of the object based on an unsuper- vised confidence scoring, and d) an iterative self-refinement methodology to train the network in a curriculum learning manner and refine the estimation result iteratively. Fig. 2 depicts a), b) and c) and Fig. 3 illustrates d). The details our architecture are described below. 3.2. Semantic Segmentation The first step is to segment the objects of interest in the image. Our semantic segmentation network is an encoder- decoder architecture that takes an image as input and gener- ates an N +1 -channelled semantic segmentation map. Each channel is a binary mask where active pixels depict objects of each of the N possible known classes. The focus of this work is to develop a pose estimation algorithm. Thus we use an existing segmentation architecture proposed by [ 41 ]. 3.3. Dense Feature Extraction The key technical challenge in this domain is the correct extraction of information from the color and depth channels and their synergistic fusion. Even though color and depth present a similar format in the RGB-D frame, their infor- mation resides in different spaces. Therefore, we process them separately to generate color and geometric features from embedding spaces that retain the intrinsic structure of the data sources. Dense 3D point cloud feature embedding: Previous ap- 3345proaches have used CNN to process the depth image as an additional image channel [ 16 ]. However, such method ne- glects the intrinsic 3D structure of the depth channel. In- stead, we first convert the segmented depth pixels into a 3D point cloud using the known camera intrinsics, and then use a PointNet-like architecture to extract geometric features. PointNet by Qi et al. [ 24 ] pioneered the use of a symmet- ric function (max-pooling) to achieve permutation invari- ance in processing unordered point sets. The original archi- tecture takes as input a raw point cloud and learns to encode the information about the vicinity of each point and of the point cloud as a whole. The features are shown to be effec- tive in shape classification and segmentation [ 24 ] and pose estimation [ 23 , 42 ]. We propose a geometric embedding network that generates a dense per-point feature by map- ping each of the P segmented points to a d geo -dimensional feature space. We implement a variant of PointNet architec- ture that uses average-pooling as opposed to the commonly used max-pooling as the symmetric reduction function. Dense color image feature embedding: The goal of the color embedding network is to extract per-pixel features such that we can form dense correspondences between 3D point features and image features. The reason for form- ing these dense correspondences will be clear in the next section. The image embedding network is a CNN-based encoder-decoder architecture that maps an image of size H × W × 3 into a H × W × d rgb embedding space. Each pixel of the embedding is a d rgb -dimensional vector repre- senting the appearance information of the input image at the corresponding location. 3.4. Pixel-wise Dense Fusion So far we have obtained dense features from both the image and the 3D point cloud inputs; now we need to fuse the information. A naive approach would be to generate a global feature from the dense color and depth features from the segmented area. However, due to heavy occlu- sion and segmentation errors, the set of features from the previous step may contain features of points/pixels on other objects or parts of the background. Therefore, blindly fus- ing color and geometric features globally would degrade the performance of the estimation. In the following we describe a novel pixel-wise 1 dense fusion network that effectively combines the extracted features, especially for pose estima- tion under heavy occlusion and imperfect segmentation. Pixel-wise dense fusion: The key idea of our dense fu- sion network is to perform local per-pixel fusion instead of global fusion so that we can make predictions based on each fused feature. In this way, we can potentially select the predictions based on the visible part of the object and minimize the effects of occlusion and segmentation noise. 1 Since the mapping between pixels and 3D points is unique, we will use interchangeably pixel-fusion and point-fusion . Concretely, our dense fusion procedure first associates the geometric feature of each point to its corresponding image feature pixel based on a projection onto the image plane us- ing the known camera intrinsic parameters. The obtained pairs of features are then concatenated and fed to another network to generate a fixed-size global feature vector using a symmetric reduction function. While we refrained from using a single global feature for the estimation, here we en- rich each dense pixel-feature with the global densely-fused feature to provide a global context. We feed each of the resulting per-pixel features into a final network that predicts the object’s 6D pose. In other words, we will train this network to predict one pose from each densely-fused feature. The result is a set of P pre- dicted poses, one per feature. This defines our first learning objective, as we will see in Sec. 3.5 . We will now explain our approach to learning to choose the best prediction in a self-supervised manner, inspired by Xu et al. [ 42 ]. Per-pixel self-supervised confidence: We would like to train our pose estimation network to decide which pose es- timation is likely to be the best hypothesis based on the spe- cific context. To do so, we modify the network to output a confidence score c i for each prediction in addition to the pose estimation predictions. We will have to reflect this sec- ond learning objective in the overall learning objective, as we will see at the end of the next section. 3.5. 6D Object Pose Estimation Having defined the overall network structure, we now take a closer look at the learning objective. We define the pose estimation loss as the distance between the points sam- pled on the objects model in ground truth pose and cor- responding points on the same model transformed by the predicted pose. Specifically, the loss to minimize for the prediction per dense-pixel is defined as L p i = 1 M ∑ j || ( Rx j + t ) − ( ˆ R i x j + ˆ t i ) || (1) where x j denotes the j th point of the M randomly selected 3D points from the object’s 3D model, p = [ R | t ] is the ground truth pose, and ˆ p i = [ ˆ R i | ˆ t i ] is the predicted pose generated from the fused embedding of the i th dense-pixel. The above loss function is only well-defined for asym- metric objects, where the object shape and/or texture deter- mines a unique canonical frame. Symmetric objects have more than one and possibly an infinite number of canoni- cal frames, which leads to ambiguous learning objectives. Therefore, for symmetric objects, we instead minimize the distance between each point on the estimated model orien- tation and the closest point on the ground truth model. The 3346color embeddings current input point cloud geometry embeddings pose residual estimator rotation residual 𝚫 R 𝚫 t next iteration transformed point cloud translation residual Dense Fusion global feature PointNet Figure 3. Iterative Pose Refinement. We introduce an network module that refines the pose estimation in an iterative procedure. loss function becomes: L p i = 1 M ∑ j min 0