GeoFusion: Geometric Consistency informed Scene Estimation in Dense Clutter
Zhiqiang Sui Haonan Chang Ning Xu Odest Chadwicke Jenkins
cs.RO















IEEE Robotics and Automation Letters. Preprint Version. July, 2020
Sui et al.: GeoFusion: Geometric Consistency informed Scene Estimation 


GeoFusion: Geometric Consistency informed 

Scene Estimation in Dense Clutter

    Zhiqiang Sui, Haonan Chang,  Ning Xu, and Odest Chadwicke Jenkins
Manuscript received: March 1, 2020; Revised May 30, 2020; Accepted June 23, 2020.
This paper was recommended for publication by Editor Cesar Cadena upon evaluation of the Associate Editor and Reviewers' comments.
This work was supported by the Laboratory for Progress, University of Michigan, Ann Arbor 
Zhiqiang Sui, Haonan Changr,  Ning Xu, and Odest Chadwicke Jenkins are with the Department of Electrical Engineering and Computer Science, Robotics Institute, University of Michigan, Ann Arbor, MI, USA
        zsui@umich.edu
Digital Object Identifier (DOI): see top of this page.

    December 30, 2023
==============================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================






We propose GeoFusion, a SLAM-based scene estimation method for building an object-level semantic map in dense clutter.  In dense clutter, objects are often in close contact and severe occlusions, which brings more false detections and noisy pose estimates from existing perception methods. To solve these problems, our key insight is to consider geometric consistency at the object level within a general SLAM framework. The geometric consistency is defined in two parts: geometric consistency score and geometric relation. The geometric consistency score describes the compatibility between object geometry model and observation point cloud. Meanwhile, it provides a reliable measure to filter out false positives in data association. The geometric relation represents the relationship (e.g. contact) between geometric features (e.g. planes) among objects. The geometric relation makes the graph optimization for poses more robust and accurate. can robustly and efficiently infer the object labels, 6D object poses, and spatial relations from continuous noisy semantic measurements. We quantitatively evaluate our method using observations from a Fetch mobile manipulation robot.  Our results demonstrate greater robustness against false estimates than frame-by-frame pose estimation from the state-of-the-art convolutional neural network. 










RGB-D Perception, Mapping, SLAM, Perception for Grasping and Manipulation






§ INTRODUCTION



To make autonomous robots taskable such that they function properly, meet human expectations, and interact fluently with human partners, they must be able to perceive and understand the semantics of their environments <cit.>. More specifically, as robots move around in the environment, they must know what objects are presented as well as their locations. It is desired for robots to understand the semantic aspects of the scene and build a map at the object-level. 
An object-oriented representation of the world is a natural and efficient way for robots to make high-level decisions using task-level planners and help them to communicate with human users. 







The challenge is that many semantic aspects of the world are difficult for robots to sense directly due to the limited field-of-view and noisy observations from their onboard sensors.  Great progress has been achieved by the advances in deep neural networks for object detection <cit.> and 6D object pose estimation <cit.>. 
However, building a semantic map at the object-level remains a challenging problem, especially in densely cluttered environments such as the one shown in Fig. <ref>. Because in such dense clutter, objects are often in close contact, causing severe or complete occlusions.







One promising approach for building object-level maps is to fuse semantic measurements from different viewpoints. Robots can take advantage of their ability to move around the environment and provide continuous observations. 
More specifically, the data association between measurements and objects are first determined <cit.> and probabilistic model are built over objects and robot poses to infer the semantic map (SLAM++ <cit.>, Fusion++ <cit.>) or belief over objects (CT-MAP <cit.>). Such methods perform inference directly on the object instance instead of low-level primitives (points, surfels).  This approach to inference offers faster inference, more compact map representation, and the potential for dynamic object reasoning. We posit that 
this approach is particularly advantageous 
in dense clutter. The geometric properties of object instances and geometric relationships between objects can be used for more robust data association and accurate pose estimation.








In this work, we present GeoFusion, a SLAM-based approach for inferring object labels and 6D poses from continuous noisy measurements in dense clutter by exploring the geometric consistency of the object instances. 
 The geometric consistency is defined in two parts: geometric consistency score and geometric relation. The geometric consistency score describes the compatibility between the object geometry model and the observation point cloud. Meanwhile, it provides a reliable measure to filter out false positives in data association. The geometric relation represents the relationship (e.g. contact) between geometric features (e.g. planes) among objects. Moreover, geometric relation is directly amenable to make better decisions for high-level task planners. 

Given noisy measurements from state-of-the-art object detection <cit.> and pose estimation <cit.> systems, our method first determines the correct correspondences between measurements and objects with geometric consistency between measurements and point cloud observations. The associations are then used to build a factor graph along with the geometric relations between objects. We use the fast optimization technique to get the maximum likelihood estimation of object and robot poses. We quantitatively evaluate our method and demonstrate that is able to estimate geometrically stable scene and be robust against false estimates from state-of-art frame-by-frame estimation system <cit.> and outperforms the baseline methods that do not consider geometric consistency.








































































§ RELATED WORK












 §.§ Semantic Mapping

From the emerging of semantic mapping <cit.>, lots of work have explored this field with various semantic representations <cit.>. With the focus on the object-level semantic map, one widely used approach is to reconstruct a 3D map from either sparse features <cit.> or dense point clouds <cit.> and then augments the map with the objects. Civera et al. <cit.> and Ekvall et al. <cit.> used SURF/SIFT descriptors to register objects to the map created in a parallel thread. However, their work can not deal with objects in clutter. To deal with clutter, Li et al. <cit.> proposed an incremental segmentation approach to fuse segmentation across different frames based on dense reconstruction 3D map and used ObjRecRANSAC <cit.> to register object poses. But their method does not scale well with substantial false detections. 

Salas-Moreno et al. <cit.> proposed an object SLAM system that recognized objects using Point-Pair Features in each frame and directly performed graph optimization on object and camera 6D poses. Their work shows promising results in the direction of building maps with 6D object poses. McCormac et al. <cit.> constructed an online object-level SLAM system with previously unknown shape and perform pose graph optimization at the object level. Both of their work assume independence between objects and hence difficult to work well in densely cluttered environments. Zeng et al. <cit.> proposed CT-MAP, which considers contextual relation between objects and temporal consistency of object poses in a conditional random field and maintain belief over object classes and poses. This approach is robust to false detections; however, does not scale well to the increasing number of objects due to the nature of pure generative inference. 
















 §.§ Scene Estimation

Great progress has been made by deep neural networks in object detection <cit.>, 6D object pose estimation <cit.> and semantic segmentation <cit.>. To be robust to false detections due to the effects of overfitting, discriminative-generative algorithms <cit.> have been proposed for robust perception, especially in adversarial environments <cit.>. These efforts combine inference by deep neural networks  with sampling and probabilistic inference models to achieve robust and adaptive perception. However, the robustness is at the cost of computation time and the assumption object independence. 

Sui et al. <cit.> propose an axiomatic scene estimation method to estimate both geometric spatial relationships between objects and their 3D object poses for goal-directed manipulation. The spatial relation introduces strong constraints to reduce the search space of object poses. Desingh et al. <cit.> and Mitash et al. <cit.> leverage physics into scene estimation to search object poses based on compatibility score from physics simulation. The above-mentioned methods take either geometric and physical constraints into account for searching object poses, at the cost of computational efficiency.









































§ PROBLEM FORMULATION

Our aim is to estimate the semantic map composed of a collection 𝒪 = { o_j }_j=1^M of M static objects as well as geometric relationships ℛ = {r_ij | i, j ∈ [1, M] } between them with the assumption of known 3D object geometries. Note that the number of objects M is unknown. Each object o_j = (o_j^c, o_j^p) contains object class o_j^c ∈ [1, C] and 6D object pose o_j^p ∈ SE(3). Each r_ij describes the geometric spatial relation between geometric features of two objects (e.g. support), more of which will be discussed at Sec. <ref>.

When the robot moves around in the environment, it observes a set of semantic measurements 
 𝒵 = {{z_t^k}_k=1^N_t}_t=1^T, 
where T is the total time step robot has travelled, 
and N_t  is the number of semantic measurements at each time step. Similar to the definition of object, each semantic measurement z_t^k = (z_t^k,c, z_t^k,p) is comprised of object class and 6D pose. The robot poses represent as 𝒳 = {x_t}_t=1^T, where x_t ∈ SE(3) is also 6D pose in our case. In addition, the correspondence between objects 𝒪 and measurements 𝒵 also needs to be determined and is defined as 𝒟 = {{d_t^k}_k=1^N_t}_t=1^T where d_t^k = j stipulates that measurement z_t^k corresponds to object o_j.

A complete statement of this problem is the maximum likelihood estimation of 𝒳, 𝒪, and 𝒟 given the semantic measurements 𝒵 and the geometric relation ℛ is computed heuristically from 𝒪:


    𝒳̂, 𝒪̂,  𝒟̂ = _𝒳, 𝒪,  𝒟log p(𝒵 | 𝒳, 𝒪,  𝒟)


As the joint estimation of objects, data association and robot poses suffers from the high dimensionality, 
the most common approach is to decompose Eq. <ref> into two separate estimation problems: data association and graph optimization.

The maximum likely estimation data association 𝒟 is first computed given initial estimates of robot poses 𝒳^(0) and objects 𝒪^(0). Then given computed 𝒟̂, the most likely robot poses and objects are estimated:


    𝒟̂   = _𝒟  p(𝒟 | 𝒳^(0), 𝒪^(0), 𝒵)  
    𝒳̂, 𝒪̂   = _𝒳, 𝒪log p(𝒵 | 𝒳, 𝒪,  𝒟̂)


Again, we decompose Eq. <ref> to a two-step optimization where the object and robot poses are first optimized and the geometric relation R will be computed heuristically from O and these geometric relations will, in turn, pose constraints on factor graph for estimating geometrically stable scene: 

    𝒳̂, 𝒪̂^'   = _𝒳, 𝒪log p(𝒵 | 𝒳, 𝒪,  𝒟̂) 
    𝒪̂   = _𝒪log p(𝒵 | 𝒪, ℛ)
 




§ DATA ASSOCIATION
 


















As the number of objects M is not known as the prior knowledge, we proposed a non-parametric clustering based approach for estimating data association in Eq. <ref> based on DPmeans algorithm <cit.>, as shown in Algorithm <ref>.  Given initial estimates of objects 𝒪^(0), robot poses 𝒳^(0) and measurements 𝒵, we compute how likely each measurement belongs to current objects, being a new object or a false positive. The objects are created, updated, and deleted dynamically as the data association process moves forward. 



























  §.§.§ Association




For each measurement z^k_t in frame t, the likelihood of z_t^k being assigned to object o_j will be calculated as follows: 

    P(d_t^k=j) ∝𝕀_z^k,c_t =o_j^c  f_gc(z_t^k|o_j)   f(z^k_t|o_j, x_t),

where 𝕀_z^k,c_t =c_i^c is the label factor and it is obtained using an indicator function that evaluates to 1 if z^k,c_t =c_i^c and 0 otherwise, meaning that each measurement will only be assigned to an object with the same label. 





Second term f_gc(z_t^k | o_j) captures the geometric consistency score between measurement and the observation point cloud and provides a measure of how reliable the given measurement is. The motivation behind this term is that the confidence score given by neural networks sometimes are not accurate enough to describe the reliability of the measurement. Given a measurement z_t^k, a point cloud is back-projected from the rendered depth image and compared with observation point cloud to compute projective inlier ratio r_i, outlier ratio r_out and occlusion ratio r_occ. A point in the rendered point cloud is
first considered as an occlusion point if it is occluded by the projective point in the observation point cloud shooting from the camera ray. If not, it is considered as an inlier if the distance with the projective point is less than certain sensor resolution ϵ_res, or outlier if the distance is greater than ϵ_out. Fig. <ref> shows an example of different types of points. The ratios are computed over the total number of rendered points. And they are used as follows:


    f_gc(z_t^k | o_j) = S(r_i) S(1 - r_out)S(1 - r_occ)

where S is a modified sigmoid function providing an S-shaped logistic curve that is well-adapted to reflect the changing tendency of confidence on the given measurement over different values of each ratio.



The last term f(z^k_t|o_j, x_t) is the pose factor that reflects the similarity of measurement pose and object pose, given the current robot pose x_t. Here, we make the assumption that the likelihood of each measurement z_t^k given the robot pose and the object pose follows a multivariate Gaussian distribution.

    f(z_t^k|o_j, x_t) ∼𝒩(x_t^-1· o_j^p , Q)

where Q is the measurement noise matrix and this factor can thus be drawn from the Gaussian probability density function. 
With all the factors described above, z_t^k is assigned to be the maximum likelihood object if the object is within a certain threshold ϵ_new, otherwise, it is assigned to a new object.



The One Measurement Per Object (OMPO) constraint <cit.> is also applied in this process so that two measurements in the same frame will never be assigned to the same object and the latter will be assigned to a new object instead.





  §.§.§ False positive removal


For each object candidate created or updated in the association step, we compute its false positive score f_j as follows: 

    f_j = 1 - R_j/1+e^(-n_j)

where n_j is the number of measurements assigned to o_j and R_j is the maximum geometric consistency score among all of these measurements. 
If there are more measurements assigned to one object, it is reasonable to consider this object as more reliable among others thus it deserves a lower false-positive score. All objects with false-positive scores higher than ϵ_fp will then be deleted.





Overlapped objects merging  
 We notice that most of the false positive measurements usually overlap with other objects, especially for symmetric objects, like a cylinder-shape tomato soup can. Object-level geometric consistency is incorporated again to address this problem as we applied Separating Axis Theorem (SAT) 
 to detect the collision ratio of the three-dimensional bounding boxes of any two objects. Corner points of each bounding box are projected to several main axes and the overall collision ratio is decided based on the percentage of overlap along each axis. One object with a high collision ratio will be merged to the other one that has a lower false-positive score.

 






§ GRAPH OPTIMIZATION
 









After the associations 𝒟 between objects 𝒪 and semantic measurements 𝒵 have been determined, the most likely robot and object poses are then inferred in the graph optimization process. The factor graph is used to model robot and object states and expresses the conditional independence between them. It gained a huge success in solving SLAM problems <cit.> due existing computational tools that allow efficient optimization <cit.>. In a factor graph, there exists a set of vertices 𝒱 that represent optimization random variables and edges represent factors ℱ among a subset of random variables. A factor f represents probabilistic dependencies among the random variables and is associated with a cost function. Graphically, the vertex is a circle and the factor is a square in the factor graph, as shown in Fig. <ref>. The joint probability of the optimization random variables can be expressed as the product of factors: 


    p(𝒱) ∝∏_f ∈ℱ f(𝒱)


The optimization process in is decomposed into two stages, as shown in Fig. <ref>. First, 6D pose of the robot and the objects are optimized over odometry and semantic measurements. In the second stage, 6D pose of objects are then fine-tuned over the computed geometric relations to produce a geometric consistent scene estimate.



 §.§ Stage I Optimization

The right hand side of Eq. <ref> is rewritten as:


    log p(𝒵 | 𝒳, 𝒪,  𝒟̂) 
         = ∑_t = 1^T ϕ (T_t^t-1 ; x_t - 1, x_t)  + ∑_t = 1^T ∑_k = 1^N^tϕ (z_t^k, p ; x_t, o_d_t^k^p)

where ϕ (T_t^t-1 ; x_t - 1, x_t) is the odometry factor and ϕ (z_t^k, p ; x_t, o_d_t^k^p) is the object measurement factor. T_t^t-1 is the odometry measurement between robot pose x_t-1 and x_t. z_t^k,p is the semantic measurement of an object between the robot pose x_t and the object pose o_d_t^k^p. With the standard assumption of additive Gaussian noise, each factor follows a quadratic form:


    ϕ (T_t^t-1 ; x_t - 1, x_t)    = - 1/2 (x_t ⊖ x_t-1 - T_t^t-1) Q^-1 (x_t ⊖ x_t-1 - T_t^t-1) 
    ϕ (z_t^k, p ; x_t, o_d_t^k^p)    = - 1/2 (x_t ⊖ o_d_t^k^p - z_t^k, p) R^-1 (x_t ⊖ o_d_t^k^p - z_t^k, p)

where ⊖ represents the operator that computes the relative transformation. Q and R is the corresponding covariance noise matrix. Thus, The maximum likelihood estimation of X and O can be written as the nonlinear least-squares problem:


    𝒳̂, 𝒪̂^'   = 
         _𝒳, 𝒪∑_t=1^T  x_t-1⊖ x_t - T_t^t-1_Σ_Q
          + ∑_t=1^T ∑_k-1^N_t x_t ⊖ o_d_t^k^p - z_t^k, p_Σ_R
 X _Ω = √(X^T Ω X)
where Σ_Q and Σ_R is the information matrix. Note that the as the metrics for translations and rotations are different, the information matrix for them is also different. We use Σ_p for translations and Σ_q for rotations which represent as quaternions. Σ_p and Σ_q are diagonal matrices and represent as Diag(ω_p, ω_p, ω_p) and Diag(ω_q, 0, 0, 0). For symmetric objects in the object measurement factor, we design a separate Sigma_q for the rotation. For example, object with rotation axis in z-axis is associated with Σ_q = Diag(0, ω_q,ω_q, 0) to exclude the influence from z-axis. 








 
 






































 §.§ Object Geometric Relationship Inference
 

In stage I optimization, the maximum likelihood estimation of robot and object poses is obtained by minimizing the odometry and object measurement cost. However, the resulting object poses are still noisy and not accurate enough for applications like precise robot manipulation. The noisy object poses lead to geometrically inconsistent scene estimate, e.g., floating or intersection between objects. To get a highly accurate and geometrically consistent scene estimate, we posit that if the geometric relationship between objects can be inferred and these relations can serve as the constraints (factors) between objects in the factor graph model, as shown in the right of Fig. <ref>. Given estimated object poses 𝒪 from Eq. <ref>, ℛ is heuristically computed.  



  §.§.§ Geometric Surface Feature
 
For common rigid household objects, we only consider contact relations between surfaces for generality in representing the geometry of the object and efficiency in the graph optimization process. In this paper, geometric surface features are divided into two classes: plane surface feature and curved surface feature. An illustration of surface features is shown in Fig <ref>.

Given an arbitrary convex polyhedron, geometric surface features can be extracted. For example, there are six plane surfaces and twelve curved surfaces for a cube-like geometry as it has six planes and twelve edges. The edge is treated as a zero radius curve surface. For cylinder-like geometry, there are two plane surfaces and one curved surface. Although the two classes of surfaces cannot represent all of the objects (e.g., articulated or non-rigid objects), they are general and sufficient for common rigid household objects.


















Plane Surface Feature
The plane surface feature refers to those planes on polyhedrons. There are three attributes associated with plane surface feature: a center point C_p ∈ R^3, a set of points B_p = {b^i_p}_i=1^N_b with b^i_p ∈ R^3, and the normal direction N_p ∈ R^3. C_p describes the 3D position of the plane center. Each b^i_p is the 3D position of a boundary point on the surface and N_b is the number of boundary points.  




Curved Surface Feature
The curved surface feature describes curved surfaces in 3D shapes like cylinders. Noticeably, edges of polyhedrons are also regarded as a curved surface feature with a radius equal to zero. Five attributes are associated with the curved surface feature: C_c ∈ R^3 and N_c ∈ R^3, determining the 3D position of center and direction of the rotation axis of the curved surface. B_c = {b_c^i}_i=1^2 with b_c^i ∈ R^3 refers to the 3D position of boundary points in rotation axis. Radius r describes the radius of the curved surface.


Accordingly, we define three types of spatial relations between these two surfaces: Plane2Plane Contact (P2P), Plane2Curve Contact (P2C), and Curve2Curve Contact (C2C). Fig <ref> illustrates these geometric relations.






  §.§.§ Geometric Feature Relationship Inference

P2P contact
If two different plane surface features P_u, P_v from two objects o_i, o_j are contacting each other, their attributes should follow the following rules:

    [              |T_o_iN_P_u^i · T_o_jN_P_v^j + 1| < ϵ_n^pp; |T_o_iN_P_u^i · (T_o_jC_P_v^j - T_o_iC_P_u^i)| < ϵ_c^pp ]

where ϵ_n^pp, ϵ_c^pp are the direction threshold and distance threshold for P2P contact. 




P2C contact
If a plane surface feature P_u and a curved surface feature C_v are contacting each other, their attributes should follow the following rules:

    [                       |T_o_iN_P_u^i ·  T_o_jN_C_v^j| < ϵ_n^pc; |T_o_iN_P_u^i · (T_o_jC_C_v^j - T_o_iC_P_u^i) - r_j| < ϵ_c^pc ]

where ϵ_n^pc, ϵ_c^pc are the direction threshold and distance threshold for P2C contact.


C2C contact
If two different curved surface features C_u, C_v are contacting each other, their attributes should follow the following rules:

    |(T_o_iN_C_u^i × T_o_jN_C_v^j) · (T_o_jC_C_v^j - T_o_iC_C_u^i) - (r_i + r_j)| < ϵ_c^cc

where ϵ_c^cc is the distance threshold for C2C contact. 









  §.§.§ Physical Relationship Check

We also add two physical based check in complement to the above geometric methods to ensure eliminating false detected contact. For example, if two cubes are placed parallel and close to each other on the table, a P2P contact can be detected. However, there is a chance that those objects are just near to each other but not contacting each other. 







Support direction check
If a plane feature can support another plane or be supported, its normal direction can not be horizontal to the gravity direction. Assume the direction of gravity is N_G, the support direction check of plane feature i is defined as:

    N_G · N_p^i > ϵ_G

where ϵ_G is the threshold for support direction check.

Here we make an implicit assumption that there is at least one grounded object, e.g. table, which supports all the other objects. Similar assumption can be found in SLAM++ <cit.>.

Qualitative Support projection check
If there is a support relationship between two features, their 2D projections along the direction of gravity must be overlapped. For those contact feature candidates, their 2D projections are the polygon or the edge projected from their boundary points. Separate axis theorem (SAT) is applied to check the overlapping of those 2D contours. Those feature candidates whose 2D projections are not overlapped will be removed.




 §.§ Stage II Optimization

After spatial relations ℛ between objects have been computed from the estimated object poses in stage I optimization, these relations are used as constraints in the factor graph optimization. These constraints will enforce proper contact between objects and fix the issues of floating and interpenetration. We can rewrite the right hand side of Eq. <ref> as:


    log p(𝒵 | 𝒪, ℛ) = ∑_i=1^M ∑_j=1^M ϕ (r_ij ; o_i^p, o_j^p) ∑_i=1^M ϕ(o_i^' ; o_i)

where ϕ (r_ij ; o_i^p, o_j^p) is the geometric relation factor and ϕ(o_i^' ; o_i) is the object prior factor. The object prior factor comes from the estimated object poses in stage I optimization. The geometric relation factor encodes the contact constraint between two objects computed from the previous section. Accordingly, there are three constraints: P2P, P2C, and C2C. For P2P constraint, the cost function is defined as: 

    E_P2P =  ω_q |T_o_iN_P_u^i · T_o_jN_P_v^j + 1|  +  ω_p
    |T_o_iN_P_u^i · (T_o_jC_P_v^j - T_o_iC_P_u^i)|

Similarly, the cost function of P2C constraint is defined as:

    E_P2C =  ω_q | T_o_iN_u^o_i· T_o_jN_v^o_j | 
        + ω_p |T_o_iN_u^o_i· (T_o_iC_u^o_i - T_o_jC_v^o_j)|

and the cost function of C2C is defined as:

    E_p2c = ω_p |(T_o_iN_u^o_i× T_o_jN_v^o_j) · (T_o_iC_u^o_i - T_o_jC_v^o_j) 
        - (r_v + r_u)|













    





































§ EXPERIMENTS
































































 §.§ Implementation

We use Mask R-CNN <cit.> and DenseFusion <cit.> to generate semantic measurements where the former detects objects along with the instance segmentation which the latter takes in to estimate the 6D pose. We finetune the Mask R-CNN with the YCB-Video Dataset <cit.> with the pretrained Microsoft coco model <cit.>. We use the public available DenseFusion weights and implementation without fine-tuning. The front-end in our implementation selects every 10th camera frame as a keyframe. The camera visual odometry is provided by ORB-SLAM2 <cit.>. We use Ceres <cit.> as the optimization backend. In our implementation, we perform stage I optimization every 10th keyframe and perform stage II optimization every keyframe.







 §.§ Dataset and Baseline

To test the performance of our method, we collect a testing dataset [ <http://www-personal.umich.edu/ zsui/geofusion_dataset.html> ] from six RGBD video streams in which our Michigan Progress Robot moves around the table. In each scene, we put around 18 objects in dense clutter on the table and collect around 200 keyframe RGB-D observations from the robot's sensor. 
The groundtruth object classes and poses are labelled using LabelFusion <cit.>. For baseline methods, we compare our method with frame-by-frame (FbF) estimation from Mask R-CNN and DenseFusion. It only considers the single frame and does not take any temporal or spatial constraints into account.  We also compare with the variations of our proposed GeoFusion: 1) a baseline SLAM (B-SLAM) method without considering geometric consistency in both data association and graph optimization. The confidence score from DenseFusion instead of the geometric consistent score is used in computing data association. The back-end graph optimization only optimizes over odometry and object poses without considering spatial constraints between objects.  2) a robust front-end (R-Front) method where only our data association is used and not the graph optimization.









 §.§ Evaluation
 


  §.§.§ Object Detection

We first compare object detection performance with baseline methods on each keyframe collected in the dataset. 

mAP
We use the common object detection metric: mean average precision (mAP), to evaluate object detection. Average precision (AP) is the area under the Precision-Recall curve of an object class and mean average precision is the mean of all the AP. The subscript under mAP is the ratio of intersection over union (IoU) between the estimated bounding box and the ground truth bounding box. 
We also use mAP@[50:95] which corresponds to the average AP for IOU from 0.5 to 0.95 with a step size of 0.05.  

Results are shown in Table <ref>. significantly outperforms the frame-by-frame method in all mAP metrics indicating that our method is capable of identifying false detections and keeping true ones from noisy semantic measurements. Robust front-end method achieves the similar mAP_50 with however underperforms in mAP_75 and mAP_50:95. It shows that the back-end optimization improves the 6D poses of objects, which in turn increases the mAP with a tighter threshold in IOU. The baseline SLAM method does not take any geometric consistency into account and the performance is even worse than the frame-by-frame method. This is because the false and noisy measurements accumulate in the baseline slam and without the help of geometric consistency, they are difficult to be eliminated from the estimates. 



Precision and Recall
For a more intuitive evaluation on object detection, we also report numbers on precision (Pr) and recall (Rec) with the confidence score threshold set to 0.5. As shown in Table <ref>, outperforms all the baseline methods under different mAP metrics. With the ratio of IOU increases for checking true positives, the performance gap between and baseline methods becomes larger. Interesting, the recall of the Frame-by-frame method under mAP_50is higher than GeoFusion. This is because has a strict way to filter out false and noisy detections. In other words, detections are chosen very conservatively by which leads to more false negatives in a looser metric. 














  §.§.§ Pose Accuracy
 For 6D object pose accuracy, we use the ADD-S metric <cit.> to compute the average point distance between estimated pose and the correct pose. Only true positives are computed at each frame for each method. As our aim is to build a high accuracy object-level map and the precision required by the robot to grasp objects are relatively high, we plot the accuracy-threshold curves within the range of [0.00m, 0.02m] as shown in Fig. <ref>. achieves the highest pose accuracy among all the methods and about 90% percent of the objects have achieved pose error under 1cm. The pose accuracy of baseline SLAM method is even worse than the Frame-by-Frame method indicating that the confidence scores from perception module are sometimes not reliable. 


















  §.§.§ Run Time



We show an analysis of run time in Fig. <ref> on each frame for data association and optimization of an example scene in Fig. <ref>. Note that our method only runs on CPU (without measurements generation), the average time for each frame is under 200 ms





  §.§.§ Geometric Relation
 Fig. <ref> compares the results of two optimization stage of the scene in Fig. <ref>. Left figure is the result of optimization stage I without geometric relation constraints and the right figure is the result after optimized over geometric constraints. The geometric constraints pulled objects to satisfy the constraints. For example, the objects on the right are more aligned with the table and the intersection between objects is fixed. 






§ CONCLUSION

In this work, we present GeoFusion, a SLAM-based scene understanding method for building a semantic map at object-level in dense clutter while taking into account geometric consistency. Our method is able to infer object labels and 6D poses from noisy semantic measurements robustly and efficiently. The reasoning at object-level with geometry offers a fast and reliable way to filter out false positives and constraint the object through geometric relation. The computed geometric relations are also directly amenable to high-level task planners for robots to reason over actions for goal-directed manipulation tasks. 





                                
                                
                                
                                
                                





















abbrv






































