42 #include <pcl/point_cloud.h>
43 #include <pcl/io/ply_io.h>
49 #include <pcl/gpu/kinfu_large_scale/device.h>
51 #include <pcl/gpu/kinfu_large_scale/float3_operations.h>
52 #include <pcl/gpu/containers/device_array.h>
53 #include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
54 #include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
55 #include <pcl/gpu/kinfu_large_scale/color_volume.h>
56 #include <pcl/gpu/kinfu_large_scale/raycaster.h>
58 #include <pcl/gpu/kinfu_large_scale/cyclical_buffer.h>
84 performLastScan (){perform_last_scan_ =
true; PCL_WARN (
"Kinfu will exit after next shift\n");}
96 KinfuTracker (
const Eigen::Vector3f &volumeSize,
const float shiftingDistance,
int rows = 480,
int cols = 640);
105 setDepthIntrinsics (
float fx,
float fy,
float cx = -1,
float cy = -1);
111 setInitialCameraPose (
const Eigen::Affine3f& pose);
118 setDepthTruncationForICP (
float max_icp_distance = 0.f);
125 setIcpCorespFilteringParams (
float distThreshold,
float sineOfAngle);
132 setCameraMovementThreshold(
float threshold = 0.001f);
138 initColorIntegration(
int max_weight = -1);
152 bool operator() (
const DepthMap& depth);
159 bool operator() (
const DepthMap& depth,
const View& colors);
166 getCameraPose (
int time = -1)
const;
169 getLastEstimatedPose ()
const;
173 getNumberOfPoses ()
const;
191 getImage (View& view)
const;
211 return (cyclical_.getBuffer ());
217 extractAndSaveWorld ();
233 disable_icp_ = !disable_icp_;
234 PCL_WARN(
"ICP is %s\n", !disable_icp_?
"ENABLED":
"DISABLED");
241 return (has_shifted_);
251 allocateBufffers (
int rows_arg,
int cols_arg);
262 using Matrix3frm = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
263 using Vector3f = Eigen::Vector3f;
276 convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in_1, Eigen::Vector3f& translation_in_2,
288 convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in,
298 convertTransforms (Matrix3frm& transform_in, Eigen::Vector3f& translation_in,
321 performICP(
const pcl::device::kinfuLS::Intr& cam_intrinsics, Matrix3frm& previous_global_rotation, Vector3f& previous_global_translation, Matrix3frm& current_global_rotation, Vector3f& current_global_translation);
333 performPairWiseICP(
const pcl::device::kinfuLS::Intr cam_intrinsics, Matrix3frm& resulting_rotation, Vector3f& resulting_translation);
340 CyclicalBuffer cyclical_;
352 float max_icp_distance_;
355 float fx_, fy_, cx_, cy_;
364 Matrix3frm init_Rcam_;
370 int icp_iterations_[LEVELS];
379 std::vector<DepthMap> depths_curr_;
382 std::vector<MapArr> vmaps_g_curr_;
385 std::vector<MapArr> nmaps_g_curr_;
388 std::vector<MapArr> vmaps_g_prev_;
391 std::vector<MapArr> nmaps_g_prev_;
394 std::vector<MapArr> vmaps_curr_;
397 std::vector<MapArr> nmaps_curr_;
400 std::vector<MapArr> vmaps_prev_;
403 std::vector<MapArr> nmaps_prev_;
406 std::vector<CorespMap> coresps_;
418 std::vector<Matrix3frm> rmats_;
421 std::vector<Vector3f> tvecs_;
424 float integration_metric_threshold_;
427 bool perform_last_scan_;
433 float shifting_distance_;
442 Matrix3frm last_estimated_rotation_;
445 Vector3f last_estimated_translation_;