def load(self, camera_config, local_config, scale=1.0): if camera_config is None: if os.path.exists(local_config): # load local config file if it exists result = props_json.load(local_config, self.config) if not result: print("Cannot continue with invalid camera file.") quit() else: print( "no camera config specified and no local camera config file found:", local_config) quit() else: # seed the camera calibration and distortion coefficients # from a known camera config print("Setting camera config from:", camera_config) props_json.load(camera_config, self.config) self.config.setString('name', camera_config) props_json.save(local_config, self.config) self.get_K() if scale: # adjust effective K to account for scaling self.K = self.K * scale self.K[2, 2] = 1.0 self.config.setLen('mount_ypr', 3, 0)
def write_configs(): config = getNode("/config", True) file = os.path.join(flight_dir, 'master-config.json') props_json.save(file, config) config = getNode("/config/autopilot", True) file = os.path.join(flight_dir, 'ap-config.json') props_json.save(file, config)
def save(self): # create a project dictionary and write it out as json if not os.path.exists(self.analysis_dir): print("Error: project doesn't exist:", self.analysis_dir) return project_file = os.path.join(self.analysis_dir, "config.json") config_node = getNode("/config", True) props_json.save(project_file, config_node)
def save_images_info(self): # create a project dictionary and write it out as json if not os.path.exists(self.analysis_dir): print("Error: project doesn't exist:", self.analysis_dir) return meta_dir = os.path.join(self.analysis_dir, 'meta') images_node = getNode("/images", True) for name in images_node.getChildren(): image_node = images_node.getChild(name, True) image_path = os.path.join(meta_dir, name + '.json') props_json.save(image_path, image_node)
def save(self, cal_file): config = PropertyNode() config.setFloat('min_temp_C', self.min_temp) config.setFloat('max_temp_C', self.max_temp) node = config.getChild('p', create=True) p = self.p_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.p_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('q', create=True) p = self.q_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.q_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('r', create=True) p = self.r_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.r_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('ax', create=True) p = self.ax_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.ax_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('ay', create=True) p = self.ay_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.ay_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('az', create=True) p = self.az_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.az_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) affine_str = [] for x in self.mag_affine.flat: affine_str.append('%.10f' % x) print(' '.join(affine_str)) config.setString('mag_affine', ' '.join(affine_str)) try: props_json.save(cal_file, config) except: print("error saving " + cal_file + ": " + str(sys.exc_info()[1])) return
def save(self, cal_file): config = PropertyNode() config.setFloat('min_temp_C', self.min_temp) config.setFloat('max_temp_C', self.max_temp) node = config.getChild('p', create=True) p = self.p_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.p_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('q', create=True) p = self.q_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.q_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('r', create=True) p = self.r_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.r_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('ax', create=True) p = self.ax_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.ax_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('ay', create=True) p = self.ay_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.ay_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) node = config.getChild('az', create=True) p = self.az_bias node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) p = self.az_scale node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2])) affine_str = [] for x in self.mag_affine.flat: affine_str.append('%.10f' % x) print ' '.join(affine_str) config.setString('mag_affine', ' '.join(affine_str)) try: props_json.save(cal_file, config) except: print "error saving " + cal_file + ": " + str(sys.exc_info()[1]) return
def save(self): if self.filename == "": filename = str(self.setFileName()) if filename == "": # print "cancelled save ..." return else: self.filename = filename self.fileroot, ext = os.path.splitext(self.filename) # print self.fileroot, ext # create the design as a property tree design = self.gen_property_tree() try: props_json.save(self.filename, design) except: print "error saving file" return self.setWindowTitle( self.default_title + " - " + os.path.basename(str(self.filename)) ) self.setClean()
def save(self): if self.filename == "": filename = str(self.setFileName()) if filename == "": # print "cancelled save ..." return else: self.filename = filename self.fileroot, ext = os.path.splitext(self.filename) # print self.fileroot, ext # create the design as a property tree design = self.gen_property_tree() try: props_json.save(self.filename, design) except: print("error saving file") return self.setWindowTitle(self.default_title + " - " + os.path.basename(str(self.filename))) self.setClean()
# copy config.json from first listed source project print("Copying config.json from source project") config_src = os.path.join(args.source[0], "ImageAnalysis", "config.json") config_dest = os.path.join(analysis_dir, "config.json") if os.path.exists(config_src): shutil.copyfile(config_src, config_dest) # assemble the collective smart.json file print("Combining all the smart.json files") smart_node = getNode("/smart", True) for p in args.source: smart_src = os.path.join(p, "ImageAnalysis", "smart.json") props_json.load(smart_src, smart_node) smart_dst = os.path.join(project_dir, "ImageAnalysis", "smart.json") props_json.save(smart_dst, smart_node) # assemble the collective annotations.json file print("Combining any annotations.json files") ann_node = getNode("/annotations", True) ann_list = [] for p in args.source: ann_src = os.path.join(p, "ImageAnalysis", "annotations.json") if os.path.exists(ann_src): f = open(ann_src, 'r') ann_list += json.load(f) ann_dst = os.path.join(project_dir, "ImageAnalysis", "annotations.json") f = open(ann_dst, 'w') json.dump(ann_list, f, indent=4) # populate the meta directory
def log_imu_calibration(config): os.path.join(flight_dir, 'imucal.json') props_json.save(config, jsonfile) return True
#!/usr/bin/python from props import PropertyNode, root, getNode import props_xml import props_json import sys # run the system through it's paces xmlfile = sys.argv[1] jsonfile = sys.argv[2] config = PropertyNode() props_xml.load(xmlfile, config) props_json.save(jsonfile, config)
cam_yaw = config.getFloatEnum('mount_ypr', 0) cam_pitch = config.getFloatEnum('mount_ypr', 1) cam_roll = config.getFloatEnum('mount_ypr', 2) # setup camera calibration and distortion coefficients if args.select_cam: # set the camera calibration from known preconfigured setups name, K, dist = cam_calib.set_camera_calibration(args.select_cam) config.setString('name', name) config.setFloat("fx", K[0][0]) config.setFloat("fy", K[1][1]) config.setFloat("cu", K[0][2]) config.setFloat("cv", K[1][2]) for i, d in enumerate(dist): config.setFloatEnum("dist_coeffs", i, d) props_json.save(camera_config, config) else: # load the camera calibration from the config file name = config.getString('name') size = config.getLen("dist_coeffs") dist = [] for i in range(size): dist.append(config.getFloatEnum("dist_coeffs", i)) K = np.zeros((3, 3)) K[0][0] = config.getFloat("fx") K[1][1] = config.getFloat("fy") K[0][2] = config.getFloat("cu") K[1][2] = config.getFloat("cv") K[2][2] = 1.0 print 'Camera:', name
def update(self, dt): if not self.active: return False # update filters p = self.imu_node.getFloat("p_rad_sec") q = self.imu_node.getFloat("q_rad_sec") r = self.imu_node.getFloat("r_rad_sec") ax = self.imu_node.getFloat("ax_mps_sec") ay = self.imu_node.getFloat("ay_mps_sec") az = self.imu_node.getFloat("az_mps_sec") hx_raw = self.imu_node.getFloat("hx_raw") hy_raw = self.imu_node.getFloat("hy_raw") hz_raw = self.imu_node.getFloat("hz_raw") self.p_filt.update(p, dt) self.q_filt.update(q, dt) self.r_filt.update(r, dt) self.ax_filt.update(ax, dt) self.ay_filt.update(ay, dt) self.az_filt.update(az, dt) up_axis = self.detect_up() if up_axis == "none": self.armed = True self.rot = 0.0 if self.state < 1: print("up axis:", up_axis, "armed:", self.armed, " rot: %0f" % self.rot) sample_time = 5 if self.state == 0: if self.armed and up_axis != "none": if self.axis_time[up_axis] < sample_time: self.samples.append([hx_raw, hy_raw, hz_raw]) if hx_raw < self.min[0]: self.min[0] = hx_raw if hx_raw > self.max[0]: self.max[0] = hx_raw if hy_raw < self.min[1]: self.min[1] = hy_raw if hy_raw > self.max[1]: self.max[1] = hy_raw if hz_raw < self.min[2]: self.min[2] = hz_raw if hz_raw > self.max[2]: self.max[2] = hz_raw print(self.min, self.max) self.axis_time[up_axis] += dt done = True for key in self.axis_time: if self.axis_time[key] < sample_time: message = "need more: " + self.axis_hint[key] self.task_calib_node.setString("mag_status", message) comms.display.show(message) done = False break if done: self.state += 1 elif self.state == 1: # did we measure a bunch of samples? if len(self.samples) < 100: print("Somehow didn't get many samples. :-(") self.state += 2 else: # center, evecs, radii, v = ellipsoid_fit(s) # print("center:\n", center) # print("evecs:\n", evecs) # print("radii:\n", radii) # print("v:\n", v) # ellipsoid fit with our sample data s = np.array(self.samples).T M, n, d = ellipsoid_fit(s) # calibration parameters. Note: some implementations # of sqrtm return complex type, taking real M_1 = linalg.inv(M) self.b = -np.dot(M_1, n) self.A_1 = np.real(self.F / np.sqrt(np.dot(n.T, np.dot(M_1, n)) - d) * linalg.sqrtm(M)) print("b:\n", self.b) print("A_1:\n", self.A_1) # assemble the mag calibration matrix T = tr.translation_matrix(-self.b.flatten()) A1_h = np.eye(4) A1_h[:3, :3] = self.A_1 self.mag_affine = A1_h @ T # this is the correct order print("mag_affine:\n", self.mag_affine) scale, shear, angles, translate, perspective = tr.decompose_matrix( self.mag_affine) print("scale:", scale) print("shear:", shear) print("angles:", angles) print("translate:", translate) print("perspective:", perspective) # fixme what can we look at to sanity check? # nothing bad detected, goto success state self.state += 1 elif self.state == 2: # calibration complete, success, save, report! print("calibration succeeded") print("magnetometer calibration:") # as if this wasn't already fancy enough, get even fancier! mapped = [] for i, v in enumerate(self.samples): #print("sample:", i, v) v1 = self.mag_affine @ np.hstack((v, 1)) v2 = self.A_1 @ (np.array(v) - self.b.flatten()) #print(v, v1[:3], v2) mapped.append(v2) print("samples:") samples = np.array(self.samples) print("x range:", np.min(samples[:, 0]), np.max(samples[:, 0])) print("y range:", np.min(samples[:, 1]), np.max(samples[:, 1])) print("z range:", np.min(samples[:, 2]), np.max(samples[:, 2])) print("mapped:") mapped = np.array(mapped) print("x range:", np.min(mapped[:, 0]), np.max(mapped[:, 0])) print("y range:", np.min(mapped[:, 1]), np.max(mapped[:, 1])) print("z range:", np.min(mapped[:, 2]), np.max(mapped[:, 2])) self.state += 2 calib_node = self.config_imu_node.getChild("calibration", True) calib_node.setLen("mag_affine", 16) for i in range(16): calib_node.setFloatEnum("mag_affine", i, self.mag_affine.flatten()[i]) home = os.path.expanduser("~") props_json.save(os.path.join(home, "imu_calibration.json"), calib_node) message = "mag calibration succeeded" self.task_calib_node.setString("mag_status", message) comms.display.show(message) self.state += 2 elif self.state == 3: # calibration complete, failed, sad face. :-( message = "mag calibration failed" self.task_calib_node.setString("mag_status", message) comms.display.show(message) self.state += 1
fx = focal_len_mm * width / args.ccd_width fy = focal_len_mm * height / ccd_height else: print("Cannot autodetect calibrated focal length, please specify a ccd-width") quit() print('ccd: %.3f x %.3f' % (ccd_width, ccd_height)) print('fx fy = %.2f %.2f' % (fx, fy)) cu = width * 0.5 cv = height * 0.5 print('cu cv = %.2f %.2f' % (cu, cv)) cam = Camera.Camera() cam.set_defaults() cam.set_meta(make, model, lens_model) cam.set_lens_params(ccd_width, ccd_height, focal_len_mm) cam.set_K(fx, fy, cu, cv) cam.set_image_params(width, height) if os.path.exists(camera_file): print("Camera config file already exists:", camera_file) if args.force: print("Overwriting ...") else: print("Use [ --force ] to overwrite ...") quit() print("Saving:", camera_file) cam_node = getNode('/config/camera', True) props_json.save(camera_file, cam_node)
# emulate alpha blending alpha = args.alpha if alpha < 0: alpha = 0 if alpha > 1: alpha = 1 cv2.addWeighted(hud1_frame, alpha, frame_undist, 1 - alpha, 0, hud1_frame) cv2.imshow('hud', hud1_frame) output.write(hud1_frame) key = cv2.waitKey(5) & 0xFF if key == 27: break elif key == ord('y'): cam_yaw += 0.5 config.setFloat('cam_yaw_deg', cam_yaw) props_json.save(movie_config, config) elif key == ord('Y'): cam_yaw -= 0.5 config.setFloat('cam_yaw_deg', cam_yaw) props_json.save(movie_config, config) elif key == ord('p'): cam_pitch += 0.5 config.setFloat('cam_pitch_deg', cam_pitch) props_json.save(movie_config, config) elif key == ord('P'): cam_pitch -= 0.5 config.setFloat('cam_pitch_deg', cam_pitch) props_json.save(movie_config, config) elif key == ord('r'): cam_roll -= 0.5 config.setFloat('cam_roll_deg', cam_roll)
basename, ext = os.path.splitext(abspath) srtname = basename + ".srt" dirname = basename + "_frames" print("basename:", basename) print("srtname:", srtname) print("dirname:", dirname) local_config = os.path.join(dirname, "camera.json") config = PropertyNode() if args.camera: # seed the camera calibration and distortion coefficients from a # known camera config print('Setting camera config from:', args.camera) props_json.load(args.camera, config) config.setString('name', args.camera) props_json.save(local_config, config) elif os.path.exists(local_config): # load local config file if it exists props_json.load(local_config, config) K_list = [] for i in range(9): K_list.append( config.getFloatEnum('K', i) ) K = np.copy(np.array(K_list)).reshape(3,3) dist = [] for i in range(5): dist.append( config.getFloatEnum("dist_coeffs", i) ) # check for required input files if not os.path.isfile(args.video): print("%s doesn't exist, aborting ..." % args.video) quit()
def save(self, local_config): props_json.save(local_config, self.config)
def update(self, dt): if not self.active: return False # update filters ax = self.imu_node.getFloat("ax_raw") ay = self.imu_node.getFloat("ay_raw") az = self.imu_node.getFloat("az_raw") self.ax_slow.update(ax, dt) self.ax_fast.update(ax, dt) self.ay_slow.update(ay, dt) self.ay_fast.update(ay, dt) self.az_slow.update(az, dt) self.az_fast.update(az, dt) # (no) motion test ax_diff = self.ax_slow.filter_value - self.ax_fast.filter_value ay_diff = self.ay_slow.filter_value - self.ay_fast.filter_value az_diff = self.az_slow.filter_value - self.az_fast.filter_value d = math.sqrt(ax_diff*ax_diff + ay_diff*ay_diff + az_diff*az_diff) if d < 0.04: stable = True else: stable = False up_axis = self.detect_up() if up_axis == "none": self.armed = True if self.state < 6: print("up axis:", up_axis, "armed:", self.armed, " slow-fast: %.3f" % d, " stable:", stable) if not self.armed: self.task_node.setInt("calib_state", 99) else: self.task_node.setInt("calib_state", self.state) if self.state == 0: print("Place level and right side up - stable:", stable) if self.armed and stable and self.new_axis(): self.meas[self.state] = [ self.ax_fast.filter_value, self.ay_fast.filter_value, self.az_fast.filter_value ] self.checked[up_axis] = True self.state += 1 self.armed = False elif self.state == 1: print("Place up side down - stable:", stable) if self.armed and stable and self.new_axis(): self.meas[self.state] = [ self.ax_fast.filter_value, self.ay_fast.filter_value, self.az_fast.filter_value ] self.checked[up_axis] = True self.state += 1 self.armed = False elif self.state == 2: print("Place nose down - stable:", stable) if self.armed and stable and self.new_axis(): self.meas[self.state] = [ self.ax_fast.filter_value, self.ay_fast.filter_value, self.az_fast.filter_value ] self.checked[up_axis] = True self.state += 1 self.armed = False elif self.state == 3: print("Place nose up - stable:", stable) if self.armed and stable and self.new_axis(): self.meas[self.state] = [ self.ax_fast.filter_value, self.ay_fast.filter_value, self.az_fast.filter_value ] self.checked[up_axis] = True self.state += 1 self.armed = False elif self.state == 4: print("Place right wing down - stable:", stable) if self.armed and stable and self.new_axis(): self.meas[self.state] = [ self.ax_fast.filter_value, self.ay_fast.filter_value, self.az_fast.filter_value ] self.checked[up_axis] = True self.state += 1 self.armed = False elif self.state == 5: print("Place right wing up - stable:", stable) if self.armed and stable and self.new_axis(): self.meas[self.state] = [ self.ax_fast.filter_value, self.ay_fast.filter_value, self.az_fast.filter_value ] self.checked[up_axis] = True self.state += 1 self.armed = False elif self.state == 6: # did we measure 6 unique axes? if len(self.checked) != 6: print("Somehow didn't calibrate 6 orientations. :-(") self.state += 2 else: # compute affine rotation fit v0 = np.array(self.meas, dtype=np.float64, copy=True).T v1 = np.array(self.ref, dtype=np.float64, copy=True).T self.accel_affine = tr.affine_matrix_from_points(v0, v1, shear=True, scale=True) print("accel_affine:\n", self.accel_affine) self.scale, shear, angles, self.translate, perspective = tr.decompose_matrix(self.accel_affine) print("scale:", self.scale) print("shear:", shear) print("angles:", angles) print("translate:", self.translate) print("perspective:", perspective) # recompose the original affine matrix with: # translate @ rotate @ scale T = tr.translation_matrix(self.translate) self.R = tr.euler_matrix(*angles) S = np.diag([self.scale[0], self.scale[1], self.scale[2], 1.0]) print("T:\n", T) print("R:\n", self.R) print("S:\n", S) print("R @ R.T:\n", self.R @ self.R.T) recompose = T @ self.R @ S print("recompose:\n", recompose) # check rotation matrix, if any row or column doesn't # have an element close to 1, then bomb if np.max(np.abs(self.R[0])) < 0.9: print("bad row 1") self.state += 2 elif np.max(np.abs(self.R[1])) < 0.9: print("bad row 2") self.state += 2 elif np.max(np.abs(self.R[2])) < 0.9: print("bad row 3") self.state += 2 elif np.max(np.abs(self.R[:,0])) < 0.9: print("bad column 1") self.state += 2 elif np.max(np.abs(self.R[:,1])) < 0.9: print("bad column 2") self.state += 2 elif np.max(np.abs(self.R[:,2])) < 0.9: print("bad column 3") self.state += 2 else: # nothing bad detected, goto success state self.state += 1 elif self.state == 7: # calibration complete, success, report! comms.events.log("calibrate accels", "calibration succeeded.") #print("strapdown calibration:") #print(self.R) # as if this wasn't already fancy enough, get even fancier! errors = [] for i, v in enumerate(self.meas): print("measure:", i, v) v1 = self.accel_affine @ np.hstack((v, 1)) v0 = self.ref[i] err = np.linalg.norm(v0 - v1[:3]) errors.append(err) #print("errors:", errors) mean = np.mean(errors) std = np.std(errors) #print("calibration mean:", mean, " std:", std) self.state += 2 calib_node = self.config_imu_node.getChild("calibration", True) calib_node.setLen("strapdown", 9) for i in range(9): calib_node.setFloatEnum("strapdown", i, self.R[:3,:3].flatten()[i]) calib_node.setFloat("accel_fit_mean", mean) calib_node.setFloat("accel_fit_std", std) calib_node.setLen("accel_scale", 3) for i in range(3): calib_node.setFloatEnum("accel_scale", i, self.scale[i]) calib_node.setLen("accel_translate", 3) for i in range(3): calib_node.setFloatEnum("accel_translate", i, self.translate[i]) home = os.path.expanduser("~") filename = os.path.join(home, "imu_calibration.json") props_json.save(filename, calib_node) comms.events.log("calibrate accels", "saved results to: " + filename) elif self.state == 8: # calibration complete, but failed. :-( comms.events.log("calibrate accels", "calibration FAILED!") pass
def save(analysis_dir): surface_file = os.path.join(analysis_dir, "smart.json") props_json.save(surface_file, smart_node)
def set_aircraft_poses(proj, posefile="", order='ypr', max_angle=25.0): log("Setting aircraft poses") #analysis_dir = os.path.join(proj.project_dir, 'ImageAnalysis') meta_dir = os.path.join(proj.analysis_dir, 'meta') images_node = getNode("/images", True) by_index = False f = fileinput.input(posefile) for line in f: line.strip() if re.match('^\s*#', line): #print("skipping comment ", line) continue if re.match('^\s*File', line): #print("skipping header ", line) continue if re.match('^\s*Image', line): #print("skipping header ", line) by_index = True file_list = gen_image_list(proj.project_dir) continue field = line.split(',') if not by_index: name = field[0] else: index = int(field[0]) - 1 name = file_list[index] lat_deg = float(field[1]) lon_deg = float(field[2]) alt_m = float(field[3]) if order == 'ypr': yaw_deg = float(field[4]) pitch_deg = float(field[5]) roll_deg = float(field[6]) elif order == 'rpy': roll_deg = float(field[4]) pitch_deg = float(field[5]) yaw_deg = float(field[6]) if len(field) >= 8: flight_time = float(field[7]) else: flight_time = -1.0 found_dir = '' if not os.path.isfile(os.path.join(proj.project_dir, name)): log("No image file:", name, "skipping ...") continue if camera.camera_node.getString( "make") == "DJI" or camera.camera_node.getString( "make") == "Hasselblad": # camera is on a gimbal so check if pitch is nearly nadir (-90) if pitch_deg > -45: log("gimbal not looking down:", name, "roll:", roll_deg, "pitch:", pitch_deg) continue elif abs(roll_deg) > max_angle or abs(pitch_deg) > max_angle: # fairly 'extreme' attitude, skip image log("extreme attitude:", name, "roll:", roll_deg, "pitch:", pitch_deg) continue base, ext = os.path.splitext(name) i1 = image.Image(proj.analysis_dir, base) i1.set_aircraft_pose(lat_deg, lon_deg, alt_m, yaw_deg, pitch_deg, roll_deg, flight_time) image_node = images_node.getChild(base, True) image_path = os.path.join(meta_dir, base + '.json') props_json.save(image_path, image_node) log("pose:", name, "yaw=%.1f pitch=%.1f roll=%.1f" % (yaw_deg, pitch_deg, roll_deg))
# in this system we force fx == fy, but here fy computed separately # just for fun. fx = focal_len_mm * float(width) / args.ccd_width fy = focal_len_mm * float(height) / ccd_height print('fx fy = %.2f %.2f' % (fx, fy)) cu = width * 0.5 cv = height * 0.5 print('cu cv = %.2f %.2f' % (cu, cv)) cam = Camera.Camera() cam.set_defaults() cam.set_meta(make, model, lens_model) cam.set_lens_params(args.ccd_width, ccd_height, focal_len_mm) cam.set_K(fx, fy, cu, cv) cam.set_image_params(width, height) file_name = os.path.join(args.config, base_name + '.json') if os.path.exists(file_name): print('Camera config file already exists:', file_name) if args.force: print('Overwriting ...') else: print('Aborting ...') quit() print('Saving:', file_name) cam_node = getNode('/config/camera', True) props_json.save(file_name, cam_node)