def testInitPose(self): bsp = bsplines.BSplinePose(4,sm.RotationVector()) # Create two random transformations. T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6)) T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6)) # Initialize the curve. bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1) # Check the values. self.assertEqual(bsp.t_min(),0.0); self.assertEqual(bsp.t_max(),1.0); curve_T_n_0 = bsp.transformation(0.0); self.assertMatricesEqual(T_n_0, curve_T_n_0, 1e-9,"T_n_0") curve_T_n_1 = bsp.transformation(1.0); self.assertMatricesEqual(T_n_1, curve_T_n_1, 1e-9,"T_n_1") tend = 2.0 # Extend the segment. T_n_25 = bsp.curveValueToTransformation(numpy.random.random(6)) bsp.addPoseSegment(tend, T_n_25); # Check the values. self.assertEqual(bsp.t_min(),0.0); self.assertEqual(bsp.t_max(),tend); curve_T_n_0 = bsp.transformation(0.0); self.assertMatricesEqual(T_n_0, curve_T_n_0, 1e-6, "T_n_0") curve_T_n_1 = bsp.transformation(1.0); self.assertMatricesEqual(T_n_1, curve_T_n_1, 1e-4, "T_n_1") curve_T_n_25 = bsp.transformation(tend); self.assertMatricesEqual(T_n_25, curve_T_n_25, 1e-4, "T_n_25")
def __generateInitialSpline(self, splineOrder, timeOffsetPadding, numberOfKnots = None, framerate = None): poseSpline = bsplines.BSplinePose(splineOrder, sm.RotationVector()) # Get the observation times. times = np.array([observation.time().toSec() for observation in self.__observations ]) # get the pose values of the initial transformations at observation time curve = np.matrix([ poseSpline.transformationToCurveValue( observation.T_t_c().T() ) for observation in self.__observations]).T # make sure all values are well defined if np.isnan(curve).any(): raise RuntimeError("Nans in curve values") sys.exit(0) # Add 2 seconds on either end to allow the spline to slide during optimization times = np.hstack((times[0] - (timeOffsetPadding * 2.0), times, times[-1] + (timeOffsetPadding * 2.0))) curve = np.hstack((curve[:,0], curve, curve[:,-1])) self.__ensureContinuousRotationVectors(curve) seconds = times[-1] - times[0] # fixed number of knots if (numberOfKnots is not None): knots = numberOfKnots # otherwise with framerate estimate else: knots = int(round(seconds * framerate/3)) print print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds) poseSpline.initPoseSplineSparse(times, curve, knots, 1e-4) return poseSpline
def testCurveToTransformation(self): rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez()) for r in rvs: bsp = bsplines.BSplinePose(4,r) # Build a random, valid transformation. T1 = bsp.curveValueToTransformation(numpy.random.random(6)) p = bsp.transformationToCurveValue(T1) T2 = bsp.curveValueToTransformation(p) self.assertMatricesEqual(T1, T2, 1e-9,"Checking the invertiblity of the transformation to curve values:")
def initPoseSplineFromCamera(self, splineOrder=6, poseKnotsPerSecond=100, timeOffsetPadding=0.02): T_c_b = self.T_extrinsic.T() pose = bsplines.BSplinePose(splineOrder, sm.RotationVector()) # Get the checkerboard times. times = np.array([ obs.time().toSec() + self.timeshiftCamToImuPrior for obs in self.targetObservations ]) curve = np.matrix([ pose.transformationToCurveValue(np.dot(obs.T_t_c().T(), T_c_b)) for obs in self.targetObservations ]).T if np.isnan(curve).any(): raise RuntimeError("Nans in curve values") sys.exit(0) # Add 2 seconds on either end to allow the spline to slide during optimization times = np.hstack((times[0] - (timeOffsetPadding * 2.0), times, times[-1] + (timeOffsetPadding * 2.0))) curve = np.hstack((curve[:, 0], curve, curve[:, -1])) # Make sure the rotation vector doesn't flip for i in range(1, curve.shape[1]): previousRotationVector = curve[3:6, i - 1] r = curve[3:6, i] angle = np.linalg.norm(r) axis = r / angle best_r = r best_dist = np.linalg.norm(best_r - previousRotationVector) for s in range(-3, 4): aa = axis * (angle + math.pi * 2.0 * s) dist = np.linalg.norm(aa - previousRotationVector) if dist < best_dist: best_r = aa best_dist = dist curve[3:6, i] = best_r seconds = times[-1] - times[0] knots = int(round(seconds * poseKnotsPerSecond)) print print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, poseKnotsPerSecond, seconds) pose.initPoseSplineSparse(times, curve, knots, 1e-4) return pose
def testInversePose2(self): rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez()) for r in rvs: bsp = bsplines.BSplinePose(4,r) # Create two random transformations. T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6)) T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6)) # Initialize the curve. bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1) for t in numpy.arange(0.0,1.0,0.1): T = bsp.transformation(t) invT,J,C = bsp.inverseTransformationAndJacobian(t) one = numpy.dot(T,invT) self.assertMatricesEqual(one,numpy.eye(4),1e-14,"T * inv(T)")
def __init__(self, order=4, target_point=[0., 0, 0], ctrl_point_num=10, time=10., random_range=[[0, 0, 0.], [1., 1., 1.]]): self.target_point = np.array(target_point) self.bsp = bsplines.BSplinePose(4, sm.RotationVector()) self.ctrl_point_num = ctrl_point_num random_range = np.array(random_range) + self.target_point self.curve = self.TargetOrientedPose(ctrl_point_num, random_range) self.time = time times = np.linspace(0, time, ctrl_point_num) curve_in = np.array(self.curve) curve_in[3:] *= -1 self.bsp.initPoseSplineSparse(times, curve_in, int(ctrl_point_num * 3.), 1e-5)
def testInverseOrientationJacobian(self): rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez()) for r in rvs: for order in range(2,7): bsp = bsplines.BSplinePose(order,r) T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6)) T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6)) # Initialize the curve. bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1) for t in numpy.linspace(bsp.t_min(), bsp.t_max(), 4): # Create a random homogeneous vector v = numpy.random.random(3) CJI = bsp.inverseOrientationAndJacobian(t); #print "TJI: %s" % (TJI) je = nd.Jacobian(lambda c: bsp.setLocalCoefficientVector(t,c) or numpy.dot(bsp.inverseOrientation(t), v)) estJ = je(bsp.localCoefficientVector(t)) JT = CJI[1] J = numpy.dot(sm.crossMx(numpy.dot(CJI[0],v)), JT) self.assertMatricesEqual(J, estJ, 1e-8,"C_n_0")
def stereoCalibrate(camL_geometry, camH_geometry, obslist, distortionActive=False, baseline=None): ##################################################### ## find initial guess as median of all pnp solutions ##################################################### if baseline is None: r = [] t = [] for obsL, obsH in obslist: #if we have observations for both camss if obsL is not None and obsH is not None: success, T_L = camL_geometry.geometry.estimateTransformation( obsL) success, T_H = camH_geometry.geometry.estimateTransformation( obsH) baseline = T_H.inverse() * T_L t.append(baseline.t()) rv = sm.RotationVector() r.append(rv.rotationMatrixToParameters(baseline.C())) r_median = np.median(np.asmatrix(r), axis=0).flatten().T R_median = rv.parametersToRotationMatrix(r_median) t_median = np.median(np.asmatrix(t), axis=0).flatten().T baseline_HL = sm.Transformation(sm.rt2Transform(R_median, t_median)) else: baseline_HL = baseline #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: dL = camL_geometry.geometry.projection().distortion().getParameters( ).flatten() pL = camL_geometry.geometry.projection().getParameters().flatten() dH = camH_geometry.geometry.projection().distortion().getParameters( ).flatten() pH = camH_geometry.geometry.projection().getParameters().flatten() sm.logDebug("initial guess for stereo calib: {0}".format( baseline_HL.T())) sm.logDebug("initial guess for intrinsics camL: {0}".format(pL)) sm.logDebug("initial guess for intrinsics camH: {0}".format(pH)) sm.logDebug("initial guess for distortion camL: {0}".format(dL)) sm.logDebug("initial guess for distortion camH: {0}".format(dH)) ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #baseline design variable baseline_dv = addPoseDesignVariable(problem, baseline_HL) #target pose dv for all target views (=T_camL_w) target_pose_dvs = list() for obsL, obsH in obslist: if obsL is not None: #use camL if we have an obs for this one success, T_t_cL = camL_geometry.geometry.estimateTransformation( obsL) else: success, T_t_cH = camH_geometry.geometry.estimateTransformation( obsH) T_t_cL = T_t_cH * baseline_HL #apply baseline for the second camera target_pose_dv = addPoseDesignVariable(problem, T_t_cL) target_pose_dvs.append(target_pose_dv) #add camera dvs camL_geometry.setDvActiveStatus(True, distortionActive, False) camH_geometry.setDvActiveStatus(True, distortionActive, False) problem.addDesignVariable(camL_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(camL_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(camL_geometry.dv.shutterDesignVariable()) problem.addDesignVariable(camH_geometry.dv.distortionDesignVariable()) problem.addDesignVariable(camH_geometry.dv.projectionDesignVariable()) problem.addDesignVariable(camH_geometry.dv.shutterDesignVariable()) ############################################ ## add error terms ############################################ #corner uncertainty # \todo pass in the detector uncertainty somehow. cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #Add reprojection error terms for both cameras reprojectionErrors0 = [] reprojectionErrors1 = [] for cidx, cam in enumerate([camL_geometry, camH_geometry]): sm.logDebug( "stereoCalibration: adding camera error terms for {0} calibration targets" .format(len(obslist))) #get the image and target points corresponding to the frame target = cam.ctarget.detector.target() #add error terms for all observations for view_id, obstuple in enumerate(obslist): #add error terms if we have an observation for this cam obs = obstuple[cidx] if obs is not None: T_cam_w = target_pose_dvs[view_id].toExpression().inverse() #add the baseline for the second camera if cidx != 0: T_cam_w = baseline_dv.toExpression() * T_cam_w for i in range(0, target.size()): p_target = aopt.HomogeneousExpression( sm.toHomogeneous(target.point(i))) valid, y = obs.imagePoint(i) if valid: # Create an error term. rerr = cam.model.reprojectionError( y, invR, T_cam_w * p_target, cam.dv) rerr.idx = i problem.addErrorTerm(rerr) if cidx == 0: reprojectionErrors0.append(rerr) else: reprojectionErrors1.append(rerr) sm.logDebug("stereoCalibrate: added {0} camera error terms".format( len(reprojectionErrors0) + len(reprojectionErrors1))) ############################################ ## solve ############################################ options = aopt.Optimizer2Options() options.verbose = True if sm.getLoggingLevel( ) == sm.LoggingLevel.Debug else False options.nThreads = 4 options.convergenceDeltaX = 1e-3 options.convergenceDeltaJ = 1 options.maxIterations = 200 options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10) optimizer = aopt.Optimizer2(options) optimizer.setProblem(problem) #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: sm.logDebug("Before optimization:") e2 = np.array([e.evaluateError() for e in reprojectionErrors0]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) e2 = np.array([e.evaluateError() for e in reprojectionErrors1]) sm.logDebug( " Reprojection error squarred (camH): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) sm.logDebug("baseline={0}".format( baseline_dv.toTransformationMatrix())) try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("stereoCalibrate: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("stereoCalibrate: Optimization failed!") success = False if sm.getLoggingLevel() == sm.LoggingLevel.Debug: sm.logDebug("After optimization:") e2 = np.array([e.evaluateError() for e in reprojectionErrors0]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) e2 = np.array([e.evaluateError() for e in reprojectionErrors1]) sm.logDebug( " Reprojection error squarred (camH): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) #verbose output if sm.getLoggingLevel() == sm.LoggingLevel.Debug: dL = camL_geometry.geometry.projection().distortion().getParameters( ).flatten() pL = camL_geometry.geometry.projection().getParameters().flatten() dH = camH_geometry.geometry.projection().distortion().getParameters( ).flatten() pH = camH_geometry.geometry.projection().getParameters().flatten() sm.logDebug("guess for intrinsics camL: {0}".format(pL)) sm.logDebug("guess for intrinsics camH: {0}".format(pH)) sm.logDebug("guess for distortion camL: {0}".format(dL)) sm.logDebug("guess for distortion camH: {0}".format(dH)) if success: baseline_HL = sm.Transformation(baseline_dv.toTransformationMatrix()) return success, baseline_HL else: #return the initial guess if we fail return success, baseline_HL
import sm import bsplines import numpy as np from scipy.spatial.transform import Rotation as R if __name__ == "__main__": bsp = bsplines.BSplinePose(4, sm.RotationVector()) curve = np.array([[0.1, 0.2, 0.3, 0.4, 0.5, 0.6], [0.1, 0, 0, 0, 0, 0.1], [0.2, 0, 0, 0, 0, 0.1]]).T bsp.initPoseSplineSparse(np.array([0., 1, 2]), curve, 5, 1e-5) r0 = R.from_dcm(bsp.orientation(0.0)).as_rotvec() r0_ = R.from_dcm(bsp.curveValueToTransformation( curve.T[0])[:3, :3]).as_rotvec() t0_ = bsp.curveValueToTransformation(curve.T[0])[:3, 3] print(r0) print(t0_)