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): '''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]