Function load_CSV_pose_with_time

src/STDesc.cpp:196–244

}

void load_CSV_pose_with_time(
    const std::string &file_path, std::vector<int> &index_vec,
    std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d>> &poses_vec,
    std::vector<std::string> &times_vec) {
  std::ifstream fp(file_path);
  std::string line;
  std::getline(fp, line); // get ride of the first line
  Eigen::Matrix<double, 1, 7> temp_matrix;

  while (std::getline(fp, line)) {
    std::stringstream sin(line);
    std::string info;
    int number = 0;
    while (std::getline(sin, info, ',')) {
      if (number == 0) {
        int index;
        std::stringstream data;
        data << info;
        data >> index;
        index_vec.push_back(index);
        number++;
      } else if (number == 1) {
        std::string time;
        std::stringstream data;
        data << info;
        data >> time;
        times_vec.push_back(time);
        number++;
      } else {
        double p;
        std::stringstream data;
        data << info;
        data >> p;
        temp_matrix[number - 2] = p;
        if (number == 8) {
          Eigen::Vector3d translation(temp_matrix[0], temp_matrix[1],
                                      temp_matrix[2]);
          Eigen::Quaterniond q(temp_matrix[6], temp_matrix[3], temp_matrix[4],
                               temp_matrix[5]);
          std::pair<Eigen::Vector3d, Eigen::Matrix3d> single_pose;
          single_pose.first = translation;
          single_pose.second = q.toRotationMatrix();
          poses_vec.push_back(single_pose);
        }
        number++;
      }
    }
  }
}

double time_inc(std::chrono::_V2::system_clock::time_point &t_end,
                std::chrono::_V2::system_clock::time_point &t_begin) {