int main(int argc, char **argv) {
int c;
static struct option longopts[] = {
{"feature",required_argument,NULL,'f'},
};
char *fep = NULL;
while((c = getopt_long(argc,argv,"f:",longopts,NULL)) != -1) {
switch(c) {
case 'f':
fep = optarg;
break;
case '?':
usage();
break;
}
}
if(!fep) {
usage();
}
feature_obj *fo = feature_reader_create(fep);
if(!fo) {
printf("Sorry: unable to create a feature reader object from the file path %s\n",fep);
}
char *formatters[2] = { "iPhone","Android"};
int res = csv_writer(fo,formatters,2);
return 0;
}
TEST(CsvWriter, TestWriteLine) {
// Write a bunch of lines to a csv file.
CsvWriter csv_writer(csv_write_file);
EXPECT_TRUE(csv_writer.IsOpen());
std::vector<int> line1 = {1, 2, 3, 4, 5};
std::vector<double> line2 = {6.0, 7.0, 8.0, 9.0, 10.0};
std::vector<std::string> line3 = {"a", "b", "c", "d", "e"};
Eigen::Matrix<double, 5, 1> line4;
line4 << 11.0, 12.0, 13.0, 14.0, 15.0;
EXPECT_TRUE(csv_writer.WriteLine(line1));
EXPECT_TRUE(csv_writer.WriteLine(line2));
EXPECT_TRUE(csv_writer.WriteLine(line3));
EXPECT_TRUE(csv_writer.WriteLine(line4));
EXPECT_TRUE(csv_writer.Close());
// Read back the lines from the csv file.
CsvReader csv_reader(csv_write_file);
EXPECT_TRUE(csv_reader.IsOpen());
EXPECT_TRUE(csv_reader.HasMoreLines());
// First line.
std::vector<std::string> tokenized_line;
EXPECT_TRUE(csv_reader.ReadLine(&tokenized_line));
EXPECT_EQ(line1.size(), tokenized_line.size());
for (size_t ii = 0; ii < tokenized_line.size(); ++ii) {
EXPECT_EQ(line1[ii], std::stoi(tokenized_line[ii]));
}
// Second line.
EXPECT_TRUE(csv_reader.ReadLine(&tokenized_line));
EXPECT_EQ(line2.size(), tokenized_line.size());
for (size_t ii = 0; ii < tokenized_line.size(); ++ii) {
EXPECT_EQ(line2[ii], std::stod(tokenized_line[ii]));
}
// Third line.
EXPECT_TRUE(csv_reader.ReadLine(&tokenized_line));
EXPECT_EQ(line3.size(), tokenized_line.size());
for (size_t ii = 0; ii < tokenized_line.size(); ++ii) {
EXPECT_EQ(line3[ii], tokenized_line[ii].c_str());
}
// Fourth line.
EXPECT_TRUE(csv_reader.ReadLine(&tokenized_line));
EXPECT_EQ(line4.size(), tokenized_line.size());
for (size_t ii = 0; ii < tokenized_line.size(); ++ii) {
EXPECT_EQ(line4(ii), std::stod(tokenized_line[ii]));
}
// Delete the file.
std::remove(csv_write_file.c_str());
}
Status KeyframeVisualOdometry::WriteMapToFile(
const std::string& filename) const {
if (tracks_.empty() && frozen_landmarks_.empty())
return Status::Cancelled("No tracks or landmarks to write.");
file::CsvWriter csv_writer(filename);
if (!csv_writer.IsOpen())
return Status::FailedPrecondition("Invalid filename.");
// Iterate over all current tracks and frozen landmarks, storing:
// p.x, p.y, p.z
// where p is the 3D point.
// First get all triangulated features that are currently being tracked.
util::ProgressBar progress("Writing map",
tracks_.size() + frozen_landmarks_.size());
for (size_t ii = 0; ii < tracks_.size(); ++ii) {
progress.Update(ii);
Landmark::Ptr track = Landmark::GetLandmark(tracks_[ii]);
CHECK_NOTNULL(track.get());
if (!track->IsEstimated())
continue;
std::vector<double> point_info = {track->Position().X(),
track->Position().Y(),
track->Position().Z()};
csv_writer.WriteLine(point_info);
}
progress.Update(tracks_.size());
// Now repeat for all frozen landmarks.
for (size_t ii = 0; ii < frozen_landmarks_.size(); ++ii) {
progress.Update(tracks_.size() + ii);
Landmark::Ptr landmark = Landmark::GetLandmark(frozen_landmarks_[ii]);
CHECK_NOTNULL(landmark.get());
if (!landmark->IsEstimated())
continue;
std::vector<double> point_info = {landmark->Position().X(),
landmark->Position().Y(),
landmark->Position().Z()};
csv_writer.WriteLine(point_info);
}
progress.Update(tracks_.size() + frozen_landmarks_.size());
if (!csv_writer.Close()) {
return Status::Unknown("Failed to close csv file.");
}
return Status::Ok();
}
TEST(CsvWriter, TestWriteLines) {
// Write a bunch of Eigen vectors as lines to a csv file.
CsvWriter csv_writer(csv_write_file);
EXPECT_TRUE(csv_writer.IsOpen());
// Make 1000 random 100-dimensional vectors.
std::vector<Eigen::VectorXd> lines;
for (int ii = 0; ii < 1000; ++ii) {
lines.push_back(Eigen::Matrix<double, 100, 1>::Random());
}
// Store the vectors in the csv file.
EXPECT_TRUE(csv_writer.WriteLines(lines));
EXPECT_TRUE(csv_writer.Close());
// Read back the lines from the csv file.
CsvReader csv_reader(csv_write_file);
EXPECT_TRUE(csv_reader.IsOpen());
EXPECT_TRUE(csv_reader.HasMoreLines());
// Make sure all lines match the original vector.
int line_number = 0;
std::vector<std::string> tokenized_line;
while (csv_reader.HasMoreLines()) {
EXPECT_TRUE(csv_reader.ReadLine(&tokenized_line));
EXPECT_EQ(100, tokenized_line.size());
for (size_t ii = 0; ii < tokenized_line.size(); ++ii) {
EXPECT_NEAR(lines[line_number](ii),
std::stod(tokenized_line[ii]),
1e-4);
}
line_number++;
}
// Delete the file.
std::remove(csv_write_file.c_str());
}
Status KeyframeVisualOdometry::WriteTrajectoryToFile(
const std::string& filename) const {
file::CsvWriter csv_writer(filename);
if (!csv_writer.IsOpen())
return Status::FailedPrecondition("Invalid filename.");
// Iterate over all views, storing camera poses to the file. Output format
// on each line will be:
// view index, t.x, t.y, t.z, r.x, r.y, r.z
// where rotations are expressed in axis-angle format.
util::ProgressBar progress("Writing trajectory", view_indices_.size());
for (size_t ii = 0; ii < view_indices_.size(); ++ii) {
progress.Update(ii);
const View::Ptr view = View::GetView(view_indices_[ii]);
CHECK_NOTNULL(view.get());
const Vector3d t = view->Camera().Translation();
const Vector3d r = view->Camera().AxisAngleRotation();
std::vector<double> camera_info = {static_cast<double>(view_indices_[ii])};
camera_info.push_back(t(0));
camera_info.push_back(t(1));
camera_info.push_back(t(2));
camera_info.push_back(r(0));
camera_info.push_back(r(1));
camera_info.push_back(r(2));
csv_writer.WriteLine(camera_info);
}
progress.Update(view_indices_.size());
if (!csv_writer.Close()) {
return Status::Unknown("Failed to close csv file.");
}
return Status::Ok();
}
