registration _example.py - few comments that might help you

[yoavlevy]

I run https://github.com/UMich-CURLY-teaching/UMich-ROB-530-public/tree/main/code-examples/Python/gicp/registration _example.py

Few comments that might help you:

• one of the algorithms work well. the other one is not
  T = gicp_SE3(np.asarray(fixed.points), np.asarray(moving.points)) - works well
  #T = gicp_Sim3(np.asarray(fixed.points), np.asarray(moving.points)) - gives wrong T. both in scale and in translation

• I am not sure the transform was to the right direction in the original code. anyway, the following works well:
  a = copy.deepcopy(ptCloudRef)
  b = copy.deepcopy(ptCloudCurrent).transform(T)
  o3d.visualization.draw_geometries([a, b])

• better use deep copy - otherwise things are changing unexpectedly.

• I had to change the way the code loads the data:
  dataset = o3d.data.LivingRoomPointClouds()
  pcds = []
  for pcd_path in dataset.paths:
  pcds.append(o3d.io.read_point_cloud(pcd_path))

  o3d.visualization.draw(pcds[0])

  ptCloudRef = pcds[0] #o3d.io.read_point_cloud("ptCloud/livingRoomData1.ply")
  ptCloudCurrent = pcds[1] #o3d.io.read_point_cloud("ptCloud/livingRoomData2.ply")

• at the end of:
  def compute_jacobian(X, p_target, p_source, Ct, Cs, target_idx, survived_idx):
  I had to squeeze the b:
  return A, np.squeeze(b)

disclimer - so far I run only the following part I from the file registration_example.py
#!usr/bin/env python
import copy
from time import sleep

3-D Point Cloud Registration and Stitching example of GICP

Author: Fangtong Liu

Date: 06/03/2020

import numpy as np
import open3d as o3d

from open3d.cpu.pybind.data import LivingRoomPointClouds
import threading
from gicp_SE3 import gicp_SE3
from gicp_Sim3 import gicp_Sim3
from scipy.io import loadmat
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from tqdm import tqdm

if name == "main":
dataset = o3d.data.LivingRoomPointClouds()
pcds = []
for pcd_path in dataset.paths:
pcds.append(o3d.io.read_point_cloud(pcd_path))
# o3d.visualization.draw(pcds[0])

ptCloudRef = pcds[0] #o3d.io.read_point_cloud("ptCloud/livingRoomData1.ply")
ptCloudCurrent = pcds[1] #o3d.io.read_point_cloud("ptCloud/livingRoomData2.ply")

gridSize = 0.05
fixed = ptCloudRef.voxel_down_sample(voxel_size=gridSize)
moving = ptCloudCurrent.voxel_down_sample(voxel_size=gridSize)

# points = np.asarray(fixed.points)
# num_points_to_keep =  len(moving.points)
# random_indices = np.random.choice(len(points), num_points_to_keep, replace=False)
# fixed = o3d.geometry.PointCloud()
# fixed.points = o3d.utility.Vector3dVector(points[random_indices])

################# solve for tf ##########################
T = gicp_SE3(np.asarray(fixed.points), np.asarray(moving.points))
#T = gicp_Sim3(np.asarray(fixed.points), np.asarray(moving.points))
accumTform = np.copy(T)
print(T)
ptCloudAligned = copy.deepcopy(ptCloudCurrent).transform(T)

ptCloudScene = o3d.geometry.PointCloud()
ptCloudScene += ptCloudAligned
ptCloudScene += ptCloudRef


a = copy.deepcopy(ptCloudRef)
b = copy.deepcopy(ptCloudCurrent).transform(T)

o3d.visualization.draw_geometries([a, b])

[yoavlevy]

sorry for the formatting that was added automatically... I submitted a plane text