43 #include <pcl/point_cloud.h>
44 #include <pcl/io/ply_io.h>
50 #include <pcl/gpu/kinfu_large_scale/device.h>
52 #include <pcl/gpu/kinfu_large_scale/float3_operations.h>
53 #include <pcl/gpu/containers/device_array.h>
54 #include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
55 #include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
56 #include <pcl/gpu/kinfu_large_scale/color_volume.h>
57 #include <pcl/gpu/kinfu_large_scale/raycaster.h>
59 #include <pcl/gpu/kinfu_large_scale/cyclical_buffer.h>
85 performLastScan (){perform_last_scan_ =
true; PCL_WARN (
"Kinfu will exit after next shift\n");}
97 KinfuTracker (
const Eigen::Vector3f &volumeSize,
const float shiftingDistance,
int rows = 480,
int cols = 640);
106 setDepthIntrinsics (
float fx,
float fy,
float cx = -1,
float cy = -1);
112 setInitialCameraPose (
const Eigen::Affine3f& pose);
119 setDepthTruncationForICP (
float max_icp_distance = 0.f);
126 setIcpCorespFilteringParams (
float distThreshold,
float sineOfAngle);
133 setCameraMovementThreshold(
float threshold = 0.001f);
139 initColorIntegration(
int max_weight = -1);
153 bool operator() (
const DepthMap& depth);
160 bool operator() (
const DepthMap& depth,
const View& colors);
167 getCameraPose (
int time = -1)
const;
170 getLastEstimatedPose ()
const;
174 getNumberOfPoses ()
const;
192 getImage (View& view)
const;
212 return (cyclical_.getBuffer ());
218 extractAndSaveWorld ();
234 disable_icp_ = !disable_icp_;
235 PCL_WARN(
"ICP is %s\n", !disable_icp_?
"ENABLED":
"DISABLED");
242 return (has_shifted_);
252 allocateBufffers (
int rows_arg,
int cols_arg);
263 using Matrix3frm = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
264 using Vector3f = Eigen::Vector3f;
277 convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in_1, Eigen::Vector3f& translation_in_2,
289 convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in,
299 convertTransforms (Matrix3frm& transform_in, Eigen::Vector3f& translation_in,
322 performICP(
const pcl::device::kinfuLS::Intr& cam_intrinsics, Matrix3frm& previous_global_rotation, Vector3f& previous_global_translation, Matrix3frm& current_global_rotation, Vector3f& current_global_translation);
334 performPairWiseICP(
const pcl::device::kinfuLS::Intr cam_intrinsics, Matrix3frm& resulting_rotation, Vector3f& resulting_translation);
341 CyclicalBuffer cyclical_;
353 float max_icp_distance_;
356 float fx_, fy_, cx_, cy_;
365 Matrix3frm init_Rcam_;
371 int icp_iterations_[LEVELS];
380 std::vector<DepthMap> depths_curr_;
383 std::vector<MapArr> vmaps_g_curr_;
386 std::vector<MapArr> nmaps_g_curr_;
389 std::vector<MapArr> vmaps_g_prev_;
392 std::vector<MapArr> nmaps_g_prev_;
395 std::vector<MapArr> vmaps_curr_;
398 std::vector<MapArr> nmaps_curr_;
401 std::vector<MapArr> vmaps_prev_;
404 std::vector<MapArr> nmaps_prev_;
407 std::vector<CorespMap> coresps_;
419 std::vector<Matrix3frm> rmats_;
422 std::vector<Vector3f> tvecs_;
425 float integration_metric_threshold_;
428 bool perform_last_scan_;
434 float shifting_distance_;
443 Matrix3frm last_estimated_rotation_;
446 Vector3f last_estimated_translation_;