## 4 Location Estimation of Truck Cargo Box
### Recognition of Initial 3D Position Using a Stereo Camera  
Although the box is plane-shaped, the 3D distance provided by the stereo camera does not provide an accurate representation of the shape if the box is single-colored. Consequently, the plane data obtained by the LiDAR sensor on the rear of the cargo box are used to correct the acquired depth value. In the dense depth map obtained from the stereo camera, the 3D point cloud is expressed as \(Q_{i}(x_{i},y_{i},z_{i})\). Hence, the points closer  
Figure 9: Result of removing outlier point clouds in LiDAR data.  
to the cargo box plane must be sorted in advance as they are likely to be situated on the cargo box door. It is possible to calculate the distance between point \(Q_{i}\) and plane \(\pi_{L}\) using Equation (7):  
\[d=\frac{|ax_{i}+by_{i}+cz_{i}+\mathrm{d}|}{\sqrt{a^{2}+b^{2}+c^{2}}},\ d<30 \tag{7}\]  
Generally, only points with a difference in distance within 30 cm are considered cargo box door points; all other points are regarded as noise and are eliminated. The sorted points can also be projected onto plane \(\pi_{L}\) using Equation (8):  
\[Q^{\prime}_{i}=Q_{i}-d\times\pi_{L}. \tag{8}\]  
Figure 10 shows a projection of the dense 3D point cloud acquired by the stereo camera on the cargo box plane obtained by LiDAR sensors. The distance errors are calibrated as the LiDAR sensor measurement values are projected onto the plane. The projected 3D points can be used to estimate the initial 3D position of the excavator cargo box. The 3D hexahedron model based on the actual dimensions of the box is generated, as shown in Figure 11. Next, four points near the vertex in the projection points cloud are detected to match the hexahedron model to the actual cargo box plane, as shown in Figure 12. The points corresponding to the hexahedron model are determined in the same order. Figure 13 illustrates the method of matching the 3D hexahedron model and the truck cargo box.  
Figure 11: Three-dimensional model of the cargo area.  
Figure 10: The result of projecting a dense point cloud obtained with a stereo camera onto the equation of the truck’s back plate plane obtained from LiDAR.  
First, the four vertices (\(M_{1}\), \(M_{2}\), \(M_{3}\), \(M_{4}\)) on the 3D hexahedron model and the four vertices (\(Q^{\prime}\), \(Q^{\prime}\), \(Q^{\prime}\), \(Q^{\prime}\), \(Q^{\prime}\)) on the cargo box plane are set as points of convergence. (\(Q^{\prime}\), \(M_{5}\)) is a virtual point of convergence used to eliminate the symmetry ambiguity. The cargo box has a rectangular parallelepiped form, and its calculation is subject to errors. The virtual point of convergence is determined from the cross-product of each point cloud unit vector, \(|A|\) and \(|\hat{B}|\), which become direction vectors of identical magnitude. When the five points of convergence are defined, the 3D transformation matrix, \(T_{M\to C}\), is obtained with its error minimized, as in the following equation. Figure 14 shows the result of matching the cargo box to the hexahedron model using the transformation matrix. The figure shows errors, but a match was made based on the cargo box plane.  
Figure 14: Example of initial 3D position and error of the truck cargo box.  
Figure 12: Example of detecting four vertices of a door.  
Figure 13: A method to match the plane between two models.