gnc_gm_pcl.py

# -*- coding: utf-8 -*-
"""GNC-GM_PCL.ipynb

Automatically generated by Colaboratory.

Original file is located at
    https://colab.research.google.com/drive/1QOKWSO6x_17b73MJHOVjHx5akVaDOJfH
"""

import numpy as np
from matplotlib import pyplot as plt
from scipy.optimize import minimize
import scipy.stats as stats
import random
import math
import time


# Weight Updation 
def weight_update(mu,res,barc2,N):
  weight=np.ones(N)
  for j in range(N):
    weight[j]=((mu*(barc2))/(res[j]+mu*(barc2)))**2
  return weight

def pcl_problem(N,outlier_percentage):
# Generate random data points
  source = np.random.rand(3, N)

# Apply arbitrary scale, translation and rotation
  scale = 1.5
  translation = np.array([[1], [1], [-1]])
  rotation = np.array([[ 0.47639458, -0.72295936, -0.50037783],
                      [ 0.76249023,  0.0563402,   0.64454203],
                      [-0.43778631, -0.68858953,  0.57808962]])
  dst = scale * np.matmul(rotation, source) + translation
  num_outliers = int(outlier_percentage / 100 * N)
  outlier_points = np.random.randint(0, N, num_outliers)
  dst[:,outlier_points]=+10

  return dst,source,num_outliers

def residuals(weights,dst,source,N):
  #noise set in datasets 
  Noiselimit=0.01                 
  w=weights
  w=np.sqrt(w)
  S_center = np.sum(source * weights.reshape(1, -1), axis=1) / np.sum(weights)
  D_center = np.sum(dst * weights.reshape(1, -1), axis=1) / np.sum(weights)

  weighted_mean_S = w.reshape(1, -1) * (source - S_center.reshape(-1, 1))
  weighted_mean_D = w.reshape(1, -1) * (dst - D_center.reshape(-1, 1))
  U, s, Vh = np.linalg.svd(weighted_mean_D @ weighted_mean_S.T)         # Singular value decomposition of the resulting covariance matrix
  R_ = U @ Vh
  S_=np.sum(s)/np.sum(np.square(weighted_mean_S))
  t_ = D_center - S_ * R_ @ S_center

  residues=dst-S_*np.matmul(R_,source)-np.expand_dims(t_,axis=1)
 
  residuals=np.sum(residues ** 2, axis=0)/Noiselimit
  return residuals,S_,R_,t_
  


def gnc(tgt,src,N):
  max_iteration=1000
  barc2=1           #as residuals already divided by noiselimit
  weights=np.ones(N)
  res,Scl0,Rot0,Trans0=residuals(weights,tgt,src,N)
  r=np.max(res)
  mu=2*r/barc2

  i=1
 
  while i<max_iteration and mu>1:
    weights=weight_update(mu,res,barc2,N)
    res,Scl,Rot,Trans=residuals(weights,tgt,src,N)
    mu=mu/1.4
    i=i+1
  return Scl,Rot,Trans,i

#Ground truth values of transformations

tgt,src,num_outliers=pcl_problem(1000,69)
start_time = time.time()
scale_tgt,Rotn,Trans_tgt,itr=gnc(tgt,src,1000)
end_time = time.time()
elapsed_time = end_time - start_time
print("Elapsed time_Gnc:", elapsed_time, "seconds")
print(scale_tgt,Rotn,Trans_tgt)