compare_poses.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#include <fstream>
#include <iomanip>
 
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
 
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "yaml-cpp/yaml.h"
 
#include "cyber/common/log.h"
#include "modules/common/math/quaternion.h"
 
#include "modules/localization/msf/common/io/velodyne_utility.h"
 
using ::apollo::common::EigenAffine3dVec;
using ::apollo::common::EigenVector3dVec;
 
static bool LoadGnssAntennaExtrinsic(const std::string &file_path,
                                     Eigen::Vector3d *imu_ant_offset) {
  CHECK_NOTNULL(imu_ant_offset);
 
  YAML::Node config = YAML::LoadFile(file_path);
  if (config["leverarm"]) {
    if (config["leverarm"]["primary"]["offset"]) {
      (*imu_ant_offset)[0] =
          config["leverarm"]["primary"]["offset"]["x"].as<double>();
      (*imu_ant_offset)[1] =
          config["leverarm"]["primary"]["offset"]["y"].as<double>();
      (*imu_ant_offset)[2] =
          config["leverarm"]["primary"]["offset"]["z"].as<double>();
    }
    return true;
  }
  return false;
}
 
static void PoseAndStdInterpolationByTime(
    const EigenAffine3dVec &in_poses, const EigenVector3dVec &in_stds,
    const std::vector<double> &in_timestamps,
    const std::vector<double> &ref_timestamps,
    std::map<unsigned int, Eigen::Affine3d> *out_poses,
    std::map<unsigned int, Eigen::Vector3d> *out_stds) {
  unsigned int index = 0;
  for (size_t i = 0; i < ref_timestamps.size(); ++i) {
    double ref_timestamp = ref_timestamps[i];
    while (index < in_timestamps.size() &&
           in_timestamps.at(index) < ref_timestamp) {
      ++index;
    }
 
    if (index < in_timestamps.size()) {
      if (index >= 1) {
        double cur_timestamp = in_timestamps[index];
        double pre_timestamp = in_timestamps[index - 1];
        DCHECK_NE(cur_timestamp, pre_timestamp);
 
        double t =
            (cur_timestamp - ref_timestamp) / (cur_timestamp - pre_timestamp);
        DCHECK_GE(t, 0.0);
        DCHECK_LE(t, 1.0);
 
        Eigen::Affine3d pre_pose = in_poses[index - 1];
        Eigen::Affine3d cur_pose = in_poses[index];
        Eigen::Quaterniond pre_quatd(pre_pose.linear());
        Eigen::Translation3d pre_transd(pre_pose.translation());
        Eigen::Quaterniond cur_quatd(cur_pose.linear());
        Eigen::Translation3d cur_transd(cur_pose.translation());
 
        Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd);
 
        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);
        (*out_poses)[static_cast<unsigned int>(i)] = re_transd * res_quatd;
 
        Eigen::Vector3d pre_std = in_stds[index - 1];
        Eigen::Vector3d cur_std = in_stds[index];
        Eigen::Vector3d std;
        std[0] = pre_std[0] * t + cur_std[0] * (1 - t);
        std[1] = pre_std[1] * t + cur_std[1] * (1 - t);
        std[2] = pre_std[2] * t + cur_std[2] * (1 - t);
        (*out_stds)[static_cast<unsigned int>(i)] = std;
      }
    } else {
      AERROR << "[ERROR] No more poses. Exit now.";
      break;
    }
  }
}
 
int main(int argc, char **argv) {
  boost::program_options::options_description boost_desc("Allowed options");
  boost_desc.add_options()("help", "produce help message")(
      "in_folder", boost::program_options::value<std::string>(),
      "provide the output folder")("loc_file_a",
                                   boost::program_options::value<std::string>(),
                                   "provide gnss localization file.")(
      "loc_file_b", boost::program_options::value<std::string>(),
      "provide lidar localization file.")(
      "compare_file", boost::program_options::value<std::string>(),
      "provide compare file.")(
      "imu_to_ant_offset_file",
      boost::program_options::value<std::string>()->default_value(""),
      "provide imu to ant offset file.");
 
  boost::program_options::variables_map boost_args;
  boost::program_options::store(
      boost::program_options::parse_command_line(argc, argv, boost_desc),
      boost_args);
  boost::program_options::notify(boost_args);
 
  if (boost_args.count("help") || !boost_args.count("in_folder")) {
    AERROR << boost_desc << std::endl;
    return 0;
  }
 
  const std::string in_folder = boost_args["in_folder"].as<std::string>();
 
  const std::string loc_file_a =
      in_folder + "/" + boost_args["loc_file_a"].as<std::string>();
  const std::string loc_file_b =
      in_folder + "/" + boost_args["loc_file_b"].as<std::string>();
  const std::string compare_file =
      in_folder + "/" + boost_args["compare_file"].as<std::string>();
  const std::string imu_to_ant_offset_file =
      boost_args["imu_to_ant_offset_file"].as<std::string>();
 
  Eigen::Vector3d imu_ant_offset = Eigen::Vector3d::Zero();
  if (imu_to_ant_offset_file != "") {
    if (!LoadGnssAntennaExtrinsic(imu_to_ant_offset_file, &imu_ant_offset)) {
      return 0;
    }
  }
 
  EigenAffine3dVec poses_a;
  EigenVector3dVec stds_a;
  std::vector<double> timestamps_a;
  apollo::localization::msf::velodyne::LoadPosesAndStds(loc_file_a, &poses_a,
                                                        &stds_a, &timestamps_a);
  if (poses_a.empty()) {
    return 0;
  }
 
  EigenAffine3dVec poses_b;
  EigenVector3dVec stds_b;
  std::vector<double> timestamps_b;
  apollo::localization::msf::velodyne::LoadPosesAndStds(loc_file_b, &poses_b,
                                                        &stds_b, &timestamps_b);
  if (poses_b.empty()) {
    return 0;
  }
 
  std::map<unsigned int, Eigen::Affine3d> out_poses_a;
  std::map<unsigned int, Eigen::Vector3d> out_stds_a;
  std::map<unsigned int, Eigen::Affine3d> out_poses_b;
  std::map<unsigned int, Eigen::Vector3d> out_stds_b;
 
  PoseAndStdInterpolationByTime(poses_b, stds_b, timestamps_b, timestamps_a,
                                &out_poses_b, &out_stds_b);
 
  PoseAndStdInterpolationByTime(poses_a, stds_a, timestamps_a, timestamps_a,
                                &out_poses_a, &out_stds_a);
 
  if (out_poses_a.empty() || out_poses_b.empty()) {
    return 0;
  }
 
  std::vector<unsigned int> vec_idx;
  std::vector<double> vec_timestamp;
  std::vector<double> vec_x_diff;
  std::vector<double> vec_y_diff;
  std::vector<double> vec_z_diff;
  std::vector<double> vec_roll_diff;
  std::vector<double> vec_pitch_diff;
  std::vector<double> vec_yaw_diff;
 
  for (unsigned int idx = 0; idx < timestamps_a.size(); idx++) {
    auto pose_a_found_iter = out_poses_a.find(idx);
    if (pose_a_found_iter != out_poses_a.end()) {
      AINFO << "Find pose a.";
      const Eigen::Affine3d &pose_a = pose_a_found_iter->second;
 
      Eigen::Quaterniond quatd_a(pose_a.linear());
      Eigen::Translation3d transd_a(pose_a.translation());
      apollo::common::math::EulerAnglesZXY<double> euler_a(
          quatd_a.w(), quatd_a.x(), quatd_a.y(), quatd_a.z());
      double roll_a = euler_a.roll();
      double pitch_a = euler_a.pitch();
      double yaw_a = euler_a.yaw();
 
      auto pose_b_found_iter = out_poses_b.find(idx);
      if (pose_b_found_iter != out_poses_b.end()) {
        AINFO << "Find pose b.";
        const Eigen::Affine3d &pose_b = pose_b_found_iter->second;
        Eigen::Quaterniond quatd_b(pose_b.linear());
        Eigen::Translation3d transd_b(pose_b.translation());
        apollo::common::math::EulerAnglesZXY<double> euler_b(
            quatd_b.w(), quatd_b.x(), quatd_b.y(), quatd_b.z());
        double roll_b = euler_b.roll();
        double pitch_b = euler_b.pitch();
        double yaw_b = euler_b.yaw();
 
        Eigen::Vector3d offset = quatd_b * imu_ant_offset;
        transd_a.x() = transd_a.x() - offset[0];
        transd_a.y() = transd_a.y() - offset[1];
        transd_a.z() = transd_a.z() - offset[2];
 
        double x_diff = fabs(transd_a.x() - transd_b.x());
        double y_diff = fabs(transd_a.y() - transd_b.y());
        double z_diff = fabs(transd_a.z() - transd_b.z());
        double roll_diff = fabs(roll_a - roll_b) * 180.0 / M_PI;
        double pitch_diff = fabs(pitch_a - pitch_b) * 180.0 / M_PI;
        double yaw_diff = fabs(yaw_a - yaw_b);
        yaw_diff = std::min(yaw_diff, 2.0 * M_PI - yaw_diff) * 180.0 / M_PI;
 
        vec_idx.push_back(idx);
        vec_timestamp.push_back(timestamps_a[idx]);
        vec_x_diff.push_back(x_diff);
        vec_y_diff.push_back(y_diff);
        vec_z_diff.push_back(z_diff);
        vec_roll_diff.push_back(roll_diff);
        vec_pitch_diff.push_back(pitch_diff);
        vec_yaw_diff.push_back(yaw_diff);
      }
    }
  }
 
  if (vec_idx.empty()) {
    return 0;
  }
 
  std::ofstream file(compare_file.c_str());
  for (unsigned int i = 0; i < vec_idx.size(); ++i) {
    file << vec_idx[i] << " " << std::setprecision(13) << vec_timestamp[i]
         << " " << vec_x_diff[i] << " " << vec_y_diff[i] << " " << vec_z_diff[i]
         << " " << vec_roll_diff[i] << " " << vec_pitch_diff[i] << " "
         << vec_yaw_diff[i] << std::endl;
  }
  file.close();
}