//
// Created by zx on 23-8-4.
//

#include "keyFramesManager.h"
#include <pcl/common/transforms.h>
keyFrame::keyFrame(PointCloudXYZI::Ptr cloud) {
  cloud_=cloud;
  cloud_world_.reset(new PointCloudXYZI());
  pose_changed_=true;
}
keyFrame::keyFrame(PointTypePose pose, PointCloudXYZI::Ptr cloud) {
  cloud_=cloud;
  cloud_world_.reset(new PointCloudXYZI());
  pose_changed_=true;

}
keyFrame::~keyFrame() {}

void keyFrame::ResetPose(PointTypePose pose){
  pose_=pose;
  pose_changed_=true;
}


PointCloudXYZI::Ptr keyFrame::world_cloud(){
  if(pose_changed_==false){
    return cloud_world_;
  }
  int cloudSize = cloud_->size();
  cloud_world_->resize(cloudSize);

  Eigen::Affine3f transCur = pcl::getTransformation(pose_.x, pose_.y, pose_.z, pose_.roll, pose_.pitch, pose_.yaw);

  int numberOfCores=4;
#pragma omp parallel for num_threads(numberOfCores)
  for (int i = 0; i < cloudSize; ++i)
  {
    const auto &pointFrom = cloud_->points[i];
    cloud_world_->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
    cloud_world_->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
    cloud_world_->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
    cloud_world_->points[i].intensity = pointFrom.intensity;
  }
  pose_changed_=false;
  return cloud_world_;
}

pcl::PointCloud<PointTypePose>::Ptr keyFramesManager::GetPoses(){
  pcl::PointCloud<PointTypePose>::Ptr ret(new pcl::PointCloud<PointTypePose>);
  ret->resize(frames_.size());
  for(int i=0;i<frames_.size();++i){
    ret->points[i]=frames_[i].Pose();
  }
  return ret;
}