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]