<jupyter_start><jupyter_text>RewardFor a maximization problem.<jupyter_code>voxel_size = 0.01

box_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(box, voxel_size)
part_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(part, voxel_size)

# get grid shape and position
grid_pos = box_voxel.get_min_bound()
grid_pos[-1] = 0.0  # floor

grid_shape = (box_voxel.get_max_bound() - grid_pos) / box_voxel.voxel_size
grid_shape = (grid_shape + 0.5).astype(int)

box_grid = voxel2np(box_voxel, grid_pos, grid_shape)
part_grid = voxel2np(part_voxel, grid_pos, grid_shape, mode='floating')

plt.imshow((part_grid | box_grid).sum(axis=-1))
plt.gca().set_axis_off()
plt.show()<jupyter_output><empty_output><jupyter_text>Intersection of the part with the boxTo motivate its insertion inside the volume captured.<jupyter_code>%matplotlib inline

# plt.imshow(box_grid.sum(axis=-1), cmap='gray')
plt.imshow((part_grid & box_grid).sum(axis=-1), cmap='Accent_r', alpha=0.3)

plt.gca().set_axis_off()
plt.show()
%matplotlib widget

ax = plt.figure().add_subplot(projection='3d')
ax.voxels(box_grid, facecolors='gray')
ax.voxels(part_grid, facecolors='red')

plt.show()
def intersection_value(box_grid, part_grid):
    return (box_grid & part_grid).sum()

intersection_value(box_grid, part_grid)<jupyter_output><empty_output><jupyter_text>As the part was created filling everything below it, this rewards more the parts positioned higher.  Cumulative surface distanceTo motivate the solution closest to the surface of the box image.<jupyter_code>%matplotlib inline

part_surface = (np.indices(part_grid.shape)[-1] * part_grid).max(axis=-1)
part_mask = part_grid.sum(axis=-1) > 0

box_surface = (np.indices(box_grid.shape)[-1] * box_grid).max(axis=-1)

plt.imshow(box_surface * part_mask)
plt.imshow(np.abs((box_surface * part_mask) - part_surface))
def surface_distance(box_grid, part_grid):
    part_surface = (np.indices(part_grid.shape)[-1] * part_grid).max(axis=-1)
    part_mask = part_grid.sum(axis=-1) > 0

    box_surface = (np.indices(box_grid.shape)[-1] * box_grid).max(axis=-1)
    
    distances = np.abs((box_surface * part_mask) - part_surface)
    
    return distances.sum()

surface_distance(box_grid, part_grid)<jupyter_output><empty_output><jupyter_text>Final reward<jupyter_code>def reward(box_grid, part_grid):
    return intersection_value(box_grid, part_grid) \
           - surface_distance(box_grid, part_grid)

reward(box_grid, part_grid)
%matplotlib widget

new_part = o3d.geometry.PointCloud(part)

new_part.translate(np.array([0.25,0,0]))

new_part_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(new_part, voxel_size)
new_part_grid = voxel2np(new_part_voxel, grid_pos, grid_shape, mode='floating')

ax = plt.figure().add_subplot(projection='3d')
ax.voxels(box_grid, facecolors='gray')
ax.voxels(new_part_grid, facecolors='red')

plt.show()
reward(box_grid, new_part_grid)<jupyter_output><empty_output><jupyter_text>Objective function<jupyter_code>class _PointCloudTransmissionFormat:  # see https://github.com/isl-org/Open3D/issues/218
    def __init__(self, pointcloud: o3d.geometry.PointCloud):
        self.points = np.array(pointcloud.points)
        self.colors = np.array(pointcloud.colors)
        self.normals = np.array(pointcloud.normals)

    def create_pointcloud(self) -> o3d.geometry.PointCloud:
        pointcloud = o3d.geometry.PointCloud()
        pointcloud.points = o3d.utility.Vector3dVector(self.points)
        pointcloud.colors = o3d.utility.Vector3dVector(self.colors)
        pointcloud.normals = o3d.utility.Vector3dVector(self.normals)
        return pointcloud

def eval_position(x, box_grid, part: _PointCloudTransmissionFormat, voxel_size, grid_pos, grid_shape):
    tx, ty, tz, rx, ry, rz = x

    part_ = part.create_pointcloud()

    part_.translate(np.array([tx, ty, tz]))

    part_.rotate(
        o3d.geometry.get_rotation_matrix_from_xyz(np.array([rx, ry, rz]))
    )

    part_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(part_, voxel_size)
    part_grid = voxel2np(part_voxel, grid_pos, grid_shape, mode='floating')

    return - reward(box_grid, part_grid)

eval_position([0., 0., -0.2, 0.05, 0., 0.1], box_grid, _PointCloudTransmissionFormat(part), voxel_size, grid_pos, grid_shape)<jupyter_output><empty_output><jupyter_text>PSO<jupyter_code>box_voxel.voxel_size
%matplotlib inline

import pyswarms as ps
from pyswarms.utils.plotters import plot_cost_history

from src.models.pso import eval_position, reward, _PointCloudTransmissionFormat

voxel_size = 0.01

box_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(box, voxel_size)

# get grid shape and position
grid_pos = box_voxel.get_min_bound()
grid_pos[-1] = 0.1288  # floor

grid_shape = (box_voxel.get_max_bound() - grid_pos) / box_voxel.voxel_size
grid_shape = (grid_shape + 0.5).astype(int)

box_grid = voxel2np(box_voxel, grid_pos, grid_shape)


min_bound = np.array([-0.5, -0.5, -0.5, -np.pi, -np.pi, -np.pi])
bounds = (min_bound, -min_bound)

optimizer = ps.single.GlobalBestPSO(
# optimizer = ps.single.LocalBestPSO(
    n_particles=18,
    dimensions=6,
    bounds=bounds,
    options={'c1': 0.5, 'c2': 0.0, 'w':0.9},
#     options={'c1': 0.5, 'c2': 0.0, 'w':0.9, 'k': 2, 'p': 2},
)

def objective_f(xs, box_grid, part, voxel_size, grid_pos, grid_shape):
#     print(xs)
    if xs.size > 0:
        return np.apply_along_axis(eval_position, -1, xs, box_grid=box_grid, part=part, voxel_size=voxel_size, grid_pos=grid_pos, grid_shape=grid_shape)
    else:
        return np.apply_along_axis(eval_position, -1, np.array([0.,0.,0.,0.,0.,0.]), box_grid=box_grid, part=part, voxel_size=voxel_size, grid_pos=grid_pos, grid_shape=grid_shape)
    
%time cost, pos = optimizer.optimize(objective_f, iters=250, n_processes=6, box_grid=box_grid, part=_PointCloudTransmissionFormat(part), voxel_size=voxel_size, grid_pos=grid_pos, grid_shape=grid_shape)

plot_cost_history(cost_history=optimizer.cost_history)
plt.show()
%matplotlib widget

tx, ty, tz, rx, ry, rz = pos

part_ = o3d.geometry.PointCloud(part)

part_.translate(np.array([tx, ty, tz]))

part_.rotate(
    o3d.geometry.get_rotation_matrix_from_xyz(np.array([rx, ry, rz]))
)

part_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(part_, voxel_size)
part_grid = voxel2np(part_voxel, grid_pos, grid_shape, mode='floating')

ax = plt.figure().add_subplot(projection='3d')
ax.voxels(box_grid, facecolors='gray')
ax.voxels(part_grid, facecolors='red')

plt.show()
tx, ty, tz, rx, ry, rz = pos

part_ = o3d.geometry.PointCloud(part)

part_.translate(np.array([tx, ty, tz]))

part_.rotate(
    o3d.geometry.get_rotation_matrix_from_xyz(np.array([rx, ry, rz]))
)

o3d.visualization.draw([box, part_])<jupyter_output>[Open3D INFO] Window window_11 created.
[Open3D INFO] DataChannelObserver::OnStateChange label: ServerDataChannel, state: closing, peerid: 0.6636255717346637
[Open3D INFO] DataChannelObserver::OnStateChange label: ServerDataChannel, state: closed, peerid: 0.6636255717346637
[Open3D INFO] DataChannelObserver::OnStateChange label: ClientDataChannel, state: closing, peerid: 0.6636255717346637
[Open3D INFO] DataChannelObserver::OnStateChange label: ClientDataChannel, state: closed, peerid: 0.6636255717346637
[Open3D INFO] [Called HTTP API] /api/getMediaList
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] /api/getIceServers
[Open3D INFO] [Called HTTP API] [...]<jupyter_text>Dig part from box<jupyter_code>tx, ty, tz, rx, ry, rz = pos

part_ = o3d.geometry.PointCloud(part)

part_.translate(np.array([tx, ty, tz]))

part_.rotate(
    o3d.geometry.get_rotation_matrix_from_xyz(np.array([rx, ry, rz]))
)

part_voxel = o3d.geometry.VoxelGrid.create_from_point_cloud(part_, voxel_size)
%matplotlib widget

ax = plt.figure().add_subplot(projection='3d')
ax.voxels(box_grid, facecolors='gray')

plt.show()
%matplotlib widget

part_grid = voxel2np(part_voxel, grid_pos, grid_shape, mode='floating')

ax = plt.figure().add_subplot(projection='3d')
ax.voxels(box_grid, facecolors='gray')
ax.voxels(part_grid, facecolors='red')

plt.show()
%matplotlib widget

part_grid = voxel2np(part_voxel, grid_pos, grid_shape, mode='reverse-shadow')

digged_box_grid = box_grid & ~(box_grid & part_grid)

ax = plt.figure().add_subplot(projection='3d')
ax.voxels(digged_box_grid, facecolors='gray')

plt.show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.13371757925072045, 'fraction_numerical': 0.025244956772334293, 'mean_word_length': 5.963081861958266, 'pattern_counts': {'":': 0, '<': 28, '<?xml version=': 0, '>': 32, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '7', 'dir': 'jupyter-structured-clean-dedup', 'id': '645770', 'license': 'CNRI-Python', 'n_tokens_mistral': 3504, 'n_tokens_neox': 3357, 'n_words': 574, 'path': 'notebooks/03.01-bmp-registration-through-blackbox-opt.ipynb', 'repo_name': 'brunompacheco/part-counting', 'size': '8675'}