swh:1:snp:67458089460e0f4324e7769efad93379abfcf654
Tip revision: 8cf122b9c7f7403efe90dca3b0af0ebfec10b26d authored by jzhzhang on 07 January 2024, 04:10:10 UTC
Update pose_estimation.cpp
Update pose_estimation.cpp
Tip revision: 8cf122b
rosefusion.h
#ifndef ROSEFUSION_H
#define ROSEFUSION_H
#include "data_types.h"
using Matf61da = Eigen::Matrix<double, 6, 1, Eigen::DontAlign>;
namespace rosefusion {
class Pipeline {
public:
Pipeline(const CameraParameters _camera_config,
const DataConfiguration _data_config,
const ControllerConfiguration _controller_config);
~Pipeline() = default;
bool process_frame(const cv::Mat_<float>& depth_map, const cv::Mat_<cv::Vec3b>& color_map, cv::Mat& shaded_img);
void get_poses() const;
PointCloud extract_pointcloud() const;
private:
const CameraParameters camera_parameters;
const DataConfiguration data_config;
const ControllerConfiguration controller_config;
const std::vector<int> particle_leve;
float iter_tsdf;
internal::VolumeData volume;
internal::QuaternionData PST;
internal::SearchData search_data;
internal::FrameData frame_data;
Eigen::Matrix4d current_pose;
std::vector<Eigen::Matrix4d> poses;
bool previous_frame_success=false;
Matf61da initialize_search_size;
size_t frame_id;
cv::Mat last_model_frame;
};
void export_ply(const std::string& filename, const PointCloud& point_cloud);
namespace internal {
void surface_measurement(const cv::Mat_<cv::Vec3b>& color_map,
const cv::Mat_<float>& depth_map,
FrameData& frame_data,
const CameraParameters& camera_params,
const float depth_cutoff);
bool pose_estimation(const VolumeData& volume,
const QuaternionData& quaternions,
SearchData& search_data,
Eigen::Matrix4d& pose,
FrameData& frame_data,
const CameraParameters& cam_params,
const ControllerConfiguration& controller_config,
const std::vector<int> particle_level,
float * iter_tsdf,
bool * previous_frame_success,
Matf61da& initialize_search_size
);
namespace cuda {
void surface_reconstruction(const cv::cuda::GpuMat& depth_image,
const cv::cuda::GpuMat& color_image,
VolumeData& volume,
const CameraParameters& cam_params,
const float truncation_distance,
const Eigen::Matrix4d& model_view);
void surface_prediction(const VolumeData& volume,
cv::cuda::GpuMat& shading_buffer,
const CameraParameters& cam_parameters,
const float truncation_distance,
const float3 init_pos,
cv::Mat& shaded_img,
const Eigen::Matrix4d& pose);
PointCloud extract_points(const VolumeData& volume, const int buffer_size);
}
}
}
#endif