Simulation-Ready Cluttered Scene Estimation via Physics-aware Joint Shape and Pose Optimization

Abstract: Estimating simulation-ready scenes from real-world observations is crucial for downstream planning and policy learning tasks. Regretfully, existing methods struggle in cluttered environments, often exhibiting prohibitive computational cost, poor robustness, and restricted generality when scaling to multiple interacting objects. We propose a unified optimization-based formulation for real-to-sim scene estimation that jointly recovers the shapes and poses of multiple rigid objects under physical constraints. Our method is built on two key technical innovations. First, we leverage the recently introduced shape-differentiable contact model, whose global differentiability permits joint optimization over object geometry and pose while modeling inter-object contacts. Second, we exploit the structured sparsity of the augmented Lagrangian Hessian to derive an efficient linear system solver whose computational cost scales favorably with scene complexity. Building on this formulation, we develop an end-to-end real-to-sim scene estimation pipeline that integrates learning-based object initialization, physics-constrained joint shape-pose optimization, and differentiable texture refinement. Experiments on cluttered scenes with up to 5 objects and 22 convex hulls demonstrate that our approach robustly reconstructs physically valid, simulation-ready object shapes and poses.

• The paper introduces the first practical optimizer to jointly estimate object shapes and poses in multi-object contact scenes using quasistatic force-balance constraints and shape-differentiable separating-plane contacts.
• The structure-aware Woodbury and Schur-complement solver achieves up to 8.71× speedup over direct LU factorization on scenes with roughly 20,000 parameters, while reducing computational bottlenecks in frictional optimization.
• The method produces MuJoCo-stable reconstructions with kinetic-energy gains of about 10⁻⁴–10⁻² J and drift below 3.1 cm, compared with baseline failures reaching 172.55 cm, but requires 46–540 minutes of CPU optimization.

This paper presents an optimization-based real-to-sim pipeline that reconstructs physically consistent, simulation-ready scenes from a single RGBD observation of cluttered tabletop environments. The central contribution is a joint optimization over object shapes and poses under quasistatic force-balance constraints, enabled by the shape-differentiable separating-plane contact model (SDRS) and a structure-aware linear solver that exploits sparsity in the augmented Lagrangian Hessian. Unlike prior physics-aware estimators that assume known geometry or restrict optimization to poses, this work demonstrates, to the authors' knowledge, the first practical numerical optimizer operating directly in the coupled shape-pose space for multi-object contact scenes.

Motivation and positioning

Existing scene estimation approaches fall short of simulation-ready output. Learning-based pose and shape estimators such as PoseCNN and FoundationPose are purely perception-driven and routinely produce inter-penetrating, floating configurations. Physics-aware numerical methods such as SIP-CC enforce non-penetration and force equilibrium explicitly, but assume known object geometries, optimize only poses, and depend on brittle heuristic contact-selection oracles; monolithic NLP formulations over contact forces and Lagrange multipliers scale poorly to cluttered scenes. The authors identify structure-unawareness as the root cause: by treating all decision variables uniformly, prior formulations fail to exploit the algebraic structure of physics-constrained estimation. Recent differentiable simulators largely target dynamic trajectories rather than quasistatic resting configurations and suffer from non-smooth contact and visibility events. The proposed method addresses these gaps by eliminating explicit normal-force variables entirely and considering all potential hull-pair contacts without heuristic selection.

Problem formulation and pipeline

The problem is posed as an equality-constrained nonlinear program $\min_{q,x} O(q,x)$ subject to $C(q,x)=0$, where $q$ concatenates per-body rotations (Rodrigues parameterization) and translations, and $x$ concatenates all convex-hull vertices, with each rigid body represented as a union of $M$ convex hulls produced by CoACD decomposition (at most 5 hulls, 50 vertices each). The objective $O$ is a Hausdorff-style sum of three ICP-type terms: distances from hull vertices to the SAM3D-generated reference mesh (Type I), from segmented point-cloud points to the union-of-hulls surface (Type II), and from mesh vertices to the hull surface as a shape prior (Type III), weighted so that the direct point-cloud observation dominates. Because closest-point updates against a deformable union of hulls can increase term values—breaking trimmed-ICP convergence guarantees—the method applies a selective deletion heuristic: after each ALM subproblem solve, terms whose reference update increases the objective are removed in decreasing order of increment until monotonicity is restored. This is acknowledged as a heuristic rather than a convergence proof.

The full pipeline begins with SAM2 segmentation, SAM3D mesh generation, FoundationPose pose registration, SDF-based shrinkage to obtain penetration-free initialization (required by the interior-point contact model), CoACD convex decomposition, ALM-based joint shape-pose optimization, and optional differentiable-rasterization texture refinement via nvdiffrast.

Physics constraints via shape-differentiable contacts

For the frictionless case, the constraint is $C = \nabla_q \Psi$ where $\Psi$ combines gravitational potential (mass concentrated at hull vertices, admitting density optimization) and a collision potential built on separating planes between every pair of convex hulls. Following IPC-style primal interior-point barrier functions generalized by SDRS, the separating-plane variables are strictly convexly minimized out implicitly, yielding a globally twice-differentiable potential by the implicit function theorem. A globally supported activation function preserves nonzero gradients at distance, aiding LICQ satisfaction. The resulting Property 1 states: when $C=0$, all objects are force and torque balanced with Newton's third law satisfied—an equilibrium condition rather than merely collision avoidance.

Friction is handled by introducing per-vertex tangential forces as additional variables, with normal forces given analytically as derivatives of the collision potential, friction-cone inequalities, and zero-mass separating planes serving as fictitious bodies whose in-plane force and torque balance is enforced. A stated limitation of this model is that torque balance holds only for components perpendicular to the separating-plane normal; residual violation along the normal axis is controlled by the complementarity gap $\mu = 5\times10^{-5}$, which the authors report is negligible in practice but not eliminated.

Structure-aware solver

The frictional formulation inflates the variable space considerably, making the LM inner solve the bottleneck. The key structural observations are: (1) the constraint Jacobian $C(q,x)=0$0 has rank at most $C(q,x)=0$1, permitting Woodbury reduction; (2) after factoring out $C(q,x)=0$2, the remaining matrix $C(q,x)=0$3 decouples across hull pairs because different contact pairs interact only through $C(q,x)=0$4, enabling a Schur-complement block elimination. Measured on their benchmarks, this structured solve achieves speedups up to 8.71× over direct LU factorization on the largest scenario (~20k parameters). The dominant overall cost is instead repeated evaluation of the constraint and its Jacobian, which requires nested separating-plane optimizations—a cost the paper leaves unaddressed beyond noting CPU-only execution and proposing GPU acceleration.

Evaluation

Experiments cover five cluttered tabletop scenes with up to 5 rigid objects and 22 convex hulls (up to ~20k optimization parameters), plus five additional YCB-V examples. The primary metric is simulator readiness: forwarding reconstructions into MuJoCo, the estimated scenes remain near equilibrium for one minute of simulation, with max kinetic energy gains of order $C(q,x)=0$5–$C(q,x)=0$6 J and drift distances below ~3.1 cm. The SAM3D+FoundationPose initialization always contains severe penetrations, blowing up the simulator with kinetic energy of several joules and drifts up to 172.55 cm. Three recent single-view reconstruction baselines (Gen3DSR, SceneComplete, MIDI) similarly produce penetrating meshes that immediately destabilize MuJoCo. Perceptual fidelity is preserved: PSNR against the ground-truth RGB view is comparable to or better than the initialization baseline (best on 3 of 5 scenarios), indicating physical consistency is gained without visual degradation. Convergence is rapid at the outer level—6–9 ALM iterations—but wall-clock times are substantial, ranging from ~46 minutes (smallest scene) to ~540 minutes for the largest, all on CPU.

Limitations and open questions

The authors concede two principal limitations. First, computational cost: shape variables extend the decision space enough that even the structured solver yields runtimes of tens to hundreds of minutes, currently prohibitive for interactive use; GPU implementation is proposed but not demonstrated here. Second, initialization quality: SAM3D meshes can be inaccurate under severe occlusion, and the pipeline depends on a full mesh-based initial guess, user-provided bounding boxes for segmentation, manually assigned masses (uniform density of 800 kg/m³ as a starting guess), and an assumed gravitational direction aligned with the fitted table normal. Whether image-guided end-to-end optimization can replace the mesh-prior dependency, whether the selective-term-deletion heuristic admits formal convergence guarantees, and how the method scales beyond five objects remain open.

Conclusion

The paper contributes a structure-aware, globally differentiable formulation for joint shape-and-pose estimation under quasistatic contact constraints, together with Woodbury/Schur solvers that make the enlarged variable space tractable. Empirically, it is the only evaluated method producing reconstructions that survive forward simulation without blow-up while matching perceptual fidelity of perception-only pipelines, at the cost of long CPU-bound optimization times and reliance on strong learned initialization.