def addDesignVariables(self, problem, noExtrinsics=True, noTimeCalibration=True, baselinedv_group_id=ic.HELPER_GROUP_ID): # Add the calibration design variables. active = not noExtrinsics self.T_c_b_Dv = aopt.TransformationDv(self.T_extrinsic, rotationActive=active, translationActive=active) for i in range(0, self.T_c_b_Dv.numDesignVariables()): problem.addDesignVariable(self.T_c_b_Dv.getDesignVariable(i), baselinedv_group_id) # Add the time delay design variable. self.cameraTimeToImuTimeDv = aopt.Scalar(0.0) self.cameraTimeToImuTimeDv.setActive(not noTimeCalibration) problem.addDesignVariable(self.cameraTimeToImuTimeDv, ic.CALIBRATION_GROUP_ID)
def registration(self, target, w=0.0, objective_type='pt2pt', maxiter=50, tol=0.001, feature_fn=lambda x: x): q = None ftarget = feature_fn(target) # build the problem self.problem = aopt.OptimizationProblem() T_init = np.array([[-0.99813747, -0.05730897, -0.02091093, 0.03399231], [0.05726434, -0.99835533, 0.00272772, 0.31433327], [-0.02103286, 0.00152519, 0.99977762, 0.22997991], [0., 0., 0., 1.]]) self.T_b_l_Dv = aopt.TransformationDv(sm.Transformation(), rotationActive=True, translationActive=True) for i in range(0, self.T_b_l_Dv.numDesignVariables()): self.problem.addDesignVariable(self.T_b_l_Dv.getDesignVariable(i)) for _ in range(maxiter): T_b_l = self.T_b_l_Dv.toExpression().toTransformationMatrix() print("Inital laser to body transformation: T_b_l ") print(T_b_l) T_l_b = np.linalg.inv(T_b_l) numPoints = self._source.shape[0] source = np.hstack([self._source, np.ones((numPoints, 1), dtype=self._source.dtype)]) t_source = np.array([np.dot(T_l_b, np.dot(self._sensor_tfs[i], np.dot(T_b_l, source[i]))) for i in xrange(numPoints)]) t_source = t_source[:, :3] util.showPointCloud([t_source, target]) fsource = feature_fn(t_source) estep_res = self.expectation_step(fsource, ftarget, target, objective_type) res = self.maximization_step(self._source, target, estep_res, w=w, objective_type=objective_type) if not q is None and abs(res.q - q) < tol: break q = res.q return res.transformation
def initializeBaselineDVs(self, baseline_guesses): self.baselines = list() for baseline_idx in range(0, len(self.cameras) - 1): self.baselines.append( aopt.TransformationDv(baseline_guesses[baseline_idx]))
def fromTargetViewObservations(cls, cameras, target, baselines, T_tc_guess, rig_observations, useBlakeZissermanMest=True): rval = CalibrationTargetOptimizationProblem() #store the arguements in case we want to rebuild a modified problem rval.cameras = cameras rval.target = target rval.baselines = baselines rval.T_tc_guess = T_tc_guess rval.rig_observations = rig_observations # 1. Create a design variable for this pose T_target_camera = T_tc_guess rval.dv_T_target_camera = aopt.TransformationDv(T_target_camera) for i in range(0, rval.dv_T_target_camera.numDesignVariables()): rval.addDesignVariable( rval.dv_T_target_camera.getDesignVariable(i), TRANSFORMATION_GROUP_ID) #2. add all baselines DVs for baseline_dv in baselines: for i in range(0, baseline_dv.numDesignVariables()): rval.addDesignVariable(baseline_dv.getDesignVariable(i), CALIBRATION_GROUP_ID) #3. add landmark DVs for p in target.P_t_dv: rval.addDesignVariable(p, LANDMARK_GROUP_ID) #4. add camera DVs for camera in cameras: if not camera.isGeometryInitialized: raise RuntimeError( 'The camera geometry is not initialized. Please initialize with initGeometry() or initGeometryFromDataset()' ) camera.setDvActiveStatus(True, True, False) rval.addDesignVariable(camera.dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) rval.addDesignVariable(camera.dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) rval.addDesignVariable(camera.dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) #4.add all observations for this view cams_in_view = set() rval.rerrs = dict() rerr_cnt = 0 for cam_id, obs in rig_observations: camera = cameras[cam_id] cams_in_view.add(cam_id) #add reprojection errors #build baseline chain (target->cam0->baselines->camN) T_cam0_target = rval.dv_T_target_camera.expression.inverse() T_camN_calib = T_cam0_target for idx in range(0, cam_id): T_camN_calib = baselines[idx].toExpression() * T_camN_calib # \todo pass in the detector uncertainty somehow. cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) rval.rerrs[cam_id] = list() for i in range(0, len(target.P_t_ex)): p_target = target.P_t_ex[i] valid, y = obs.imagePoint(i) if valid: rerr_cnt += 1 # Create an error term. rerr = camera.model.reprojectionError( y, invR, T_camN_calib * p_target, camera.dv) rerr.idx = i #add blake-zisserman mest if useBlakeZissermanMest: mest = aopt.BlakeZissermanMEstimator(2.0) rerr.setMEstimatorPolicy(mest) rval.addErrorTerm(rerr) rval.rerrs[cam_id].append(rerr) else: rval.rerrs[cam_id].append(None) sm.logDebug( "Adding a view with {0} cameras and {1} error terms".format( len(cams_in_view), rerr_cnt)) return rval
def solveFullBatch(cameras, baseline_guesses, graph): ############################################ ## solve the bundle adjustment ############################################ problem = aopt.OptimizationProblem() #add camera dvs for cam in cameras: cam.setDvActiveStatus(True, True, False) problem.addDesignVariable(cam.dv.distortionDesignVariable()) problem.addDesignVariable(cam.dv.projectionDesignVariable()) problem.addDesignVariable(cam.dv.shutterDesignVariable()) baseline_dvs = list() for baseline_idx in range(0, len(cameras) - 1): baseline_dv = aopt.TransformationDv(baseline_guesses[baseline_idx]) for i in range(0, baseline_dv.numDesignVariables()): problem.addDesignVariable(baseline_dv.getDesignVariable(i)) baseline_dvs.append(baseline_dv) #corner uncertainty cornerUncertainty = 1.0 R = np.eye(2) * cornerUncertainty * cornerUncertainty invR = np.linalg.inv(R) #get the target target = cameras[0].ctarget.detector.target() #Add calibration target reprojection error terms for all camera in chain target_pose_dvs = list() #shuffle the views reprojectionErrors = [] timestamps = graph.obs_db.getAllViewTimestamps() for view_id, timestamp in enumerate(timestamps): #get all observations for all cams at this time obs_tuple = graph.obs_db.getAllObsAtTimestamp(timestamp) #create a target pose dv for all target views (= T_cam0_w) T0 = graph.getTargetPoseGuess(timestamp, cameras, baseline_guesses) target_pose_dv = addPoseDesignVariable(problem, T0) target_pose_dvs.append(target_pose_dv) for cidx, obs in obs_tuple: cam = cameras[cidx] #calibration target coords to camera X coords T_cam0_calib = target_pose_dv.toExpression().inverse() #build pose chain (target->cam0->baselines->camN) T_camN_calib = T_cam0_calib for idx in range(0, cidx): T_camN_calib = baseline_dvs[idx].toExpression() * T_camN_calib ## add error terms for i in range(0, target.size()): p_target = aopt.HomogeneousExpression( sm.toHomogeneous(target.point(i))) valid, y = obs.imagePoint(i) if valid: rerr = cameras[cidx].model.reprojectionError( y, invR, T_camN_calib * p_target, cameras[cidx].dv) problem.addErrorTerm(rerr) reprojectionErrors.append(rerr) sm.logDebug("solveFullBatch: added {0} camera error terms".format( len(reprojectionErrors))) ############################################ ## 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 = 250 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 reprojectionErrors]) sm.logDebug( " Reprojection error squarred (camL): mean {0}, median {1}, std: {2}" .format(np.mean(e2), np.median(e2), np.std(e2))) #run intrinsic calibration try: retval = optimizer.optimize() if retval.linearSolverFailure: sm.logError("calibrateIntrinsics: Optimization failed!") success = not retval.linearSolverFailure except: sm.logError("calibrateIntrinsics: Optimization failed!") success = False baselines = list() for baseline_dv in baseline_dvs: baselines.append(sm.Transformation(baseline_dv.T())) return success, baselines