convergence

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.01])
sphereId2 = p.loadURDF("sphere_small.urdf", [0.5, 1, 0.01])
#robotId = p.loadURDF("r2d2.urdf", [0.5,0.5,0.5],useFixedBase=True)
cubeId = p.loadURDF("cube_small.urdf", [0, 0.5, 0.01])
cubeId2 = p.loadURDF("cube_small.urdf", [0, 0.9, 0.01])

# 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
    new_points = copy.copy(points)
    while number_of_iteration != max_iteration and C != 1:
        # Create Open3D point cloud object
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(points)
        labels = pcd.cluster_dbscan(eps=eps, min_points=9)
        num_segments = len(set(labels))
        print("number of segments = ", num_segments)
        print("eps = ", eps)



        # 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
        else:
            print(number_of_iteration)
            number_of_iteration += 1
            eps = eps / convergence_speed
            #C = C / convergence_speed
            if C<=1:
                C = 1
                break
    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, 300, 1000, 0.1, 1.001, points=points)