convergence function

import copy

import open3d as o3d
import open3d.core as o3c
import numpy as np
import matplotlib.pyplot as plt
import pybullet as p
import pybullet_data
import time

from sklearn.neighbors import NearestNeighbors
import plotly.express as px

# Initialize the physics simulation
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)

# Load the objects into the simulation
planeId = p.loadURDF("plane.urdf")
sphereId = p.loadURDF("sphere_small.urdf", [0, 1, 0.1])
sphereId2 = p.loadURDF("sphere_small.urdf", [0.5, 1, 0.1])
#robotId = p.loadURDF("r2d2.urdf", [0.5,0.5,0.5],useFixedBase=True)
cubeId = p.loadURDF("sphere_small.urdf", [0, 0.5, 0.1])
cubeId2 = p.loadURDF("cube_small.urdf", [0, 0.9, 0.1])

# Define constant C for depth
C=10000

number_of_iteration = 0

eps = 2


# Convergence algorithm to find eps for DBSCAN

def convergence_eps(number_of_objects, max_iteration, C, eps, convergence_speed, points):
    number_of_iteration = 0

    while number_of_iteration != max_iteration and C != 1:
        new_points = copy.copy(points)
        for y in range(height):
            for x in range(width):
                new_points[y * width + x, 2] = new_points[y * width + x, 2] * C
        # Create Open3D point cloud object
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(new_points)
        labels = pcd.cluster_dbscan(eps=eps, min_points=9)
        num_segments = len(set(labels))
        print("number of segments = ", num_segments)
        print("eps = ", eps)
        colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [2, 0, 0], [0, 3, 0]]
        color_array = [colors[label % num_segments] for label in np.asarray(labels).flatten()]
        pcd.colors = o3d.utility.Vector3dVector(color_array)


        # number of objects must include the ground
        if number_of_objects == num_segments and C == 1:
            print('number_of_iteration ', number_of_iteration)
            print('eps= ', eps)
            return eps
            break

        elif num_segments < number_of_objects:
            number_of_iteration += 1
            print("convergence speed = ", convergence_speed)
            eps -= convergence_speed
            print("2")

        elif num_segments >= number_of_objects:
            number_of_iteration += 1
            C -= 200
            if C <= 1:
                C = 1
            print("C = ", C)
            print('eps =', eps)

            o3d.visualization.draw_geometries([pcd])
            print("1")


    print('eps = ', eps)
    print("C = ", C)


while True:
    # Get the image from the camera
    camera_pos = [0, 1, 2]
    viewMatrix = p.computeViewMatrix(
        cameraEyePosition=camera_pos,
        cameraTargetPosition=[0, 1, 0],
        cameraUpVector=[0, 1, 0])

    # Set the intrinsic parameters of the camera
    fov = 60
    aspect = 320.0 / 240.0
    near = 0.01
    far = 100

    # Get the image from the camera
    width, height, rgbImg, depthImg, segImg = p.getCameraImage(width=320, height=240, viewMatrix=viewMatrix,
                                                               projectionMatrix=p.computeProjectionMatrixFOV(fov,
                                                                                                             aspect,
                                                                                                             near, far))
    # Convert the depth image to a point cloud
    points = np.zeros((height * width, 3))

    for y in range(height):
        for x in range(width):
            depth = depthImg[y, x]
            if depth != 0:
                points[y * width + x, 0] = (x - 160) * depth / 160.0
                points[y * width + x, 1] = (y - 120) * depth / 120.0
                points[y * width + x, 2] = depth

    convergence_eps(5, 1000, 7000, 0.1, 0.01, points=points)