# Object-Level Semantic Map Construction for Dynamic Scenes
## 3 Methodology
### Object Motion Detection through 3D Tracking Based on Object Model Database
#### 3.3.2 Photometric Error Term  
The photometric image registration is defined as minimizing the brightness constancy between the live frame and synthesized view of the 3D model in frame \(t-1\). The cost takes the form:  
Figure 2: Based on the camera pose corresponding to the previous frame, the dynamic object mask is projected from the dynamic object database, and the dynamic object part in the current frame is removed. (**a**) Final dynamic object mask; (**b**) dynamic part culling in intensity image; (**c**) dynamic part culling in depth image. \[E_{rgb}^{static}= \begin{cases}\sum\limits_{u}(I_{t}(u)-I_{t-1}(\pi\left(\text{ KT}_{rou}\pi^{-1}(u,D_{t}))\right))\end{cases} ^{2}&\text{if }u\
otin Mask_{dynamic}\\ 0&\text{if }u\in Mask_{dynamic}\end{cases} \tag{4}\]  
After the current image RGB data approximately eliminates the dynamic objects in the object database, the remaining intensity data \(I_{t}(u)\) and \(I_{t-1}(\cdot)\) that come from the predicted intensity map of the static model (including the background model and static objects) from the previous frame \(t-1\) are used to construct an error equation to iteratively calculate the current frame camera pose \(T_{rou}\) based on the assumption of photometric consistency. Where \(\pi^{-1}(u,\ D_{t})\) represents that the current frame depth data \(D_{t}\) will be back-projected into 3D space and \(T_{rou}\pi^{-1}(u,\ D_{t})\) will transform vertex data \(\pi^{-1}(u,\ D_{t})\) to the camera coordinate system in the previous frame via pose \(T_{rou}\), the pixel coordinates in the predicted intensity image will be obtained through perspective projection \(\pi\left(K_{rou}\pi^{-1}(u,\ D_{t})\right)\), and, then the gray intensity value will be obtained. The final overall alignment error term is:  
\[E_{track}^{static}=\frac{min}{T_{rou}}\left(W_{icp}E_{icp}^{static}+E_{rgb} ^{static}\right) \tag{5}\]  
where \(W_{icp}\) is weight of the geometric error term. The cost function is minimized using the Gauss-Newton approach in a three-level coarse-to-fine scheme to calculate the rough camera pose \(T_{rou}\) relative to the static part of the environment. This tracking method fully utilizes the geometric and texture information of the effective observation data in the current frame, increases the constraint conditions of the camera pose estimation, and improves the camera pose accuracy in real-time. Especially for ICP iterations, if the number of vertex data is small, camera pose estimation will be affected. The relatively precise camera pose \(T_{rou}\) obtained in the previous step is used to update each object pose in the dynamic and static object databases according to the following definition:  
\[T_{m}=T_{obs}{}^{-1}T_{rou} \tag{6}\]  
Then, 3D tracking for each object is performed using the current frame data, the movement information of the object is updated as the prior knowledge for object motion judgement in the next frame. The vertex position \(p\), normal vector \(n\), and intensity \(I\) stored in object \(m\) are transferred to the current frame camera coordinate system using the object pose \(T_{m}\), as follows:  
\[p_{local}=T_{m}{}^{-1}p \tag{7}\]  
\[n_{local}{=T_{m}{}^{-1}n} \tag{8}\]  
\[I_{local}(\pi\left(KT_{m}{}^{-1}\pi^{-1}(u,\ D_{obs})\right)=I_{obs}(u) \tag{9}\]  
where \(p_{local}\) and \(n_{local}\) are the vertex position and normal vector, respectively, in the camera coordinate system corresponding to the current frame and \(I_{local}\) is the intensity value of the vertex in the current image plane.