## Materials and Methods  
### Notation and Preliminaries  
In this section, for the sake of illustration, we briefly summarize some relevant mathematical concepts and notation, including the camera model, the representation of 3D rigid body transformation, and the point cloud representation of 3D scene model [41; 42]. Let \(\mathtt{p}_{c}=(x_{c},y_{c},z_{c},1)^{t}\) be the homogeneous coordinates of a 3D point in the camera coordinate system. The pinhole camera model unprojects the 3D point into the pixel \(\mathrm{p}_{I}=(u,v,1)^{t}\) in an image \(I\) using the following equation:  
\[\mathrm{p}_{I}=\pi(\mathrm{p}_{c})=\left(\frac{f_{x}x_{c}+c_{x}}{z(\mathrm{p}_{ I})},\frac{f_{y}y_{c}+c_{x}}{z(\mathrm{p}_{I})},1\right)^{t} \tag{1}\]  
where \(f_{x}\) and \(f_{y}\) are the \(x\) and \(y\) direction focal length, respectively; \(c_{x}\) and \(c_{y}\) are the principal point offsets; \(z(\mathrm{p}_{I})\) is the depth value of the 2D point \(\mathrm{p}_{I}\). Obviously, given the depth value \(z(\mathrm{p}_{I})\), the inverse projection function \(\pi^{-1}(\cdot)\) projects a 2D point \(\mathrm{p}_{I}\) back to the 3D point \(\mathrm{p}_{c}\):  
\[\mathrm{p}_{c}=\pi^{-1}\Big{(}\mathrm{p}_{I},z(\mathrm{p}_{I})\Big{)}=z( \mathrm{p}_{I})\Bigg{(}\frac{u-c_{x}}{f_{x}},\frac{v-c_{y}}{f_{y}},1\Bigg{)}^{t} \tag{2}\]  
Based on the theory of three-dimensional special orthogonal (SO(3)) Lie group, the pose of the classical rigid body can be described by the camera motion model of six-degree-of-freedom (6 DOF) which constitutes an orthogonal rotation matrix \(\mathrm{R}_{3\times 3}\in\mathbb{R}^{3\times 3}\) and a translation vector \(\mathrm{t}_{3\times 1}\in\mathbb{R}^{3}\). The 6 DOF camera motion model also defines the rigid body transformation in a 3D space \(\mathbb{R}^{3}\) and is defined as a \(4\times 4\) matrix:  
\[\mathrm{M}_{4\times 4}=\left[ \begin{array}{cc}\mathrm{R}_{3\times 3}& \mathrm{t}_{3\times 3}\\ 0&1\end{array} \right] \tag{3}\]  
The rigid body transformations in \(\mathbb{R}^{3}\) form a smooth manifold and therefore are also a Lie group. The group operator is the matrix left-multiplication. Thus, a 3D point \(\mathrm{p}_{c}\) in the camera coordinate system can be transformed into a 3D point \(\mathrm{p}_{w}=(x_{w},y_{w},z_{w},1)^{t}\) in the world coordinate system by matrix left-multiplication:  
\[\mathrm{p}_{w}=\psi(\mathrm{M}_{4\times 4},\mathrm{p}_{c})=\mathrm{M}_{4 \times 4}\mathrm{p}_{c} \tag{4}\]  
Inversely, the inverse function \(\psi^{-1}(\cdot)\) transformation \(\mathrm{p}_{g}\) back to \(\mathrm{p}_{c}\):  
\[\mathrm{P}_{c}=\psi^{-1}(\mathrm{M}_{4\times 4}^{-1},\mathrm{p}_{w})=\mathrm{M}_{4 \times 4}^{-1}\left[ \begin{array}{c}x_{w}\\ y_{w}\\ z_{w}\\ 1\end{array} \right]=\left[ \begin{array}{cc}\mathrm{R}_{3\times 3}^{-1}&- \mathrm{R}_{3\times 3}^{-1}\mathrm{t}_{3\times 1}\\ 0&1\end{array} \right]\left[ \begin{array}{c}x_{g}\\ y_{g}\\ z_{g}\\ 1\end{array} \right] \tag{5}\]  
Estimating the pose of a rigid scene means to determine its rigid motion in the 3D space from 2D images. Researchers in 3D reconstruction have shown that the colored point cloud representation, which builds up one-to-one correspondence between points in the 3D space and pixels in the image space is an efficient and effective approach to define the visual appearance and shape of a scene or object. Accordingly, in this paper, we integrate the camera parameters and depth images of a video that captures the RGB-D image frame from individual viewpoints of the scene under surveillance into a colored point cloud to represent the geometry and texture of the scene. A colored point cloud is a collection of points \(\{\mathrm{p}_{i}\}\), each representing a color vector and a normal vector at a particular position in a 3D space.