kittpose2tum.cpp

kittpose2tum.cpp

#include <iostream>
#include <string>
#include <vector>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <tools/nvutils.h>

#include "tools/pose_utils.hpp"
using namespace std;

void loadSuMaPoses(string txt, vector<Eigen::Matrix4f>& poses) {
  // Pose is transformed into lidar pose!

  poses.clear();
  poses.reserve(2000);
  std::ifstream in(txt);
  std::string line;

  int count = 0;
  while (std::getline(in, line)) {
    vector<float> pose    = pose_utils::splitLine(line, ' ');
    Eigen::Matrix4f tf4x4 = Eigen::Matrix4f::Identity();  // Crucial!
    pose_utils::vec2tf4x4(pose, tf4x4);
    poses.emplace_back(tf4x4);
    count++;
  }
  std::cout << "Total " << count << " poses are loaded" << std::endl;
}
int main() {
  vector<Eigen::Matrix4f> poses;
  int iter         = 5;
  string suma_path = "/home/beomsoo/suma_poses_itv" + to_string(iter) + ".txt";
  loadSuMaPoses(suma_path, poses);

  std::string output_path = "/home/beomsoo/suma_poses_tum_itv" + to_string(iter) + ".txt";
  std::ofstream output_obj(output_path);

  cout << "poses:" << poses.size() << endl;
  int idx = 0;
  for (auto& pose : poses) {
    std::ofstream output_obj(output_path, ios::app);
    cout << idx << endl;
    pose_utils::Pose3 geo_pose(pose);
    pose_utils::writePose(output_obj, idx, geo_pose);
    idx = idx + iter;
  }
  cout << "Comeplete" << endl;
  return 0;
}