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";
}
