// SPDX-License-Identifier: BSD-2-Clause

#include <hdl_graph_slam/map_cloud_generator.hpp>

#include <pcl/octree/octree_search.h>
#include <glog/logging.h>

namespace hdl_graph_slam {

MapCloudGenerator::MapCloudGenerator() {}

MapCloudGenerator::~MapCloudGenerator() {}

pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
  if(keyframes.empty()) {
    std::cerr << "warning: keyframes empty!!" << std::endl;
    return nullptr;
  }

  // 问题定位到这里, 坐标转换没算对.
  // LOG(INFO) << "关键帧数量  keyframes.size() = " << keyframes.size();

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());    // 创建一个新的点云对象 cloud
  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());  // 预留足够的内存空间，以容纳所有关键帧的点云数据

  for(const auto& keyframe : keyframes) {
    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();  // 每一帧关键帧相对第一帧的 pose 的齐次变换矩阵      这个齐次变换矩阵是对的
    // std::cout << "pose: ===================:" << std::endl;
    // std::cout << pose << std::endl;
    // int ii = 0;
    for(const auto& src_pt : keyframe->cloud->points) {
      PointT dst_pt;
      // std::cout << ii++ << " ";
      // dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap(); // ?
      // 进行变换
      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
      // dst_pt.getVector4fMap() = src_pt.getVector4fMap();
      dst_pt.intensity = src_pt.intensity;  // 将源点的强度信息复制到目标点中。这通常用于保持激光点云的强度数据
      cloud->push_back(dst_pt);             // 将变换后的点 dst_pt 添加到合并的点云 cloud 中
    }
  }
  // LOG(INFO)<<"gg";
  cloud->width = cloud->size();
  cloud->height = 1;
  cloud->is_dense = false;

  if(resolution <= 0.0) return cloud;  // To get unfiltered point cloud with intensity

  pcl::octree::OctreePointCloud<PointT> octree(resolution);
  octree.setInputCloud(cloud);
  octree.addPointsFromInputCloud();

  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
  octree.getOccupiedVoxelCenters(filtered->points);

  filtered->width = filtered->size();
  filtered->height = 1;
  filtered->is_dense = false;

  return filtered;
}

}  // namespace hdl_graph_slam
