void LoadMapFromVoodooTextFile(const string& text_file, const string& image_pattern, Map& map) { static const string kMagic = "#timeindex"; static const string kPointsHeader = "# 3D Feature Points"; sifstream in(expand_path(text_file)); double Cx, Cy, Cz, Ax, Ay, Az, Hx, Hy, Hz, Vx, Vy, Vz, K3, K5; double sx, sy, Width, Height, ppx, ppy, f, fov; double H0x, H0y, H0z, V0x, V0y, V0z; DLOG << "WARNING: using first camera intrinsics for all cameras"; int i = 0; Vec3 v; bool reading_points = false; boost::format image_fmt(image_pattern); while (!in.eof()) { if (reading_points) { in >> v[0] >> v[1] >> v[2]; map.points.push_back(v); } else { string line; getline(in, line); if (line.substr(0,kMagic.size()) == kMagic) { int id = lexical_cast<int>(line.substr(kMagic.size()+1)); in >> Cx >> Cy >> Cz >> Ax >> Ay >> Az >> Hx >> Hy >> Hz >> Vx >> Vy >> Vz >> K3 >> K5 >> sx >> sy >> Width >> Height >> ppx >> ppy >> f >> fov >> H0x >> H0y >> H0z >> V0x >> V0y >> V0z; string image_file = str(image_fmt % 21);//id); CHECK_PRED1(fs::exists, image_file); Mat3 C; C[0] = makeVector(f/sx, 0, ppx); C[1] = makeVector(0, f/sy, ppx); C[2] = makeVector(0, 0, 1); if (i == 0) { Vec2I image_size = GetImageSize(expand_path(image_file)); map.camera.reset(new LinearCamera(C, image_size)); } Mat3 R; R[0] = makeVector(H0x, H0z, H0y); R[1] = makeVector(V0x, V0z, V0y); R[2] = makeVector(Ax, Az, Ay); Vec3 t = makeVector(-Cx, -Cy, -Cz); SE3<> pose(SO3<>(R), t); map.AddFrame(new Frame(id, image_file, pose)); i++; } else if (line.substr(0, kPointsHeader.size()) == kPointsHeader) {
void LoadBundlerMap(const string& structure_file, const string& image_list_file, Map& map) { CHECK_PRED1(fs::exists, structure_file); CHECK_PRED1(fs::exists, image_list_file); // Get the base path to help find images fs::path basedir = fs::path(image_list_file).parent_path(); // Read list of images sifstream list_in(image_list_file); vector<fs::path> image_paths; int nonexistent = 0; while (true) { string image_file; getline(list_in, image_file); if (list_in.eof()) break; fs::path path = basedir / image_file; if (!fs::exists(path)) { DLOG << "WARNING: image file not found: " << path; } image_paths.push_back(path); } // Load structure sifstream in(structure_file); // Ignore comment string comment; getline(in, comment); // Load number of cameras and points int num_cameras, num_points; in >> num_cameras >> num_points; CHECK(num_cameras > 0); CHECK_EQ(image_paths.size(), num_cameras) << "Number of cameras should equal number of image files in " << image_list_file; // Load poses bool warned_focal = false; bool warned_distortion = false; double focal_length; double f; Mat3 R; Vec3 t; Vec2 distortion; for (int i = 0; i < num_cameras; i++) { in >> f >> distortion >> R >> t; if (distortion != makeVector(0,0) && !warned_distortion) { DLOG << "WARNING: Distorted cameras not supported yet, " << "loading map anyway"; warned_distortion = true; } if (i == 0) { focal_length = f; // Construct a linear camera Mat3 cam = Identity; cam[0][0] = cam[1][1] = focal_length; map.camera.reset(new LinearCamera(cam, *gvDefaultImageSize)); } else { if (f != focal_length && !warned_focal) { DLOG << "WARNING: Multiple focal lengths not supported yet, " "loading map anyway"; warned_focal = true; } } // Bundler considers cameras looking down the negative Z axis so // invert (this only really matters for visualizations and // visibility testing). SE3<> pose(SO3<>(-R), -t); string image_file = i < image_paths.size() ? image_paths[i].string() : ""; map.AddFrame(new Frame(i, image_file, pose)); } // Load points // TODO: store observations Vec3 v, color; Vec2 obs_pt; int num_obs, obs_camera, obs_key; for (int i = 0; i < num_points; i++) { in >> v >> color >> num_obs; for (int j = 0; j < num_obs; j++) { in >> obs_camera >> obs_key >> obs_pt; } map.points.push_back(v); } }
void LoadXmlMap(const string& path, Map& map, bool all_frames) { // Clear any old state map.frames.clear(); map.points.clear(); // Configure the camera map.orig_camera.reset(new ATANCamera(*gvDefaultCameraParams, *gvDefaultImageSize)); if (*gvLinearizeCamera) { Mat3 cam = map.orig_camera->Linearize(); map.camera.reset(new LinearCamera(cam, map.orig_camera->image_size())); } else { map.camera = map.orig_camera; } // Parse the XML file TiXmlDocument doc; CHECK_PRED1(doc.LoadFile, path.c_str()) << "Failed to load " << path; fs::path xml_dir(fs::path(path).parent_path()); fs::path frames_dir(*gvOrigFramesDir); const TiXmlElement* root_elem = doc.RootElement(); // Read the points std::map<int, int> points_id_to_index; const TiXmlElement* points_elem = root_elem->FirstChildElement("MapPoints"); CHECK(points_elem) << "There was no <MapPoints> element in the map spec."; for (const TiXmlElement* pt_elem = points_elem->FirstChildElement("MapPoint"); pt_elem != NULL; pt_elem = pt_elem->NextSiblingElement("MapPoint")) { int id = lexical_cast<int>(pt_elem->Attribute("id")); points_id_to_index[id] = map.points.size(); // indices start from zero map.points.push_back(stream_to<Vec3>(pt_elem->Attribute("position"))); } // vector of frame IDs for which we fell back to B&W images vector<int> fallback_frames; // Read frame poses int next_id = 0; const TiXmlElement* frames_elem = root_elem->FirstChildElement("FramePoses"); if (all_frames) { if (frames_elem != NULL) { // FramePoses is optional for (const TiXmlElement* frame_elem = frames_elem->FirstChildElement("Frame"); frame_elem != NULL; frame_elem = frame_elem->NextSiblingElement("Frame")) { string image_file = (frames_dir/frame_elem->Attribute("name")).string(); Vec6 lnPose = stream_to<Vec6>(frame_elem->Attribute("pose")); Frame* f = new Frame(next_id++, image_file, SE3<>::exp(lnPose)); f->initializing = norm_sq(lnPose) == 0; f->lost = frame_elem->Attribute("lost") == "1" || f->initializing; map.AddFrame(f); } } } else { // Read key frames const TiXmlElement* kfs_elem = root_elem->FirstChildElement("KeyFrames"); CHECK(kfs_elem) << "There was no <KeyFrames> element in the map spec."; // Create an ID-to-XMLElement map for (const TiXmlElement* kf_elem = kfs_elem->FirstChildElement("KeyFrame"); kf_elem != NULL; kf_elem = kf_elem->NextSiblingElement("KeyFrame")) { // Get the id, pose, and hash int id = lexical_cast<int>(kf_elem->Attribute("id")); Vec6 lnPose = stream_to<Vec6>(kf_elem->Attribute("pose")); string hash = kf_elem->FirstChildElement("Image")->Attribute("md5"); // Get the filename fs::path image_file; if (kf_elem->Attribute("name") != NULL) { image_file = kf_elem->Attribute("name"); } if (image_file.empty() || !*gvLoadOriginalFrames) { fallback_frames.push_back(id); image_file = xml_dir/kf_elem->FirstChildElement("Image")->Attribute("file"); } else { image_file = frames_dir/image_file; } // Add the keyframe Frame* frame = new Frame(id, image_file.string(), SE3<>::exp(lnPose)); map.AddFrame(frame); // Add the measurements const TiXmlElement* msms_elem = kf_elem->FirstChildElement("Measurements"); for (const TiXmlElement* msm_elem = msms_elem->FirstChildElement("Measurement"); msm_elem != NULL; msm_elem = msm_elem->NextSiblingElement("Measurement")) { Measurement msm; int point_id = lexical_cast<int>(msm_elem->Attribute("id")); std::map<int, int>::const_iterator it = points_id_to_index.find(point_id); if (it == points_id_to_index.end()) { DLOG << "No point with ID="<<point_id; } else { msm.point_index = it->second; msm.image_pos = stream_to<Vec2>(msm_elem->Attribute("v2RootPos")); msm.retina_pos = stream_to<Vec2>(msm_elem->Attribute("v2ImplanePos")); msm.pyramid_level = lexical_cast<int>(msm_elem->Attribute("nLevel")); } frame->measurements.push_back(msm); } } } DLOG << "Loaded " << map.points.size() << " points and " << map.frames.size() << " frames"; }