2.1. Real data and simulation

The development and the experiments with the algorithm MonoVisual3DFilter were done under two environments: simulation and a testbed in the laboratory (near real-world conditions).

A simplistic simulation environment was designed using the Ignition Gazebo Simulator.Footnote ² The scene comprises six spheres to assess the algorithm’s validity and test during implementation (Figure 2). The spheres have sizes of 5 cm and 10 cm. A bounding box camera was added to the scene to perceive the objects and support the position estimation algorithm. During the execution of the MonoVisual3DFilter (a histogram filter), the bounding box camera is moved to fixed viewpoints to enable and validate the estimator. The bounding box camera detects the objects through object detection algorithms using bounding boxes, detecting only their visible regions.

Figure 2. Simulated environment to validate the histogram filter effectiveness. Green spheres are the objects being detected, representing the tomatoes, and the black box is the bounding box camera looking at the spheres.

Figure 3. Simulated testbed in the laboratory to essay the histogram filter algorithm.

To essay the algorithm in the laboratory, near real-world greenhouse conditions, we designed a testbed with realistic leaves and plastic realistic tomatoes (Figure 3a and 3b). For perceiving the tomatoes, we assembled an OAK-1 cameraFootnote ³ (Figure 3d), as a bounding box camera, to the 6 degrees of freedom (DoF) Robotis Manipulator-H (Figure 3c). The OAK-1 camera computes an object detection model algorithm trained to perceive tomatoes. We used the You Only Look Once (YOLO) v8 Tiny trained on the tomato dataset [Reference Magalhães, Castro, Moreira, dos Santos, Cunha, Dias and Moreira45, Reference Magalhães46] and some samples of the plastic tomato from multiple perspectives. However, any object detection or instance segmentation algorithm can be used to perceive the objects of interest in the scene, since they can effectively lead with the different environment perturbances, such as lighting variations or are robust to occlusion. The manipulator also moved to fixed viewpoints that ensured the tomatoes’ visibility. The manipulator was assembled on the mobile platform AgRob v16 from INESC TEC (Figure 1), but 3D position of tomatoes was computed to the manipulator’s base frame.

The OAK-1 is a fully integrated system for bounding box cameras. This camera was designed by the Luxonis Holding Corporation and has a single 12 MP RGB camera module. To allow the on-system object detection, the camera module is connected to the OAK-SoM.Footnote ⁴ The full camera connects to other devices through USB-C communication. The OAK-SoM is a system on module (SoM) designed to integrate into top-level and low-power systems and has the capability to process artificial neural networks (ANNs). This camera module was integrated into the AgRob v16 robot (Figure 1).

This robot is based on the Clearpath HuskyFootnote ⁵ mobile platform and was designed to operate in harsh agricultural environments, such as the Douro’s steep slope vineyards. At the front of the mobile platform, there is a controlling head, which is the unit with the computer and all the required devices to control the robot and establish communications. At the backside, the robot has the 6 DoF anthropomorphic manipulator Robotis Manipulator-H.Footnote ⁶

2.2. Histogram filter

Histogram filters have been widely used in literature for self-localisation and navigation in mobile robots [Reference Boyko and Hladun36, Reference Moscowsky37]. However, for the current study, we intend to apply histogram filters to localise the 3D position of tomatoes concerning the manipulator’s base frame, in a solution called MonoVisual3DFilter.

The histogram filter is a computationally intensive algorithm that can estimate the relative position of objects. The filter computes probabilities for multiple points in a discretised space. After, it intersects the chances of various views to estimate the localisation and the occupied area of the regions of interest (Figure 4).

Figure 4. Intersection between multiple viewpoints in 2D plane.

Histogram filters can be set as an application of a discrete Bayes filter to the continuous state space. For this study, we applied the histogram filter as stated by Thrun [Reference Thrun, Burgard and Fox38].

The histogram filter decomposes the continuous state space in a finite number of regions. Eq. 1 describes a discretised state space. $X_t$ is the random variable describing the state of the objects being detected at the time $t$ . $\text{dom}(X_t)$ denotes the state space, which is all the possible values that $X_t$ might assume. The most straightforward discretisation of a continuous state space is through a multidimensional grid, where $x_{k,t}$ denotes each grid cell.

(1) \begin{equation} \text{dom}(X_t) = x_{1,t} \cup x_{2,t} \cup \cdots \cup x_{K,t} \end{equation}

Only part of the state space must be discredited to limit computation efforts, mainly due to the manipulator’s reachability. Therefore, as soon as the manipulator’s camera detects an object of interest, in the first viewpoint, the space behind the camera is decomposed through a grid scheme. We considered the probable space around the object as twice the manipulator’s reaching limits. Twice the manipulator’s reachability offers enough margin to identify and validate some fruits in the limits of the manipulator’s reachability. The 3D decomposition is centred in the camera in $y\mathcal{O}z$ , i.e. in $(0, 0)$ in the camera’s frame and further distanced by the manipulator’s reachability radius in $\mathcal{O}x$ . Once decomposed, the discrete state space remains static and only $\text{dom}(X_t)$ is updated.

The histogram filter demands moving the camera to multiple strategic viewpoints and updating the probabilities grid. So, at the space decomposition, an associated probabilities matrix is created with a probability to each cell initialised to one, i.e. $\text{dom}(X_0) = [1 \ldots 1]$ . This means that at the beginning, the object of interest is probable to be anywhere in the decomposed space.

For estimating the position of the objects, $\text{dom}(X_t)$ is updated in each viewpoint. Each cell, $x_{i,t}$ , from the decomposed state space is transformed from the manipulator’s base frame to the image’s frame. The probability of an object in a given cell, $x_{i,t}$ , knowing the viewpoint, is given by (2). Finally, the updated probability of an object being in the cell $x_{i,t}$ is given by (3).

(2) \begin{equation} p(x_{i,t} | \text{viewpoint}_k) = \dfrac{1}{N} \sum _j^N p(x_{i,t} | \text{bbox}_j, \text{viewpoint}_k) \end{equation}

(3) \begin{equation} p(x_{i,t}) = p(x_{i,t} | \text{viewpoint}_k) \cdot p(x_{i,t-1}) \end{equation}

In eq. 2, to get $p(x_{i,t} | \text{bbox}_j, \text{viewpoint}_k)$ , a kernel function was designed. Two kernel functions were essayed to localise the objects in the state space: the square and Gaussian functions. The square kernel, applied to each bounding box, states that if the transformed point is inside a bounding box of the image’s frame, the probability is one; otherwise is zero (4). Using the square kernel function, we will have a binary mask stating that if a point is inside the bounding box, then we can have an object. Otherwise, we don’t.

(4) \begin{equation} p(x_{i,t} | \text{bbox}_j, \text{viewpoint}_k) = \begin{cases} 1 & \text{if inside bounding box} \\[5pt] 0 & \text{otherwise} \end{cases} \end{equation}

As an alternative to the aggressive behaviour of the square function, we also essayed the bidimensional Gaussian function (5). This function delivers a smooth effect for the borders of the bounding box and some cells $x_{i,t}$ outside the bounding boxes. Therefore, a Gaussian function should better tolerate irregular objects and noise. In Eq. 5, $(x_0, y_0)$ is the centre of bounding box $j$ in the sensor’s frame, and $(x,y)$ is the position of each cell $x_{i,t}$ in the sensor’s frame. The coordinates in the image’s frame are projected by a projection model stated in section 2.3. The standard deviation values $(\sigma _x, \sigma _y)$ correspond to half of the size of the bounding box $j$ . All the values were obtained experimentally and had reasonable results. If we use the Gaussian kernel (5) to estimate the objects’ position, Eq. 2 will correspond to a mixture of Gaussians, attending that the detection camera detects multiple objects. The mixture of Gaussians results in a function that smooths with increasing Gaussians in the mixture. To avoid this effect, a normalised version of the mixture of Gaussians is used to highlight the different detected objects (6). Besides, the updated state space $\text{dim}(X_t)$ is also normalised at the end of each iteration of the histogram filter.

(5) \begin{equation} p(x_{i,t} | \text{bbox}_j, \text{viewpoint}_k) = \exp{\left (-\dfrac{(x-x_0)^2}{2\sigma _x^2}-\dfrac{(y-y_0)^2}{2\sigma _y^2}\right )} \end{equation}

(6) \begin{equation} p(x_{i,t} | \text{viewpoint}_k) = \dfrac{p(x_{i,t} | \text{viewpoint}_k)}{\max (p(x_{i,t} | \text{viewpoint}_k))} \end{equation}

The algorithm 1 summarises the procedures for updating the cell’s weights during the histogram filtering.

Algorithm 1. Histogram filter – updating weights

2.3. Camera projection model

While applying the histogram filter, we decomposed the state space in a finite state space grid. To effectively estimate the 3D position of the object, we moved the detection camera around the object and the decomposed state space to visualise the scene from multiple perspectives (Figure 5).

Figure 5. Intersection of the camera around in the decomposed space.

Detecting objects using the detection camera requires an effective projection model to estimate the object’s position in the 3D space, transforming between the 3D space coordinates and the image’s frame. For simplification, we applied the Pinhole model to transform the 3D space coordinates to the image’s frame.

Acknowledging the points of the 3D space in the camera’s frame, before we project them in the image’s frame, we have to convert them to the sensor’s frame. Both are placed in the same origin, but they have different orientations. The sensor uses the frame as illustrated in Figure 6. The illustrated rotation can be stated by Euler angles like Euler(YZX) $=$ ( $0^{\circ }$ , $90^{\circ }$ , $-90^{\circ }$ ) which reflects in the quaternion $q=(\!-0.5, 0.5, -0.5, 0.5)$ .

Figure 6. Conversion between the camera’s and sensor’s frames (blue – sensor’s frame; black – camera’s frame).

The transformation between the sensor’s frame and the image’s coordinates can be made by the intrinsics parameters matrix as stated in Eq. 7. This matrix depends on the image’s width ( $w$ ) and height ( $h$ ), as well as on the camera’s focal length ( $f$ ). The focal length depends on the camera’s horizontal field of view (HFOV) and the image’s width and can be calculated by Eq. 8.

(7) \begin{equation} (u,v,1) = \begin{bmatrix} f & 0 & \frac{w}{2} \\[5pt] 0 & f & \frac{h}{2} \\[5pt] 0 & 0 & 1 \\[5pt] \end{bmatrix} \cdot \begin{bmatrix} \frac{x}{z} \\[5pt] \frac{y}{z} \\[5pt] 1 \end{bmatrix} \end{equation}

(8) \begin{equation} f = \dfrac{0.5 \times w}{\tan \left (0.5 \times HFOV \times \dfrac{\pi }{180}\right )} \end{equation}

However, this model type is only valid behind ideal scenes, such as in the simulation. For the OAK-1 camera, an additional calibration step was required to estimate the intrinsic parameters. For calibration, we used the Kalibr software [Reference Maye, Furgale and Siegwart47].

2.4. Objects positioning

At the end of the execution of the histogram filter for multiple viewpoints, the state space $\text{dom}(X_t)$ should have different clusters of points. As we know the number of objects in the scene through the number of detected objects by the detection camera, we can use the k-means algorithm to aggregate the points and compute the centre of each cluster.

The k-means algorithm tries to cluster the different points of the discrete state space by minimising the geometric distance between points to the cluster’s centre (9). In Eq. 9, we minimise the Euclidean distance between points to $\mu _j$ , the centre point of each cluster in $C$ .

(9) \begin{equation} \sum _{i=0}^{n} \min _{\mu _j \in C}(||x_{i,t} - \mu _j||^2) \end{equation}

After clustering by the k-means, the state space should have as many point clouds as the number of objects detected by the detection camera. The computation of the centre of the clouds to get the position of the detected objects can be done in two ways:

1. 1. computation of the geometric centre of the cloud; or

2. 2. computation of the weighted centre of the cloud.

The geometric centre of each point cloud is the Euclidean centre $\mu _j$ minimised during the k-means algorithm for Eq. 9. Besides the geometric centre, the k-means also return the points, $x_{i,t}$ , that belong to each cloud, $S_j$ . Considering the state of each element of the state space $\text{dom}(X_t)$ at the end of the histogram filter, each element $x_{i,t}$ should have a weight attributed, $w_i$ . So the weighted centre is the weighted average (10) of the coordinates of $x_{i,t}$ that belong to the set $S_j$ .

(10) \begin{equation} \mu _j = \begin{bmatrix} \dfrac{\sum _i^N w_i \cdot x_{{i,t}_1}}{N} & \dfrac{\sum _i^N w_i \cdot x_{{i,t}_2}}{N} & \dfrac{\sum _i^N w_i \cdot x_{{i,t}_3}}{N} \end{bmatrix}^T \qquad \forall x_{i,t} \in S_j \end{equation}

2.5. Experiments

Three essays were performed in different environments to validate the effectiveness of the MonoVisual3DFilter.

Using the Gazebo simulator, we created a scene with multiple spheres to estimate their position in the scene (Figure 2). Once we used a bounding box camera without noise, this approach allowed validating the real performance of the filter without external artefacts or noise. Additionally, we performed an additional essay, introducing some Gaussian noise that randomly changes the position and size of the bounding box of the detected objects, as well as whether the object is successfully detected. Because we do not assemble any manipulator at the simulator, the bounding box camera has more freedom to state its pose. So, during the simulations, we set the camera’s pose to ensure the spheres’ visibility. Figure 7 illustrates the visible image of the camera at each pose. In the first pose, the camera looks straight towards the spheres (Figure 7a). After the camera moves down and left, looking upwards (Figure 7b), and finally, the camera moves up and right to the first pose, looking downwards (Figure 7c). This composition of the camera was kept for both experiments in the simulator.

Figure 7. View of the spheres by the bounding box camera at each fixed viewpoint. The green square boxes around the spheres are the bounding boxes of the detected spheres by the bounding box camera.

In the third essay, a realistic testbed was deployed to experiment with the algorithm in near-real-world conditions at the laboratory (Figure 3). The testbed is composed of realistic artificial leaves and realistic plastic tomatoes. The tomatoes were hung in the testbed between the leaves. For baselining the tomatoes’ position in the testbed, we relied on the manipulator’s kinematics. For each tomato in the testbed, we moved the manipulator end-effector until the fruit and retrieved the end-effector’s position. This will be the tomato position to the manipulator’s base frame. Similarly to the essays in simulation, the bounding box camera at the simulator moved to three fixed poses that always assured the visibility of the tomatoes. A similar scheme to the one used before was set concerning the several limitations of the manipulator’s manoeuvrability that made it difficult to set some poses to the camera. Unlike the simulation essays, several experiments were performed in the testbed. In the different experiments, we considered between one to three tomatoes being localised simultaneously, summing up to ten tomatoes and sixty measures. Figure 8 illustrates the tomatoes’ visibility by the OAK camera at each selected pose, for the different experiments. The rows represent the different considered experiments and each image has the tomatoes being localised simultaneously.

Figure 8. View of the tomatoes in the testbed at each pose of the OAK-1 camera. The blue squares around the tomatoes are the detected tomatoes by the bounding box camera OAK-1 using a custom-trained YOLO v8 tiny detector. Inside each bounding box are the detected class (tomato) and the detection confidence. Each row is an experiment, in a total of six experiments, and each figure contains the number of tomatoes being detected.

To better assess the performance of the histogram filter to estimate the position of objects, we extracted some error metrics, namely, the mean absolute error (11), the mean square error (12), the root mean square error or standard deviation (13), and the mean absolute percentage error (14). In these equations, $\mu _j$ is the real centre of the object for the cluster $S_j$ , and $\hat{\mu }_j$ is the estimated one using the previous methods, given the cluster $j$ until the maximum of clusters $M$ .

(11) \begin{equation} \text{MAE }(\mu _j,\hat{\mu }_j) = \dfrac{1}{N\cdot M} \sum _i^N \sum _j^M |\mu _{ij} - \hat{\mu }_{ij}| \qquad \forall j \in \mathbb{N}\;:\;\{1 .. M\} \end{equation}

(12) \begin{equation} \text{MSE }(\mu _j,\hat{\mu }_j) = \dfrac{1}{N\cdot M} \sum _i^N \sum _j^M (\mu _{ij} - \hat{\mu }_{ij})^2 \qquad \forall j \in \mathbb{N}\;:\;\{1 .. M\} \end{equation}

(13) \begin{equation} \text{RMSE }(\mu _j,\hat{\mu }_j) = \sqrt{ \dfrac{1}{N\cdot M} \sum _i^N \sum _j^M (\mu _{ij} - \hat{\mu }_{ij})^2} \qquad \forall j \in \mathbb{N}\;:\;\{1 .. M\} \end{equation}

(14) \begin{equation} \text{MAPE }(\mu _j, \hat{\mu }_j) = \dfrac{1}{N\cdot M} \sum _i^N \sum _j^M \left |\dfrac{\mu _{ij}-\hat{\mu }_{ij}}{\mu _{ij}}\right | \times 100 \qquad \forall j \in \mathbb{N}\;:\;\{1 .. M\} \end{equation}

Figure 9. Iteration of the histogram filter during simulation for detecting the six spheres, considering a square kernel. (a) Decomposition of the state space at the beginning of the algorithm; (b) detection at the end of the first viewpoint; (c) detection at the end of the second viewpoint; (d) detection at the end of the third viewpoint.