Demo on structure approximation from a set of 3D points
Useful in global Structure-from-Motion
- Install dependencies 1/2
- Download the dataset with initial structure
- Install dependencies 2/2
- Class that computes the covariance matrix over a pointset
- Class that generates virtual points
- Do 3D structure approximation
- Plot the initial and approximated structure
!pip install wget gdown
import os
from os.path import exists, join, basename, splitext
YOUR_PATH = '/content/'
!cd $YOUR_PATH
!pwd
#input_filename = 'init_structure'
dir_name = 'structure_approx'
# download dataset
# https://drive.google.com/uc?id=1_R4cFk8gbQE_fUSQ5Bcsfcfh2KLLS15w
dataset_url = 'https://drive.google.com/uc?id=12hOunzCbMB3fvcxD_W-165jxCaj3LxZn'
!gdown $dataset_url -O $dir_name'.tar.gz'
# unpack
if not exists(YOUR_PATH+dir_name):
!mkdir $YOUR_PATH$dir_name
!tar -xf $dir_name'.tar.gz' -C $YOUR_PATH$dir_name
%cd $YOUR_PATH$dir_name
import time
import argparse
import numpy as np
from numpy import linalg as la
from scipy import special as sp
from utils import mm3d_utils
from mpl_toolkits import mplot3d
%matplotlib inline
import matplotlib.pyplot as plt
import pandas as pd
import plotly.express as px
class Ellipse3D():
""" Class that calculates & stores an ellipsoid """
def __init__(self):
self.CoG = [0,0,0]
self.Sxx = 0.0
self.Syy = 0.0
self.Szz = 0.0
self.Sxy = 0.0
self.Sxz = 0.0
self.Syz = 0.0
self.Pds = 0
self.Norm = False
def Normalise(self):
if self.Norm == False:
self.Norm = True
Pds_ = self.Pds
self.CoG /= Pds_
CoG_ = self.CoG
self.Sxx = self.Sxx / Pds_ - CoG_[0]*CoG_[0]
self.Syy = self.Syy / Pds_ - CoG_[1]*CoG_[1]
self.Szz = self.Szz / Pds_ - CoG_[2]*CoG_[2]
self.Sxy = self.Sxy / Pds_ - CoG_[0]*CoG_[1]
self.Sxz = self.Sxz / Pds_ - CoG_[0]*CoG_[2]
self.Syz = self.Syz / Pds_ - CoG_[1]*CoG_[2]
def Add(self,Pt,Pds):
if self.Norm == False:
self.CoG += Pds*Pt
self.Sxx += Pds*Pt[0]*Pt[0]
self.Syy += Pds*Pt[1]*Pt[1]
self.Szz += Pds*Pt[2]*Pt[2]
self.Sxy += Pds*Pt[0]*Pt[1]
self.Sxz += Pds*Pt[0]*Pt[2]
self.Syz += Pds*Pt[1]*Pt[2]
self.Pds += Pds
def Show(self):
print('==== Ellipsoid Params ===== ')
print('Is normalised?:', self.Norm)
print('CoG:',self.CoG)
print('Coviariance matrix transposed:')
print(self.Sxx,"-","-")
print(self.Sxy,self.Syy,"-")
print(self.Sxz,self.Syz,self.Szz)
class GenPoints():
"""" Class that generates the visrtual points """
def __init__(self,anEl3D):
self.CoG = anEl3D.CoG
# update covariance matrix
CovMat = np.zeros([3,3],dtype=float)
CovMat[0,0] = anEl3D.Sxx
CovMat[1,1] = anEl3D.Syy
CovMat[2,2] = anEl3D.Szz
CovMat[0,1] = CovMat[1,0] = anEl3D.Sxy
CovMat[0,2] = CovMat[2,0] = anEl3D.Sxz
CovMat[1,2] = CovMat[2,1] = anEl3D.Syz
self.EigVal,self.EigVec = la.eig(CovMat)
self.EigVal = np.sqrt(self.EigVal)
self.EigVec = self.EigVec.transpose()
"""
_ X
/ Y
| Z
_____
P1 * /|
/ / |
/___ P2* |
| P4x | *P3 x - pt in the middle
| | / * - pts in the corners
P5*_____|/
"""
def Generate5Pts(self):
CorrFactor = np.sqrt(2.0)/(1-0.3333/(1.0+0.5))
#print("CorrFactor",CorrFactor)
e1E1 = self.EigVec[0,:] * (CorrFactor*self.EigVal[0])
e2E2 = self.EigVec[1,:] * (CorrFactor*self.EigVal[1])
e3E3 = self.EigVec[2,:] * (CorrFactor*self.EigVal[2])
h1 = [-1.0, 1.0, 1.0]
h2 = [1.0, -1.0, 1.0]
h3 = [1.0, 1.0, -1.0]
h4 = [0.0, 0.0, 0.0]
h5 = [-1.0,-1.0, -1.0]
# erfinv is a function of x in <-1,1> and it returns inf at the limits
# to avoid the 'inf', we make sure not to go too close to -1 or 1
Pt1 = self.CoG \
+ e1E1 * sp.erfinv(h1[0]*2.0/(2.0+1.0)) \
+ e2E2 * sp.erfinv(h1[1]*2.0/(2.0+1.0)) \
+ e3E3 * sp.erfinv(h1[2]*2.0/(2.0+1.0))
Pt2 = self.CoG \
+ e1E1 * sp.erfinv(h2[0]*2.0/(2.0+1.0)) \
+ e2E2 * sp.erfinv(h2[1]*2.0/(2.0+1.0)) \
+ e3E3 * sp.erfinv(h2[2]*2.0/(2.0+1.0))
Pt3 = self.CoG \
+ e1E1 * sp.erfinv(h3[0]*2.0/(2.0+1.0)) \
+ e2E2 * sp.erfinv(h3[1]*2.0/(2.0+1.0)) \
+ e3E3 * sp.erfinv(h3[2]*2.0/(2.0+1.0))
Pt4 = self.CoG \
+ e1E1 * sp.erfinv(h4[0]*2.0/(2.0+1.0)) \
+ e2E2 * sp.erfinv(h4[1]*2.0/(2.0+1.0)) \
+ e3E3 * sp.erfinv(h4[2]*2.0/(2.0+1.0))
Pt5 = self.CoG \
+ e1E1 * sp.erfinv(h5[0]*2.0/(2.0+1.0)) \
+ e2E2 * sp.erfinv(h5[1]*2.0/(2.0+1.0)) \
+ e3E3 * sp.erfinv(h5[2]*2.0/(2.0+1.0))
V5Pts = [Pt1,Pt2,Pt3,Pt4,Pt5]
return np.array(V5Pts,dtype=float)
def Show(self):
print("Eigenvalues:",self.EigVal)
print("Eigenvectors:\n",self.EigVec[0,:])
print(self.EigVec[1,:])
print(self.EigVec[2:])
input_filename = 'init_structure.txt'
assert( os.path.exists(input_filename) == True )
# Read the 3D points (i.e., the structure)
Pts3DVec = mm3d_utils.ReadPts(input_filename)
print("Nb of input 3D points: ",np.shape(Pts3DVec)[0])
# Initialise the ellipsoid class with the structure
El3D = Ellipse3D()
for i in Pts3DVec :
El3D.Add(i,1.0)
El3D.Normalise()
El3D.Show()
# Generate virtual points and save to file
GenVPts = GenPoints(El3D)
GenVPts.Show()
V5Pts = GenVPts.Generate5Pts()
print("Nb of virtual 3D points: ",np.shape(V5Pts)[0])
Pts3DVecCol = np.column_stack((Pts3DVec,np.ones(len(Pts3DVec))*len(Pts3DVec)))
V5PtsCol = np.column_stack((V5Pts,np.ones(len(V5Pts))*5))
# concatenate intial structure with the virtual structure
Pts3DV5PtsCol = np.vstack((Pts3DVecCol,V5PtsCol))
# transform to dataframe
df_Pts3D = pd.DataFrame(Pts3DV5PtsCol, columns = ['X','Y','Z','RGB'])
# convert the RGB to string for discrete colors
df_Pts3D["RGB"] = df_Pts3D["RGB"].astype(str)
# plot
fig = px.scatter_3d(df_Pts3D,x='X', y='Y', z='Z',color="RGB")
fig.show()