def similarityTransformation(pt1, pt2):
'''see runSimilarityTransformObj.py for detailed documentation'''
pt1V = opt.pt3Vector()
meas = opt.MeasurementCombinationsVector(0)
transV = opt.transformVector(1)
transV[0].CX = 0.0
transV[0].CY = 0.0
transV[0].CZ = 0.0
transV[0].q1 = 1.0
transV[0].q2 = 0.0
transV[0].q3 = 0.0
transV[0].q4 = 0.0
transV[0].s = 1.0
delta = numpy.mean(pt1, axis=0) - numpy.mean(pt2, axis=0)
#print delta
# transV[0].CX=-delta[0]
# transV[0].CY=-delta[1]
# transV[0].CZ=-delta[2]
for i, p1, p2 in zip(range(pt1.shape[0]), pt1, pt2):
print p1, p2
p1a = opt.pt3_t()
p1a.X = p1[0]
p1a.Y = p1[1]
p1a.Z = p1[2]
pt1V.push_back(p1a)
p2a = opt.pt3_t()
p2a.X = p2[0]
p2a.Y = p2[1]
p2a.Z = p2[2]
pt1V.push_back(p2a)
m = opt.MeasurementCombinations_t()
m.type = opt.eSimilarityTransformation
m.v.push_back(2 * i)
m.v.push_back(2 * i + 1)
m.v.push_back(0)
meas.push_back(m)
res = opt.doubleVector()
m = opt.MeasurementCombinations_t()
m.type = opt.eQuat
m.v.push_back(0)
meas.push_back(m)
opt.minimize_simtrans(pt1V, transV, meas, 1000, res)
print "Residual", res[res.size() - 1]
return numpy.array([
transV[0].CX, transV[0].CY, transV[0].CZ, transV[0].q1, transV[0].q2,
transV[0].q3, transV[0].q4, transV[0].s
]), res[res.size() - 1]
def similarityTransformation(pt1,pt2):
'''see runSimilarityTransformObj.py for detailed documentation'''
pt1V=opt.pt3Vector()
meas=opt.MeasurementCombinationsVector(0)
transV=opt.transformVector(1)
transV[0].CX=0.0
transV[0].CY=0.0
transV[0].CZ=0.0
transV[0].q1=1.0
transV[0].q2=0.0
transV[0].q3=0.0
transV[0].q4=0.0
transV[0].s=1.0
delta=numpy.mean(pt1,axis=0)-numpy.mean(pt2,axis=0)
#print delta
# transV[0].CX=-delta[0]
# transV[0].CY=-delta[1]
# transV[0].CZ=-delta[2]
for i,p1,p2 in zip(list(range(pt1.shape[0])),pt1,pt2):
print(p1,p2)
p1a=opt.pt3_t()
p1a.X=p1[0]
p1a.Y=p1[1]
p1a.Z=p1[2]
pt1V.push_back(p1a)
p2a=opt.pt3_t()
p2a.X=p2[0]
p2a.Y=p2[1]
p2a.Z=p2[2]
pt1V.push_back(p2a)
m=opt.MeasurementCombinations_t()
m.type=opt.eSimilarityTransformation
m.v.push_back(2*i)
m.v.push_back(2*i+1)
m.v.push_back(0)
meas.push_back(m)
res=opt.doubleVector()
m=opt.MeasurementCombinations_t()
m.type=opt.eQuat
m.v.push_back(0)
meas.push_back(m)
opt.minimize_simtrans(pt1V,transV,meas,1000,res)
print("Residual",res[res.size()-1])
return numpy.array([transV[0].CX,transV[0].CY,transV[0].CZ,transV[0].q1,transV[0].q2,transV[0].q3,transV[0].q4,transV[0].s]),res[res.size()-1]
def similarityTransformation(pt1, pt2):
'''code example which shows how to use the optimization in an object oriented sense'''
min = opt.MinimizerSimTrans()
'''verbose=0 : no print statements; verbose=1 : initial and final values; verbose=2 : residual in each iteration'''
min.verbose = 1
'''termination values of the levenberg marquardt algorithm'''
min.eps1 = 1e-10
min.eps2 = 1e-10
'''maximum number of iterations'''
min.kmax = 100
min.cg_it = 150
min.cg_thresh_abs = 1e-12
'''vector of the 3D points struct, to fill the values'''
pt1V = min.pt3
'''vector which contains all measurement combination'''
meas = min.measurementCombinations
'''create a vector which contains transformation structs with one element'''
min.transform = opt.transformVector(1)
transV = min.transform
transV[0].CX = 0.0
transV[0].CY = 0.0
transV[0].CZ = 0.0
transV[0].q1 = 1.0
transV[0].q2 = 0.0
transV[0].q3 = 0.0
transV[0].q4 = 0.0
transV[0].s = 1.0
'''can be enabled to have a better initial estimate'''
#delta=numpy.mean(pt1,axis=0)-numpy.mean(pt2,axis=0)
#print delta
#transV[0].CX=-delta[0]
#transV[0].CY=-delta[1]
#transV[0].CZ=-delta[2]
'''insert the points in the struct and generate a measurement struct for each point pair'''
for i, p1, p2 in zip(range(pt1.shape[0]), pt1, pt2):
print p1, p2
p1a = opt.pt3_t()
p1a.X = p1[0]
p1a.Y = p1[1]
p1a.Z = p1[2]
pt1V.push_back(p1a)
p2a = opt.pt3_t()
p2a.X = p2[0]
p2a.Y = p2[1]
p2a.Z = p2[2]
pt1V.push_back(p2a)
m = opt.MeasurementCombinations_t()
m.type = opt.eSimilarityTransformation
#m.robust=opt.OF_SORT_WEIGHT
#m.robust_para_a=1
m.v.push_back(2 * i)
m.v.push_back(2 * i + 1)
m.v.push_back(0)
meas.push_back(m)
'''inclue a measurement struct for the Quaternion'''
m = opt.MeasurementCombinations_t()
m.type = opt.eQuat
'''the value corresponds to the index in the corresponding vector'''
m.v.push_back(0)
'''a inverse covariance can be set for each error/cost function'''
#m.inv_cov.push_back(10);
meas.push_back(m)
min.verbose = 0
min.kmax = 100
min.run()
res = min.residuals
print "Residuals per function:"
'''each measurement combination contains its final residual'''
for m in meas:
i = 0
for r in m.res:
i += 1
print i, ":", r
print "Residual", res[res.size() - 1]
return numpy.array([
transV[0].CX, transV[0].CY, transV[0].CZ, transV[0].q1, transV[0].q2,
transV[0].q3, transV[0].q4, transV[0].s
]), res[res.size() - 1]
def similarityTransformation(pt1,pt2):
'''code example which shows how to use the optimization in an object oriented sense'''
min=opt.MinimizerSimTrans()
'''verbose=0 : no print statements; verbose=1 : initial and final values; verbose=2 : residual in each iteration'''
min.verbose=1
'''termination values of the levenberg marquardt algorithm'''
min.eps1=1e-10
min.eps2=1e-10
'''maximum number of iterations'''
min.kmax=100
min.cg_it=150
min.cg_thresh_abs=1e-12
'''vector of the 3D points struct, to fill the values'''
pt1V=min.pt3
'''vector which contains all measurement combination'''
meas=min.measurementCombinations
'''create a vector which contains transformation structs with one element'''
min.transform=opt.transformVector(1)
transV=min.transform
transV[0].CX=0.0
transV[0].CY=0.0
transV[0].CZ=0.0
transV[0].q1=1.0
transV[0].q2=0.0
transV[0].q3=0.0
transV[0].q4=0.0
transV[0].s=1.0
'''can be enabled to have a better initial estimate'''
#delta=numpy.mean(pt1,axis=0)-numpy.mean(pt2,axis=0)
#print delta
#transV[0].CX=-delta[0]
#transV[0].CY=-delta[1]
#transV[0].CZ=-delta[2]
'''insert the points in the struct and generate a measurement struct for each point pair'''
for i,p1,p2 in zip(range(pt1.shape[0]),pt1,pt2):
print p1,p2
p1a=opt.pt3_t()
p1a.X=p1[0]
p1a.Y=p1[1]
p1a.Z=p1[2]
pt1V.push_back(p1a)
p2a=opt.pt3_t()
p2a.X=p2[0]
p2a.Y=p2[1]
p2a.Z=p2[2]
pt1V.push_back(p2a)
m=opt.MeasurementCombinations_t()
m.type=opt.eSimilarityTransformation
#m.robust=opt.OF_SORT_WEIGHT
#m.robust_para_a=1
m.v.push_back(2*i)
m.v.push_back(2*i+1)
m.v.push_back(0)
meas.push_back(m)
'''inclue a measurement struct for the Quaternion'''
m=opt.MeasurementCombinations_t()
m.type=opt.eQuat
'''the value corresponds to the index in the corresponding vector'''
m.v.push_back(0)
'''a inverse covariance can be set for each error/cost function'''
#m.inv_cov.push_back(10);
meas.push_back(m)
min.verbose=0
min.kmax=100
min.run()
res=min.residuals
print "Residuals per function:"
'''each measurement combination contains its final residual'''
for m in meas:
i=0
for r in m.res:
i+=1
print i,":", r
print "Residual",res[res.size()-1]
return numpy.array([transV[0].CX,transV[0].CY,transV[0].CZ,transV[0].q1,transV[0].q2,transV[0].q3,transV[0].q4,transV[0].s]),res[res.size()-1]
