#include <Eigen/Geometry> // 用于四元数
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char **argv) {
  // 创建PCL可视化对象
  pcl::visualization::PCLVisualizer::Ptr viewer(
      new pcl::visualization::PCLVisualizer("3D Trajectory Viewer"));
  // 设置背景色
  viewer->setBackgroundColor(0, 0, 0);

  // 添加全局坐标系
  viewer->addCoordinateSystem(1.0); // 1.0表示坐标轴的长度

  // 定义点云类型
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  // 假设轨迹点云数据已经加载到cloud中，并且有相应的方向数据
  cloud->width = 10;
  cloud->height = 1;
  cloud->points.resize(cloud->width * cloud->height);

  std::vector<Eigen::Quaternionf> orientations;
  for (size_t i = 0; i < cloud->points.size(); ++i) {
    cloud->points[i].x = i * 0.5;
    cloud->points[i].y = sin(i * 0.5);
    cloud->points[i].z = cos(i * 0.5);
    // 随机生成四元数
    Eigen::Quaternionf q;
    q = Eigen::AngleAxisf(i * 0.1, Eigen::Vector3f::UnitX()) *
        Eigen::AngleAxisf(M_PI / 2.0, Eigen::Vector3f::UnitY()) *
        Eigen::AngleAxisf(i * 0.1, Eigen::Vector3f::UnitZ());
    orientations.push_back(q);
  }
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map(
      new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile("./temp_data/marker_map.pcd", *cloud_map);
  for (size_t i = 0; i < cloud->points.size(); ++i) {
    pcl::PointXYZ point = cloud->points[i];
    Eigen::Quaternionf q = orientations[i];
    // 创建变换矩阵
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << point.x, point.y, point.z;
    transform.rotate(q);

    std::string id = "pose_" + std::to_string(i);
    viewer->addCoordinateSystem(0.2, transform,
                                id); // 0.2表示每个pose坐标轴的长度
  }
  *cloud = *cloud + *cloud_map;
  viewer->addPointCloud<pcl::PointXYZ>(cloud, "trajectory");
  // 设置相机位置和角度
  viewer->initCameraParameters();
  viewer->setCameraPosition(0, 0, 15, 0, -1, 0);

  // 循环直到视图关闭
  while (!viewer->wasStopped()) {
    viewer->spinOnce(100);
  }

  return 0;
}