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])

Hi,

First of all, thanks for sharing those well-prepared materials as open-source, I highly benefitted from them.

I just wanted to offer a correction to slides/07_Matrix_Lie_Groups_note.pdf.
In the first page, wedge notation should be written as
wx = [0, -w(3), w(2); w(3), 0, -w(1); -w(2), w(1), 0]
In the file (and also in the video), it's written as -wx.
Also, the matrices of the basis (G1, G2, G3) need to be written accordingly.
In the current form [G1, G2]=-G3 instead of G3.

Thank you,
Best regards

Hi,
it seems like some links in pose_sync.m (in LeftJacobian_SO3/SE3) are pointing to a hacked website...

Hi! First of all, thank you for providing these invaluable materials. :)

I've studied these materials to fully understand invariant EKF, but there's a question about the Adjoint matrix in matrix_groups/odometry_propagation_se2.m and matrix_groups/odometry_propagation_se3.m.

In matrix_groups/odometry_propagation_se2.m, adjoint matrix is defined as:

% SE(2) Adjoint
robot.Ad = @(X) [X(1:2,1:2), [X(2,3); -X(1,3)]; 0 0 1];

and in matrix_groups/odometry_propagation_se3.m, the adjoint matrix is defined as:

% SE(3) Adjoint
robot.Ad = @(X) [X(1:3,1:3), skew(X(1:3,4))*X(1:3,1:3); zeros(3), X(1:3,1:3)];

The point is that the forms of adjoints are different from the one explained in Lecture 08, i.e. [R 0; P^R R].
(1:44:30 @ https://www.youtube.com/watch?v=k2miimTn6rk)

Could you explain why these adjoints have a different forms? Thank you in advance!