added result output

src/mapper_sim.cpp

@@ -139,6 +139,7 @@ std::string marg_data_path;
 int main(int argc, char** argv) {
   bool show_gui = true;
   std::string cam_calib_path;
+  std::string result_path;
 
   CLI::App app{"App description"};
 
@@ -150,6 +151,9 @@ int main(int argc, char** argv) {
   app.add_option("--marg-data", marg_data_path, "Path to cache folder.")
       ->required();
 
+  app.add_option("--result-path", result_path,
+                 "Path to result file where the system will write RMSE ATE.");
+
   try {
     app.parse(argc, argv);
   } catch (const CLI::ParseError& e) {
@@ -204,6 +208,17 @@ int main(int argc, char** argv) {
 
       std::this_thread::sleep_for(std::chrono::milliseconds(50));
     }
+  } else {
+    setup_points();
+    optimize();
+
+    if (!result_path.empty()) {
+      double error = alignButton();
+
+      std::ofstream os(result_path);
+      os << error << std::endl;
+      os.close();
+    }
   }
 
   return 0;

src/mapper_sim_naive.cpp

@@ -150,6 +150,7 @@ int main(int argc, char** argv) {
 
   bool show_gui = true;
   std::string cam_calib_path;
+  std::string result_path;
 
   CLI::App app{"App description"};
 
@@ -162,6 +163,9 @@ int main(int argc, char** argv) {
                  "Folder to store marginalization data.")
       ->required();
 
+  app.add_option("--result-path", result_path,
+                 "Path to result file where the system will write RMSE ATE.");
+
   try {
     app.parse(argc, argv);
   } catch (const CLI::ParseError& e) {
@@ -365,6 +369,29 @@ int main(int argc, char** argv) {
   t3.join();
   // t4.join();
 
+  if (!result_path.empty()) {
+    Eigen::vector<Eigen::Vector3d> vio_t_w_i;
+
+    auto it = vis_map.find(kf_t_ns.back());
+
+    if (it != vis_map.end()) {
+      for (const auto& t : it->second->states)
+        vio_t_w_i.emplace_back(t.translation());
+
+    } else {
+      std::cerr << "Could not find results!!" << std::endl;
+    }
+
+    BASALT_ASSERT(kf_t_ns.size() == vio_t_w_i.size());
+
+    double error =
+        basalt::alignSVD(kf_t_ns, vio_t_w_i, gt_frame_t_ns, gt_frame_t_w_i);
+
+    std::ofstream os(result_path);
+    os << error << std::endl;
+    os.close();
+  }
+
   return 0;
 }
 

src/vio_sim.cpp

@@ -159,6 +159,7 @@ int main(int argc, char** argv) {
 
   bool show_gui = true;
   std::string cam_calib_path;
+  std::string result_path;
 
   CLI::App app{"App description"};
 
@@ -171,6 +172,9 @@ int main(int argc, char** argv) {
                  "Folder to store marginalization data.")
       ->required();
 
+  app.add_option("--result-path", result_path,
+                 "Path to result file where the system will write RMSE ATE.");
+
   app.add_option("--num-points", NUM_POINTS, "Number of points in simulation.");
 
   try {
@@ -406,6 +410,15 @@ int main(int argc, char** argv) {
   t3.join();
   // t4.join();
 
+  if (!result_path.empty()) {
+    double error = basalt::alignSVD(gt_frame_t_ns, vio_t_w_i, gt_frame_t_ns,
+                                    gt_frame_t_w_i);
+
+    std::ofstream os(result_path);
+    os << error << std::endl;
+    os.close();
+  }
+
   return 0;
 }