[do not merge] SLAM experiments

cli/cli.cpp

@@ -13,7 +13,7 @@
 
 /* The minimum sets of parameters in CLI mode (to pass verification) are:
  *   new + project_name + project_path + images_path + action
- *   open + action
+ *   open + project_path + action
  *   temp + images_path + action
  */
 
@@ -45,8 +45,8 @@ bool VerifyCommandlineArguments() {
     return false;
   }
 
-  // TODO(uladbohdan): to support more actions.
-  if (FLAGS_action != "reconstruction") {
+  // TODO(uladbohdan): to rename 'slam' action.
+  if (FLAGS_action != "reconstruction" && FLAGS_action != "slam") {
     LOG(ERROR) << "You must pass a valid action to proceed.";
     return false;
   }
@@ -82,7 +82,7 @@ void RunCLI() {
   LOG(INFO) << "Running in CLI mode.";
 
   if (!VerifyCommandlineArguments()) {
-    LOG(ERROR) << "CL arguments verification failed.";
+    LOG(ERROR) << "Command Line arguments verification failed.";
     return;
   }
 
@@ -100,18 +100,22 @@ void RunCLI() {
     QString temp_project_name =
         QString("appa-project-") +
         QDateTime::currentDateTime().toString(Qt::ISODate);
-    temp_project_path = QDir::temp().filePath(temp_project_name);
+    temp_project_path = "/home/parallels/tmp/" + temp_project_name; // QDir::temp().filePath(temp_project_name);
     LOG(INFO) << "Temp project will be created in "
               << temp_project_path.toStdString();
     project = new Project(temp_project_name,
-                          temp_project_path,
+                          "/home/parallels/tmp",
                           QString::fromStdString(FLAGS_images_path),
                           QString::fromStdString(FLAGS_output_path));
   }
 
   if (FLAGS_action == "reconstruction") {
     LOG(INFO) << "Starting reconstruction...";
     project->BuildModelToBinary();
+  } else if (FLAGS_action == "slam") {
+    LOG(INFO) << "Performing SLAM experiments...";
+    project->Load_SLAM_data();
+    project->SLAM_Build();
   }
 
   if (FLAGS_cli_mode == "temp") {

main.cpp

@@ -23,7 +23,8 @@ int main(int argc, char* argv[]) {
     awesome->initFontAwesome();
 
     MainWindow w;
-    w.SetIcons(awesome);
+    // Temporary avoiding rendering icons.
+    // w.SetIcons(awesome);
     w.show();
 
     return a.exec();

src/io/projectio.cpp

@@ -1,12 +1,22 @@
 // Copyright 2017 Lybros.
 
+#include <algorithm>
+#include <unordered_map>
+#include <vector>
+
 #include "projectio.h"
 
 #include "../project.h"
 
 #include <QDir>
 #include <QFile>
+#include <QString>
 #include <QTextStream>
+#include <QVector>
+
+#include <Eigen/Geometry>
+
+#include <theia/sfm/camera/camera.h>
 
 ProjectIO::ProjectIO(Project *project) : project_(project), pr(project) {
 }
@@ -158,3 +168,183 @@ bool ProjectIO::ReadOutputLocation(QTextStream& stream) {
       << "Failed to initialize output location from config file.";
   return true;
 }
+
+bool ProjectIO::ReadCameras() {
+  QFile trajectoryFile("/home/parallels/co-graph/KeyFrameTrajectory.txt");
+
+  // TUM format: The format of each line is 'timestamp tx ty tz qx qy qz qw'
+
+  LOG(INFO) << "Now we are reading KeyFrameTrajectory file...";
+
+  if (!trajectoryFile.open(QIODevice::ReadOnly)) { return false; }
+
+  QVector<QString>& imageNames = pr->GetStorage()->GetImages();
+
+  QTextStream stream(&trajectoryFile);
+  while (!stream.atEnd()) {
+    double ts;
+    stream >> ts; // the timestamp.
+    double tx, ty, tz, qx, qy, qz, qw;
+    stream >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
+
+    // tx ty tz are world camera position. Might be enought to perform the
+    // experiment.
+
+    theia::Camera* camera = new theia::Camera();
+    camera->SetFocalLength(517.);
+    camera->SetPrincipalPoint(318.643040, 255.313989);
+    camera->SetImageSize(640, 480);
+
+    camera->SetPosition(Eigen::Vector3d(tx, ty, tz));
+//    LOG(INFO) << "Camera: " << camera.GetPosition() << "  PUSHED.";
+    auto q = Eigen::Quaterniond{qx, qy, qz, qw};
+    auto rotMatrix = q.normalized().toRotationMatrix();
+    LOG(INFO) << rotMatrix;
+    camera->SetOrientationFromRotationMatrix(rotMatrix);
+
+    pr->slam_cameras_.push_back(*camera);
+    if (ts > 0.0001) {
+//      LOG(INFO) << "TS " << ts << " segfault?";
+      QString image_name = imageNames[(int)ts - 1];
+      pr->slam_camera_names_.push_back(image_name);
+      std::string short_name = FileNameFromPath(image_name).toStdString();
+      pr->slam_cameras_map_[short_name] = *camera;
+      LOG(INFO) << "OK so for image " << short_name << " camera POS is "
+                << pr->slam_cameras_map_[short_name].GetPosition();
+    }
+  }
+
+  // to avoid adding fake (0,0,0) camera in the end.
+  pr->slam_cameras_.pop_back();
+
+  LOG(INFO) << "SLAM CAMERAS PUSHED: " << pr->slam_cameras_.size();
+}
+
+bool ProjectIO::ReadSlamData() {
+  ReadCameras();
+
+  QFile slamFile("/home/parallels/co-graph/co-graph.txt");
+
+  LOG(INFO) << "in ProjectIO class.";
+
+  if (!slamFile.open(QIODevice::ReadOnly)) { return false; }
+
+  QTextStream stream(&slamFile);
+  QString temp_line;
+
+  int number_of_keyframes;
+
+  stream >> temp_line; // NUMBER_OF_KEYFRAMES
+  stream >> number_of_keyframes;
+
+  LOG(INFO) << "Number of keyframes read: " << number_of_keyframes;
+
+  std::vector<int> kfIDs;
+
+  // maps ID of a world point to list of KF IDs where it is observed from.
+  std::unordered_map<int, std::vector<int>> observations;
+
+  // this maps keyframeID : worldPointID : 2d_coordinates
+  std::unordered_map<int, std::unordered_map<int, std::pair<int,int>>> image_coordinates;
+
+  QVector<QString>& imageNames = pr->GetStorage()->GetImages();
+
+  for (int i = 0; i < number_of_keyframes; i++) {
+    stream >> temp_line; // KF
+    int kfId;
+    stream >> kfId;
+    stream >> temp_line; // COVISIBLE_WITH
+    int covisibleWith;
+    stream >> covisibleWith;
+    for (int j = 0; j < covisibleWith; j++) {
+      stream >> temp_line;
+      // Seems like I do not need this information at the moment.
+    }
+    stream >> temp_line; // NUMBER_OF_POINTS
+    int number_points;
+    stream >> number_points;
+//    LOG(INFO) << "number of points: " << number_points;
+    for (int j = 0; j < number_points; j++) {
+      int pointId, x, y;
+      stream >> pointId >> x >> y;
+//      LOG(INFO) << "about to update maps...";
+      image_coordinates[kfId][pointId] = std::make_pair(x, y);
+      observations[kfId].push_back(pointId);
+//      LOG(INFO) << "done.";
+
+      pr->slam_observations_[pointId].push_back(
+            Observation(pointId,
+                        FileNameFromPath(imageNames[kfId - 1]).toStdString(),
+                        x, y)
+          );
+    }
+    kfIDs.push_back(kfId);
+  }
+
+  for (auto it = observations.begin(); it != observations.end(); it++) {
+    std::sort(it->second.begin(), it->second.end());
+  }
+
+  std::sort(kfIDs.begin(), kfIDs.end());
+
+  LOG(INFO) << "all sorted.";
+
+  int LARGE_CONST = 10000;
+  int MIN_SIZE_THRESHOLD = 10;
+
+  for (int i = 0; i < kfIDs.size(); i++) {
+    for (int j = i+1; j < kfIDs.size(); j++) {
+      int kf1 = kfIDs[i];
+//      LOG(INFO) << "First point vector: kf1=" << kf1;
+//      LOG(INFO) << "Size: " << observations[kf1].size();
+//      for (auto it = observations[kf1].begin(); it != observations[kf1].end(); it++) {
+//        LOG(INFO) << *it;
+//      }
+      int kf2 = kfIDs[j];
+      int pairID = kf1 * LARGE_CONST + kf2;
+      std::vector<int> merged(2000);
+      std::vector<int>::iterator it = std::set_intersection(
+            observations[kf1].begin(), observations[kf1].end(),
+            observations[kf2].begin(), observations[kf2].end(), merged.begin());
+      merged.resize(it-merged.begin());
+
+//      LOG(INFO) << kf1 << ":" << kf2 << " merged.";
+
+      if (merged.size() < MIN_SIZE_THRESHOLD) {
+//        LOG(INFO) << kf1 << ":" << kf2 << " MERGED SIZE TOO SMALL " << merged.size();
+        continue;
+      }
+
+      theia::ImagePairMatch pMatch;
+
+      for (int k = 0; k < merged.size(); k++) {
+//        LOG(INFO) << "about to push...";
+        pMatch.correspondences.push_back(
+              theia::FeatureCorrespondence(
+                theia::Feature(image_coordinates[kf1][merged[k]].first,
+                               image_coordinates[kf1][merged[k]].second),
+                theia::Feature(image_coordinates[kf2][merged[k]].first,
+                               image_coordinates[kf2][merged[k]].second))
+            );
+//        LOG(INFO) << "done.";
+      }
+//      LOG(INFO) << "???";
+
+      pMatch.image1 = imageNames[kf1 - 1].toStdString();
+      pMatch.image2 = imageNames[kf2 - 1].toStdString();
+
+      if (pMatch.image1.length() > 1 && pMatch.image2.length() > 1) {
+        pr->slam_data_[pairID] = pMatch;
+//        LOG(INFO) << "INFO PUSHED FOR " << kf1 << ":" << kf2
+//                  << "  which are " << pMatch.image1 << " " << pMatch.image2
+//                  << "  matches: " << merged.size() << endl;
+       } else {
+        LOG(INFO) << " ####  ALERT BROKEN NAMES";
+      }
+    }
+  }
+
+  slamFile.close();
+
+  return true;
+}

src/io/projectio.h

@@ -44,6 +44,9 @@ class ProjectIO {
   bool WriteConfigurationFile();
   bool ReadConfigurationFile();
 
+  bool ReadCameras();
+  bool ReadSlamData();
+
   ~ProjectIO();
 
  private:

src/project.cpp

@@ -111,6 +111,34 @@ QSet<theia::TrackId>* Project::SearchImage(
   return found_tracks;
 }
 
+void Project::Load_SLAM_data() {
+  LOG(INFO) << "About to start with SLAM experiments.";
+  CHECK(ReadSlamData()) << "Failed to read Experimental SLAM data.";
+
+  LOG(INFO) << "SLAM DATA size " << slam_data_.size();
+
+  LOG(INFO) << "181 data: " << slam_data_[181*10000+184].correspondences.size();
+
+  LOG(INFO) << "SLAM Camera Names size: " << slam_camera_names_.size();
+
+//  LOG(INFO) << "====\n" << "Starting with reconstruction...\n"
+//            << "====\n";
+
+  // Now building a subset of images.
+  storage_->KeepImagesSubset(slam_camera_names_);
+
+  LOG(INFO) << "Subset size:" << storage_->GetSlamImages().size();
+}
+
+void Project::SLAM_Build() {
+  Reconstructor(this).FastBuild();
+//  Reconstructor(this).SmartBuild();
+}
+
+void Project::SLAM_New_Build() {
+  Reconstructor(this).SLAM_New_Build();
+}
+
 void Project::ExtractFeatures() {
   features_->ForceExtract();
 }
@@ -153,6 +181,10 @@ bool Project::ReadConfigurationFile() {
   return ProjectIO(this).ReadConfigurationFile();
 }
 
+bool Project::ReadSlamData() {
+  return ProjectIO(this).ReadSlamData();
+}
+
 QString Project::GetConfigurationFilePath() {
   return QDir(project_path_).filePath(CONFIG_FILE_NAME);
 }