*2.3. Fruit Tree Point Cloud Data Collection Method Based on 2D-ICP*

With the increase in distance, the amount of LiDAR sensor data collected will become less and less. At the same time, in the process of acquisition, the fruit tree rows on both sides of the non-test platform will be blocked and the information will be incomplete, resulting in a poor iterative effect. Therefore, for the preprocessing of the collected LiDAR point cloud data, only the fruit tree row data on both sides of the test platform will be retained. After obtaining the LiDAR sensor data of fruit trees on both sides of the test platform, the 2D-ICP algorithm can be used for registration. The specific algorithm steps are as follows. The preprocessed target point cloud and source point cloud are *P<sup>k</sup>* = *Pk i* and *Pk*+<sup>1</sup> = *Pk*+<sup>1</sup> *i* , *i* = 1, 2, 3 ... *n*. According to the 2D-ICP algorithm, Equation (1) gives the objective function.

$$Dist(R, T)\_{min} = \frac{1}{n} \sum\_{i=1}^{n} \left| P\_i^k - \left( RP\_i^{k+1} + T \right) \right|^2 \tag{1}$$

There are two variables in Equation (1) that can be regularized by considering only the rotation matrix *<sup>R</sup>*—that is, the centers of two frame point clouds are *CPk* = <sup>1</sup> *<sup>n</sup>* <sup>∑</sup>*<sup>n</sup> <sup>i</sup>*=<sup>1</sup> *P<sup>k</sup> i* and *CPk*<sup>+</sup><sup>1</sup> = <sup>1</sup> *<sup>n</sup>* <sup>∑</sup>*<sup>n</sup> <sup>i</sup>*=<sup>1</sup> *<sup>P</sup>k*+<sup>1</sup> *<sup>i</sup>* , where *<sup>P</sup><sup>k</sup> <sup>i</sup>* = *<sup>P</sup><sup>k</sup> <sup>i</sup>* <sup>−</sup> *CPk* and *<sup>P</sup>k*+<sup>1</sup> *<sup>i</sup>* <sup>=</sup> *<sup>P</sup>k*+<sup>1</sup> *<sup>i</sup>* − *CPk*<sup>+</sup><sup>1</sup> .

Then, Equation (1) becomes Equation (2).

$$Dist(R, T)\_{min} = \frac{1}{n} \sum\_{i=1}^{n} \left| P\_i^k - RP\_i^{k+1} \right|^2 \tag{2}$$

Decomposing Equation (2) into Equation (3) gives us:

$$Dist(R, T)\_{min} = \frac{1}{n} \left( \sum\_{i=1}^{n} \left| P\_i^k \right|^2 + \sum\_{i=1}^{n} \left| RP\_i^{k+1} \right|^2 - 2 \sum\_{i=1}^{n} \left| P\_i^k RP\_i^{k+1} \right|^2 \right) \tag{3}$$

If the objective function is minimized, Equation (4) is maximized:

$$F(R)\_{\max} = \frac{1}{n} \sum\_{i=1}^{n} \left| P\_i^k R P\_i^{k+1} \right|^2 \tag{4}$$

Since the fruit tree data are two-dimensional, the rotation matrix *R* is valued as *cosθ* −*sinθ sinθ cosθ* , where *θ* is the rotation angle between the two frame point clouds, which is substituted into Equation (4) to obtain Equation (5).

$$F(R)\_{\max} = \frac{1}{n} \sum\_{i=1}^{n} \left| \begin{bmatrix} P\mathbf{x}\_i^k & P\mathbf{y}\_i^k \end{bmatrix} \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} P\mathbf{x}\_i^{k+1} \\ P\mathbf{y}\_i^{k+1} \end{bmatrix} \right|^2 \tag{5}$$

The derivation and extreme value of *θ* are used to deduce Equation (6).

$$\frac{\sin \theta}{\cos \theta} = \sin \theta \frac{1}{n} \sum\_{i=1}^{n} \left( \frac{P y\_i^k \times P x\_i^{k+1} - P x\_i^k \times P y\_i^{k+1}}{P x\_i^k \times P x\_i^{k+1} + P y\_i^k \times P y\_i^{k+1}} \right) \tag{6}$$

After *θ* is calculated, *R* can be obtained. *T* can be obtained by Equation (7), and then iterated until the threshold is satisfied.

$$T = \begin{bmatrix} \Delta x\\ \Delta y \end{bmatrix} = \begin{bmatrix} \mathbf{C}\_{p^k}^x\\ \mathbf{C}\_{p^k}^y \end{bmatrix} - R \begin{bmatrix} \mathbf{C}\_{p^{k+1}}^x\\ \mathbf{C}\_{p^{k+1}}^y \end{bmatrix} \tag{7}$$
