[PYTHON] [Verification] Try to align the point cloud with the optimization function of pytorch Part 1
Introduction
I recently learned pytorch so I'll play with it.
All you have to do is align the point cloud.
For the purpose, I personally have the image that icp tends to fall into a local solution, Isn't pytorch's state-of-the-art optimization function going pretty good? I will try it with the fluffy expectation.
The flow is as follows.
for Each point in point cloud B
1.Transformation matrix$P = [R|t]$To define.
Alignment is performed by optimizing this parameter.
2.Calculate the nearest neighbor of the point cloud and get the set of the closest points.
3.Transformation matrix$P$The points of point cloud B to which
The points closest to the point cloud A, these two, are evaluated by the loss function.
4.Optimization process
Optimized transformation matrix$P$Apply and run from 1 again
Verify with the following flow.
1.Prepare two same point clouds and only move (adjust 3D parameters)
2.Prepare two same point clouds and perform only rotation (adjustment of 9-dimensional parameters)
3.Prepare two same point clouds and rotate / move (12-dimensional parameter adjustment)
3.Prepare two different point clouds and rotate / move (12-dimensional parameter adjustment)
3.Add noise to the other side, prepare two different point clouds, and rotate / move (12-dimensional parameter adjustment)
result
I wrote that, but I failed at the first stage. I'm not used to pytorch yet, so maybe something is missing.
tetst.py
import copy
import numpy as np
import open3d as o3d
import random
import math
import torch.nn as nn
import torch.nn.functional as F
import torch
import torch.optim as optim
import matplotlib.pyplot as plt
epoch = 1000
def getPLYfromNumpy(nplist):
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(nplist)
return pcd
def write_point_cloud(path, pcl):
assert o3d.io.write_point_cloud(path, pcl), "write pcl error : " + path
def read_point_cloud(path):
pcl = o3d.io.read_point_cloud(path)
if len(pcl.points) == 0: assert False, "load pcl error : " + path
return pcl
def Register_EachPoint_RT(pclA, pclB,testP,criterion,optimizer):
history = {
'train_loss': [],
'test_acc': [],
}
transP = torch.tensor(
[[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]],
requires_grad=True)
params = [transP]
optimizer = optimizer(params)
kd_tree_A = o3d.geometry.KDTreeFlann(pclA)
cnt = 0
#Try it in points
for j in range(epoch):
for didx in range(len(pclB.points)):
cnt += 1
optimizer.zero_grad()
#Nearest neighbor calculation
[_, Aidx1, _] = kd_tree_A.search_knn_vector_3d(pclB.points[didx], 1)
ptA_sample = pclA.points[Aidx1[0]]
ptB_sample = pclB.points[didx]
#Homogeneous coordinates
ptA_sample = np.array([ptA_sample[0], ptA_sample[1], ptA_sample[2], 1])
ptB_sample = np.array([ptB_sample[0], ptB_sample[1], ptB_sample[2], 1])
ptA_sample = ptA_sample.reshape(4, 1)
ptB_sample = ptB_sample.reshape(4, 1)
A_tor = torch.tensor(ptA_sample.tolist(), requires_grad=False)
B_tor = torch.tensor(ptB_sample.tolist(), requires_grad=False)
answer = A_tor
output = torch.mm(transP, B_tor)
loss = criterion(answer, output)
loss.backward()
optimizer.step()
# print( j, cnt, " :error= ", loss.item(),"\n",transP)
ls = np.linalg.norm(testP - transP.to('cpu').detach().numpy().copy())
history['train_loss'].append(loss)
history['test_acc'].append(ls)
print(" :error= ", loss.item(),"\t Error with the correct transformation matrix= ",ls)
plt.figure()
plt.plot(range(1, cnt + 1), history['train_loss'], label='train_loss')
plt.xlabel('train_loss')
plt.legend()
plt.savefig('train_loss.png')
plt.figure()
plt.plot(range(1, cnt + 1), history['test_acc'], label='test_acc')
plt.xlabel('test_acc')
plt.legend()
plt.savefig('test_acc.png')
return transP
def Register_EachPoint_T(pclA, pclB,testP,criterion,optimizer):
history = {
'train_loss': [],
'test_acc': [],
}
transP = torch.tensor([[0.0], [0.0], [0.0]],requires_grad=True)
params = [transP]
optimizer = optimizer(params)
kd_tree_A = o3d.geometry.KDTreeFlann(pclA)
cnt = 0
#Try it in points
for j in range(epoch):
for didx in range(len(pclB.points)):
cnt += 1
optimizer.zero_grad()
#Get the point of point cloud A closest to each point of point cloud B
[_, Aidx1, _] = kd_tree_A.search_knn_vector_3d(pclB.points[didx], 1)
ptA_sample = pclA.points[Aidx1[0]]
ptB_sample = pclB.points[didx]
#Homogeneous coordinates
ptA_sample = np.array([ptA_sample[0], ptA_sample[1], ptA_sample[2]])
ptB_sample = np.array([ptB_sample[0], ptB_sample[1], ptB_sample[2]])
ptA_sample = ptA_sample.reshape(3, 1)
ptB_sample = ptB_sample.reshape(3, 1)
#Convert each point to Tensor
A_tor = torch.tensor(ptA_sample.tolist(), requires_grad=False)
B_tor = torch.tensor(ptB_sample.tolist(), requires_grad=False)
#Adjust point cloud B to match point cloud A.
answer = A_tor
output = (B_tor + transP)
#Loss calculation->optimisation
loss = criterion(answer, output)
loss.backward()
optimizer.step()
#Comparison with the correct transformation matrix. (0 is desirable)
ls = np.linalg.norm(testP - transP.to('cpu').detach().numpy().copy())
history['train_loss'].append(loss)
history['test_acc'].append(ls)
print(" :error= ", loss.item(), "\t Error with the correct transformation matrix= ", ls)
#Reflect the adjustment result->Nearest neighbor calculation again in the next loop
nptransP = transP.to('cpu').detach().numpy().copy().reshape(1,3)
pclB = getPLYfromNumpy(pclB.points + nptransP)
plt.figure()
plt.plot(range(1, cnt + 1), history['train_loss'], label='train_loss')
plt.xlabel('train_loss')
plt.legend()
plt.savefig('train_loss.png')
plt.figure()
plt.plot(range(1, cnt + 1), history['test_acc'], label='test_acc')
plt.xlabel('test_acc')
plt.legend()
plt.savefig('test_acc.png')
return transP
POINT_NUM = 1024
# http://graphics.stanford.edu/data/3Dscanrep/
pclA = read_point_cloud("bun000.ply")
A = np.array(pclA.points)
A = np.array(random.sample(A.tolist(), POINT_NUM))
#A point group with a slightly higher difficulty level. Maybe this can't be done yet ...
# pclB = read_point_cloud("bun045.ply")
# B = np.array(pclB.points)
# B = np.array(random.sample(B.tolist(), POINT_NUM))
# #Add noise
# B += np.random.randn(POINT_NUM, 3) * 0.005
# #Granting unordered point cloud
# np.random.shuffle(B)
# pclB_sample = getPLYfromNumpy(B)
pclA_sample = getPLYfromNumpy(A)
T_Projection = np.array([[1, 0, 0, 0.5],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
T_translation = np.array([[T_Projection[0][3]], [T_Projection[1][3]], [T_Projection[2][3]]])
pclA_trans_sample = getPLYfromNumpy(A).transform(T_Projection)
write_point_cloud("A_before.ply", pclA_sample)
write_point_cloud("A_rot_before.ply", pclA_trans_sample)
def testEstimateT(pclA_sample,pclA_trans_sample,T_translation):
optimizer = optim.Adam
# MSELoss
transP = Register_EachPoint_T(pclA_sample, pclA_trans_sample, T_translation, nn.MSELoss(),optimizer)
T_res = np.array([[1, 0, 0, transP[0]],
[0, 1, 0, transP[1]],
[0, 0, 1, transP[2]],
[0, 0, 0, 1]])
pclA_res = copy.copy(pclA_trans_sample)
pclA_res = pclA_res.transform(T_res)
write_point_cloud("TOnlytest_A_rot_after_MSELoss.ply", pclA_res)
# # L1Loss
# transP = Register_EachPoint_T(pclA_sample, pclA_trans_sample, T_translation, nn.L1Loss(),optimizer)
# T_res = np.array([[1, 0, 0, transP[0]],
# [0, 1, 0, transP[1]],
# [0, 0, 1, transP[2]],
# [0, 0, 0, 1]])
# pclA_res = copy.copy(pclA_trans_sample)
# pclA_res = pclA_res.transform(T_res)
# write_point_cloud("TOnlytest_A_rot_after_L1Loss.ply", pclA_res)
def testEstimateRT(pclA_sample,pclA_trans_sample,T_Projection):
optimizer = optim.Adam
# MSELoss
transP = Register_EachPoint_RT(pclA_sample, pclA_trans_sample, T_Projection, nn.MSELoss(),optimizer)
transP = transP.to('cpu').detach().numpy().copy()
pclA_res = copy.copy(pclA_trans_sample)
pclA_res = pclA_res.transform(transP)
write_point_cloud("RTtest_A_rot_after_MSELoss.ply", pclA_res)
testEstimateT(pclA_sample, pclA_trans_sample, T_translation)
# testEstimateRT(pclA_sample, pclA_trans_sample, T_Projection)
exit()
Let's see the output result of the loss function.
Looking at this, it seems that it converges to 0 in a blink of an eye.
And this is a comparison of the transformation matrix output by optimization and the correct transformation matrix.
... I think I've dropped a little at first, but I'm hungry all the time. It is a large error of 0.5. I also output a point cloud, but I will omit it because it was only a little closer.
Summary
There are no outliers and there are only 3 dimensions, but why? I feel like I'm simply making a mistake in using pytorch. If you know, please contact me.
Next, we will study PointNet LK, which is an extension of PointNet's T-net. https://github.com/wentaoyuan/it-net https://www.slideshare.net/naoyachiba18/pointnetlk-robust-efficient-point-cloud-registration-using-pointnet-167874587
Recommended Posts