Point cloud registration, which seeks the best 3D rigid transformation, i.e.,
rotation and translation, to align two point clouds, is a fundamental problem in robotics and computer vision and finds applications in motion estimation and 3D reconstructionHenry12ijrr-rgbdMapping ; Blais95pami-registration ; Choi15cvpr-robustReconstruction ; Zhang15icra-vloam ; Yang20arxiv-teaser , object recognition and localization Drost10cvpr ; Wong17iros-segicp ; Zeng17icra-amazonChallenge ; Marion18icra-labelFusion ; Yang19rss-teaser , panorama stitching Bazin14eccv-robustRelRot ; Yang19iccv-QUASAR , and medical imaging Audette00mia-surveyMedical ; Tam13tvcg-registrationSurvey .
We revisit the simplest setup where point-to-point correspondences are known and correct. Formally, let and , be two point clouds with correspondences (e.g., obtained from hand-crafted Rusu09icra-fast3Dkeypoints
or deep-learned feature matchingchoy19iccv-FCGF
), then it is well known that, if the noise follows isotropic Gaussian distribution, themaximum likelihood estimator (MLE) for the rotation 111, where
is the identity matrix of size. and translation is given by:
is the standard deviation of the Gaussian noise associated with the-th correspondence. Despite the nonconvexity of problem (1) ( is a nonconvex set), its globally optimal solution can be computed in closed form due to Horn Horn87josa and Arun et al. Arun87pami
, by first computing the optimal rotation using singular value decomposition (SVD) and then computing the translation analytically. Although problem (1) requires all correspondences to be correct, it is the most fundamental building block for robust estimation frameworks such as RANSAC Fischler81 , M-estimation Zhou16eccv-fastGlobalRegistration and graduated non-convexity Yang20ral-GNC
in the presence of outlier correspondences. In addition, when no correspondences are given, theIterative Closest Point (ICP) algorithm alternates in finding correspondences and updating the transformation from the solution of problem (1).
Recently, physics-based registration Golyanik16CVPR-gravitationalRegistration ; Jauer18PAMI-physicsBasedRegistration ; Golyanik19ICCV-acceleratedGravitational has become a popular alternative to optimization-based registration. By creating virtual forces such as gravitational force Golyanik16CVPR-gravitationalRegistration ; Golyanik19ICCV-acceleratedGravitational and electrostatic force Jauer18PAMI-physicsBasedRegistration between two point clouds, one of which is static and the other is dynamically moving (rotating and translating) under the virtual forces as a rigid body of particles, point cloud registration can be solved from simulating the differential equations derived from rigid body dynamics. The appealing advantage of solving registration using simulation lies in its computational efficiency: the simulation of dynamics can be efficiently performed using GPUs with rich literature on implementation and acceleration Golyanik19ICCV-acceleratedGravitational . Nevertheless, little has been analyzed about the dynamics of the moving point cloud under virtual forces, and the convergence properties of simulation-based methods are unknown. In fact, simulation-based techniques are also prone to getting stuck at local minima and hence good initialization is required Jauer18PAMI-physicsBasedRegistration .
Contributions. In this paper, we focus on providing a theoretical understanding of the dynamical system arising from point cloud registration and its connection to optimization-based registration. Our first contribution is, instead of creating virtual gravitational and electrostatic forces as in previous works Golyanik16CVPR-gravitationalRegistration ; Jauer18PAMI-physicsBasedRegistration , we place virtual springs between each pair of corresponding points with the spring coefficient proportional to the inverse of the square of the standard deviation of the noise (i.e., in (1)). Under this construction, the potential energy of the dynamical system exactly recovers the objective function of problem (1). Our second contribution is to leverage Lyaponov theory, in particular the invariant set theorem Slotine91book-appliedNonlinearControl , to analyze the stability of this system. We show that the system, starting from any initial conditions, will tend to the set of equilibrium points, which must contain the globally optimal solution of problem (1). This result is not enough to guarantee that simulating the dynamics will converge to the globally optimal solution. Therefore, our third contribution is to conjecture and numerically show that, besides the equilibrium point corresponding to the optimal solution of problem (1), the system contains either three or infinite “spurious” equilibrium points, and all spurious equilibria are locally unstable. When the point cloud has generic shape and noise distribution, the dynamical system has three spurious equilibria, while the case of infinite spurious equilibria happens when the point cloud exhibits symmetry. The conjecture suggests that a small perturbation is sufficient for the system to escape these spurious “suboptimal” equilibria. In fact, our simulation of the dynamics demonstrates that, even without perturbation, the system always converges to the globally optimal solution of problem (1), supporting our conjecture. Moreover, numerical experiments suggest that the rotations corresponding to the three locally unstable equilibrium points are related to the globally optimal rotation via an additional rotation. Although our analysis only focuses on point cloud registration in its simplest form (1), this is the first theoretical understanding of physics-based registration and we hope future work can be built upon this.
The rest of the paper is organized as follows. Section 2 describes the construction of a dynamical system for point cloud registration and the expression for the rigid body dynamics. Section 3 presents the stability analysis of the dynamical system using Lyapunov theory. Section 4 provides numerical experiments supporting our analysis. Section 5 concludes the paper. Due to the focus of this paper, we omit a section dedicated to related work, and point the interested readers to Yang20arxiv-teaser ; Jauer18PAMI-physicsBasedRegistration for thorough reviews of related work on point cloud registration.
2 Dynamical Construction for Point Cloud Registration
We consider each point cloud and as a collection of particles each with mass (red and blue solid circles in Fig. 1), and the particles are mutually rigidly connected via massless links (red and blue lines in Fig. 1). We are given the coordinates of the particles at (with slight abuse of notation, we also use and to refer to the particles). The center of mass (CM) of is located at , and the CM of is located at , where is the total mass of (also the total mass of ). Without loss of generality, we assume and is already centered at zero.222Otherwise, we can shift every point by : . As shown in Fig. 1, we then attach a fixed global coordinate frame to the CM of , and attach a moving coordinate frame to the CM of . Point cloud is static for all and point cloud is allowed to move.
Virtual springs and damping. We place a virtual spring with coefficient between each pair of corresponding particles and , and we assume that the particles are subject to viscous damping with constant coefficient . Under spring forces and viscous damping, will undergo rigid body dynamics and moves in the virtual environment.
State space of the dynamical system. We use the following states to describe the dynamical system (blue symbols in Fig. 1): (i) , the position of the CM of in the coordinate frame ; (ii) , the relative orientation between frame and frame ; (iii) , the velocity of the CM of in frame ; (iv) , the angular velocity of w.r.t. its CM in its body frame . In order to describe the position and velocity of each particle , we use to denote the relative position of each particle w.r.t. the CM in frame , which is constant for all by definition. Then the position of each particle in the frame can be expressed as:
With this construction, we consider the following initial condition for the dynamical system:
which means that point cloud is at rest position and frame is axis-aligned with frame (with an offset in the location of the CM).
Forces and torques. We now derive the total force and torque applied on induced by the virtual springs and damping. The force acting on each point , expressed in frame , is:
where we have assumed the damping force is also proportional to the mass of the particle.333Under the assumption that the particles have equal density, then larger mass implies larger volume, and hence larger damping force. Hence the total force is:
where we have used:
due to the definition of . From the expression of in eq. (4), we can find the expression of :
Hence, we can compute the total torque:
where we have used:
to the following skew-symmetric matrix:
and we have the equality that for any .
Dynamics of the system. Now we are ready to introduce the dynamics of the system.
Proposition 1 (Dynamics of Point Cloud Registration).
Using as the state space of , the differential equations that describe the dynamics of are:
where is the linear acceleration of in frame , is the angular acceleration of in frame , is the inertia matrix of w.r.t. its CM, and are the total force and torque in (5) and (8). Further, let , and use to denote the vector of states, we also write
to denote the vector of states, we also writeto denote the nonlinear dynamics in (11).
3 Stability Analysis
In this section, we apply Lyapunov theory, in particular the invariant set theorem, to analyze the stability of the nonlinear dynamics in (11). We first introduce some preliminaries.
Definition 2 (Invariant Set Slotine91book-appliedNonlinearControl ).
A set is an invariant set for a dynamic system if every system trajectory which starts from a point in remains in for all future time.
A first trivial example for an invariant set is the entire state space, and a second trivial example for an invariant set is an equilibrium point, i.e., a point at which . The following theorem establishes when the dynamical system will converge to an invariant set.
Theorem 3 (Global Invariant Set Theorem Slotine91book-appliedNonlinearControl ).
Consider the dynamical system , with continuous, and let be a scalar function with continuous first partial derivatives. Assume that (i) as ; (ii) over the entire state space. Let be the set of all points where , and be the largest invariant set in . Then all trajectories of the system globally asymptotically converge to as .
The crux of Theorem 3 is to find a scalar function , often referred to as the Lyapunov function or the energy function, and the set defines an interesting set. Interestingly, by choosing to be the total energy of the system, we obtain the following result.
Proposition 4 (Convergence to Equilibria).
Let be the total energy of the dynamical system described in (11), where is the total kinetic energy and is the total potential energy. Then (i) exactly recovers the objective function of problem (1); (ii) all trajectories of the system globally asymptotically converge to the set of equilibrium points , where the total force and torque both vanish, and the origin of aligns with the origin of , i.e., .
The total kinetic energy of the system is:
where the first term is the translational energy and the second term is the rotational energy w.r.t. its CM. The total potential energy of the system comes purely from spring forces:
which apparently satisfies as .555Because there are at least three noncollinear measurements ’s, there are at least three nonparallel ’s. Hence, as , at least one of the will go to infinity. We then compute the derivative of , using the dynamic equations (11). The derivative of the kinetic energy is:
and the derivative of the potential energy is:
and therefore the derivative of the total energy is:
We remark that one can obtain in (19) directly from physics: is the energy dissipation rate, and the only energy dissipation of this system comes from viscous damping. From the expression of , we know the set is precisely the set because if and only if both velocities and vanish. Now consider the invariant set within : because both and , for any point to stay inside , the translational and rotational accelerations must be zero, i.e., , . In other words, the largest invariant set in is precisely the set of equilibrium points , and from the global invariant set theorem 3, we know that the system tends towards from any initial state. Eventually, from the dynamics (11), we know that any point in satisfies:
where the total force and torque applied on are both zero. From the second equation in (20), we know that the origin of aligns with the origin of . ∎
An immediate corollary states that the unique globally optimal minimizer of the potential energy written in (13) (and hence, the globally optimal solution to problem (1)) lies inside the set of equilibrium points.
Corollary 5 (Global Optimizer).
If the measurements contain at least three noncollinear points ’s, then the potential energy in (13) admits a unique global minimizer . Let , then is contained in the set of equilibrium points .
From Horn87josa , we know that the potential energy admits a unique closed-form global optimizer when there are at least three noncollinear measurements. Since is the unique minimizer of the kinetic energy in (12), it follows that is the unique global minimizer of the total energy . Clearly and . Therefore must hold. Otherwise, suppose , and becomes at the next time step. From , we have , contradicting the fact that is the unique global minimizer of . ∎
Proposition 4 and Corollary 5 together imply that the dynamical system (11) will globally asymptotically tend to the set of equilibrium points, which contains the unique global minimizer of the potential energy as a specific solution. However, this result is insufficient to conclude that simulating the dynamics (11) can solve the optimization problem (1), because there could exist many equilibrium points. The next conjecture states that the system does have spurious equilibrium points other than the global minimizer in Corollary 5, but all spurious equilibrium points are locally unstable.
Conjecture 6 (Characterization of Equilibria).
Besides the equilibrium point corresponding to the global minimizer of the energy function as stated in Corollary 5, the dynamical system in (11) has either three or infinite spurious equilibrium points. Moreover,
when the point cloud has generic shape, i.e., all points are randomly generated, the system has three spurious equilibria, and the rotations at all three equilibria differ from the globally optimal rotation by ;
when the point cloud exhibits symmetry, the system has infinite spurious equilibria;
all spurious equilibria are locally unstable.
We provide a graphical proof for b in the case of and , as shown in Fig. 2. When (Fig. 2(a)), consider both (blue) and (red) are equilateral triangles with being the length from the vertex to the center. Assume the particles have equal masses such that the CM is also the geometric center , and all virtual springs have equal coefficients. is obtained from by first rotating counter-clockwise (CCW) around with angle , and then flipped about the line that goes through point 1 and the middle point between point 2 and 3. We will show that this is an equilibrium point of the dynamical system for any . When the CM of and the CM of aligns, we know the forces are already balanced. It remains to show that the torques are also balanced for any . and applies clockwise (CW, cyan) and the value of their sum is:
and applies CCW (green) and its value is:
Therefore, the torques cancel with each other and the configuration in Fig. 2(a) is an equilibrium state for all . However, it is easy to observe that this type of equilibrium is unstable because any perturbation that drives point 2 out of the 2D plane will immediately drives the system out of this type of equilibrium. When , one can verify that same torque cancellation happens:
and the system also has infinite locally unstable equilibria. ∎
), and compute the eigenvalues of the Jacobianat each of the solutions. From Lyapunov’s linearization method Slotine91book-appliedNonlinearControl , if the Jacobian has at least one eigenvalue strictly in the right-half complex plane, then it serves as a certificate for local instability of the equilibrium point. In Section 4, we show that solving the system of polynomial equations (20) always yields four solutions, and the Jacobian at three of the spurious solutions always has eigenvalues with positive real parts.
If Conjecture 6
were true, then simulating the dynamics has high probability of converging to the globally optimal solution. In addition, if the simulation does converge to a locally unstable equilibrium, then computing the Jacobian can inform this unexpected convergence and adding a small perturbation can help the simulation escape this locally unstable equilibrium until it reaches the true global optimizer. In fact, in Section4, all the experiments converge to the global optimizer.
4 Numerical Experiments
4.1 Characterization of Equilibria
We first run numerical experiments to justify Conjecture 6. At each Monte Carlo run, we first randomly generate a point cloud , where each point follows an isotropic Gaussian distribution with standard deviation 1, i.e., . Then we randomly samples a rotation matrix and a translation vector , and obtain the point cloud by: , where models the Gaussian noise with . Then we shift both and by so that is centered at the origin, so that the ground-truth transformation is now: . We then use Matlab to solve the second equation in (20): , together with the constraint that , which boils down to a set of quadratic polynomial equalities in the entries of (see e.g., Yang20cvpr-shapeStar for the expressions). This gives us the set of equilibrium points . At each Monte Carlo run, we also obtain the closed-form solution using the method from Horn87josa .
We run 50 Monte Carlo runs, and we find that, (i) the set always contains four equilibrium points; (ii) the Jacobian at the four equilibrium points has zero, one, two and three eigenvalues strictly on the right-half complex plane, respectively. Fig. 3 provides the superimposed scatter plots of the eigenvalues on the complex plane. This clearly shows that there are three equilibrium points that are locally unstable; (iii) moreover, we compute the angular error between the rotation estimations at the four equilibrium points, denoted , and the closed-form rotation , which is . We find that the three locally unstable equilibrium points yield rotation errors that are always , and the equilibrium point that has no eigenvalues with positive real parts always produces exactly , i.e., rotation errors of . These observations strongly support Conjecture 6. In addition, it suggests that the set of equilibrium points contains the globally optimal solution and three other solutions that are generated from by an additional rotation of around some axis, and these solutions preserve torque balance.
4.2 Simulation of the Dynamics
We then simulate the dynamics in (11) with constant time step size s to solve 100 random instances of point cloud registration generated by the same procedure as in Section 4.1. At each time step, we project the matrix to to make sure it is a valid rotation matrix. We stop the simulation when . Fig. 4(a) shows the statistics of rotation errors over 100 Monte Carlo runs, demonstrating that the system always escapes the three locally unstable equilibrium points and converges to the globally optimal solution. Fig. 4(b) plots a typical example of the trajectories of the total energy and total potential energy w.r.t. time. We see that the total energy is always non-increasing, but the potential energy oscillates and eventually reaches its global minimum. From the lens of optimization, the simulation method can be seen as first building an upper bound (the total energy) for the target objective function (the potential energy), and then minimizing the upper bound. In this case, the upper bound eventually becomes tight.
In conclusion, we provide the first theoretical understanding of using dynamics-based simulation to solve point cloud registration. We treat point clouds as rigid bodies that contain a finite collection of particles and we treat registration as the process of one point cloud moving towards the other point cloud under forces and torques induced by virtual springs and viscous damping. We show that the total potential energy of the dynamical system recovers the objective function in maximum likelihood estimation. We then leverage the invariant set theorem to show that the system converges to the set of equilibrium points, of which one equilibrium point corresponds to the global optimizer of the maximum likelihood estimation. Further, supported by numerical experiments, we conjecture that, besides the globally optimal equilibrium point, the system contains either three or infinite spurious equilibrium points, and all spurious equilibria are locally unstable. This suggests that running the simulation can always obtain the global optimizer. Future work includes extending the analysis to the case where correspondences are corrupted by outliers.
The authors would like to thank Prof. Jean-Jacques Slotine for fruitful discussions.
- (1) K.S. Arun, T.S. Huang, and S.D. Blostein. Least-squares fitting of two 3-D point sets. IEEE Trans. Pattern Anal. Machine Intell., 9(5):698–700, sept. 1987.
- (2) Haruhiko Asada and J-JE Slotine. Robot analysis and control. John Wiley & Sons, 1986.
- (3) M. A. Audette, F. P. Ferrie, and T. M. Peters. An algorithmic overview of surface registration techniques for medical imaging. Med. Image Anal., 4(3):201–217, 2000.
- (4) J.C. Bazin, Y. Seo, R.I. Hartley, and M. Pollefeys. Globally optimal inlier set maximization with unknown rotation and focal length. In European Conf. on Computer Vision (ECCV), pages 803–817, 2014.
- (5) G. Blais and M. D. Levine. Registering multiview range data to create 3d computer objects. IEEE Trans. Pattern Anal. Machine Intell., 17(8):820–824, 1995.
S. Choi, Q. Y. Zhou, and V. Koltun.
Robust reconstruction of indoor scenes.
IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), pages 5556–5565, 2015.
- (7) Christopher Choy, Jaesik Park, and Vladlen Koltun. Fully convolutional geometric features. In Intl. Conf. on Computer Vision (ICCV), pages 8958–8966, 2019.
- (8) B. Drost, M. Ulrich, N. Navab, and S. Ilic. Model globally, match locally: Efficient and robust 3D object recognition. In IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), pages 998–1005, 2010.
- (9) M. Fischler and R. Bolles. Random sample consensus: a paradigm for model fitting with application to image analysis and automated cartography. Commun. ACM, 24:381–395, 1981.
- (10) Vladislav Golyanik, Sk Aziz Ali, and Didier Stricker. Gravitational approach for point set registration. In IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), pages 5802–5810, 2016.
- (11) Vladislav Golyanik, Christian Theobalt, and Didier Stricker. Accelerated gravitational point set alignment with altered physical laws. In Intl. Conf. on Computer Vision (ICCV), pages 2080–2089, 2019.
- (12) P. Henry, M. Krainin, E. Herbst, X. Ren, and D. Fox. Rgb-d mapping: Using kinect-style depth cameras for dense 3d modeling of indoor environments. Intl. J. of Robotics Research, 31(5):647–663, 2012.
- (13) Berthold K. P. Horn. Closed-form solution of absolute orientation using unit quaternions. J. Opt. Soc. Amer., 4(4):629–642, Apr 1987.
- (14) Philipp Jauer, Ivo Kuhlemann, Ralf Bruder, Achim Schweikard, and Floris Ernst. Efficient registration of high-resolution feature enhanced point clouds. IEEE Trans. Pattern Anal. Machine Intell., 41(5):1102–1115, 2018.
- (15) P. Marion, P. R. Florence, L. Manuelli, and R. Tedrake. Label fusion: A pipeline for generating ground truth labels for real rgbd data of cluttered scenes. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages 1–8. IEEE, 2018.
- (16) R.B. Rusu, N. Blodow, and M. Beetz. Fast point feature histograms (fpfh) for 3d registration. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages 3212–3217. Citeseer, 2009.
- (17) Jean-Jacques E Slotine, Weiping Li, et al. Applied nonlinear control, volume 199. Prentice hall Englewood Cliffs, NJ, 1991.
- (18) G. K. L. Tam, Z. Q. Cheng, Y. K. Lai, F. C. Langbein, Y. Liu, D. Marshall, R. R. Martin, X. F. Sun, and P. L. Rosin. Registration of 3d point clouds and meshes: a survey from rigid to nonrigid. IEEE Trans. Vis. Comput. Graph., 19(7):1199–1217, 2013.
J. M. Wong, V. Kee, T. Le, S. Wagner, G. L. Mariottini, A. Schneider, L.
Hamilton, R. Chipalkatty, M. Hebert, D. M. S. Johnson, et al.
Segicp: Integrated deep semantic segmentation and pose estimation.In IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages 5784–5789. IEEE, 2017.
- (20) H. Yang, P. Antonante, V. Tzoumas, and L. Carlone. Graduated non-convexity for robust spatial perception: From non-minimal solvers to global outlier rejection. IEEE Robotics and Automation Letters (RA-L), 5(2):1127–1134, 2020. arXiv preprint arXiv:1909.08605 (with supplemental material).
- (21) H. Yang and L. Carlone. A polynomial-time solution for robust registration with extreme outlier rates. In Robotics: Science and Systems (RSS), 2019.
- (22) H. Yang and L. Carlone. A quaternion-based certifiably optimal solution to the Wahba problem with outliers. In Intl. Conf. on Computer Vision (ICCV), 2019.
- (23) H. Yang and L. Carlone. In perfect shape: Certifiably optimal 3D shape reconstruction from 2D landmarks. In IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), 2020. Arxiv version: 1911.11924.
- (24) H. Yang, J. Shi, and L. Carlone. TEASER: Fast and Certifiable Point Cloud Registration. arXiv preprint arXiv: 2001.07715, 2020.
- (25) A. Zeng, K. T. Yu, S. Song, D. Suo, E. Walker, A. Rodriguez, and J. Xiao. Multi-view self-supervised deep learning for 6d pose estimation in the amazon picking challenge. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages 1386–1383. IEEE, 2017.
- (26) J. Zhang and S. Singh. Visual-lidar odometry and mapping: Low-drift, robust, and fast. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages 2174–2181. IEEE, 2015.
- (27) Q.Y. Zhou, J. Park, and V. Koltun. Fast global registration. In European Conf. on Computer Vision (ECCV), pages 766–782. Springer, 2016.