[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. train_loss.png

And this is a comparison of the transformation matrix output by optimization and the correct transformation matrix. test_acc.png

... 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

[Verification] Try to align the point cloud with the optimization function of pytorch Part 1
Try to specify the axis with PyTorch's Softmax function
Try function optimization with Optuna
Try to get the function list of Python> os package
Try to automate the operation of network devices with Python
Try to extract the features of the sensor data with CNN
I tried to display the point cloud data DB of Shizuoka prefecture with Vue + Leaflet
[Note] Let's try to predict the amount of electricity used! (Part 1)
Find the optimal value of a function with a genetic algorithm (Part 2)
Try to solve the N Queens problem with SA of PyQUBO
Try to solve the function minimization problem using particle swarm optimization
Align the size of the colorbar with matplotlib
How to crop the lower right part of the image with Python OpenCV
Try to image the elevation data of the Geographical Survey Institute with Python
Try to react only the carbon at the end of the chain with SMARTS
Try to separate the background and moving object of the video with OpenCV
Try to detect an object with Raspberry Pi ~ Part 1: Comparison of detection speed ~
Try installing only the core part of Ubuntu
Combinatorial optimization to find the hand of "Millijan"
Try to solve the man-machine chart with Python
How to try the friends-of-friends algorithm with pyfof
Change the decimal point of logging from, to.
Story of trying to use tensorboard with pytorch
Try to simulate the movement of the solar system
Various methods to numerically create the inverse function of a certain function Part 1 Polynomial regression
Try to create a battle record table with matplotlib from the data of "Schedule-kun"
[Django] Let's try to clarify the part of Django that was somehow through in the test
It's Christmas, so I'll try to draw the genealogy of Jesus Christ with Cabocha
[Cloud9] Try to build an environment with django 1.11 of Python 3.4 without understanding even 1 mm
I tried to learn the sin function with chainer
Try to solve the programming challenge book with python3
Add information to the bottom of the figure with Matplotlib
[Introduction to Python] How to iterate with the range function?
Try to solve the problems / problems of "Matrix Programmer" (Chapter 1)
Try to solve the internship assignment problem with Python
Try to estimate the number of likes on Twitter
[Neo4J] ④ Try to handle the graph structure with Cypher
Preparing the execution environment of PyTorch with Docker November 2019
I tried to erase the negative part of Meros
How to hit the document of Magic Function (Line Magic)
How to know the number of GPUs from python ~ Notes on using multiprocessing with pytorch ~
Try to import to the database by manipulating ShapeFile of national land numerical information with Python
Try to visualize the nutrients of corn flakes that M-1 champion Milkboy said with Python
Point Cloud with Pepper
[Python] Change the Cache-Control of the object uploaded to Cloud Storage
Try scraping the data of COVID-19 in Tokyo with Python
Try to evaluate the performance of machine learning / regression model
Acquisition of 3D point cloud with Softbank Pepper (Choregraphe, Python)
Try to play with the uprobe that supports Systemtap directly
Save the output of GAN one by one ~ With the implementation of GAN by PyTorch ~
I tried to find the average of the sequence with TensorFlow
Try to evaluate the performance of machine learning / classification model
Check the scope of local variables with the Python locals function.
I made a function to check the model of DCGAN
[Part.2] Crawling with Python! Click the web page to move!
Settings to debug the contents of the library with VS Code
Try to improve the accuracy of Twitter like number estimation
Try to solve the problems / problems of "Matrix Programmer" (Chapter 0 Functions)
How to run the Export function of GCP Datastore automatically
Attempt to automatically adjust the speed of time-lapse movies (Part 2)
I tried to fight the Local Minimum of Goldstein-Price Function