kinfu.cpp
Go to the documentation of this file.
1 #include <kfusion/kinfu.hpp>
2 #include <kfusion/precomp.hpp>
3 #include <kfusion/internal.hpp>
4 
5 using namespace std;
6 using namespace kfusion;
7 using namespace kfusion::cuda;
8 
9 kfusion::KinFu::KinFu(const KinFuParams& params) : frame_counter_(0), params_(params), has_shifted_(false), perform_last_scan_(false), perform_shift_(false)
10  , cyclical_(params), checkForShift_(true)
11 
12 {
13  CV_Assert(params.volume_dims[0] % 32 == 0);
14 
15  volume_ = cv::Ptr<cuda::TsdfVolume>(new cuda::TsdfVolume(params_.volume_dims));
16 
17  volume_->setTruncDist(params_.tsdf_trunc_dist);
18  volume_->setMaxWeight(params_.tsdf_max_weight);
19  volume_->setSize(params_.volume_size);
20  volume_->setPose(params_.volume_pose);
21  volume_->setRaycastStepFactor(params_.raycast_step_factor);
22  volume_->setGradientDeltaFactor(params_.gradient_delta_factor);
23 
24  // initialize cyclical buffer
26 
27  icp_ = cv::Ptr<cuda::ProjectiveICP>(new cuda::ProjectiveICP());
28  icp_->setDistThreshold(params_.icp_dist_thres);
29  icp_->setAngleThreshold(params_.icp_angle_thres);
30  icp_->setIterationsNum(params_.icp_iter_num);
32  reset();
33 }
34 
36 { return params_; }
37 
39 { return params_; }
40 
42 {
43  perform_last_scan_ = true;
44 }
45 
47 { return *volume_; }
48 
50 { return *volume_; }
51 
53 { return cyclical_; }
54 
56 { return cyclical_; }
57 
59 { return *icp_; }
60 
62 { return *icp_; }
63 
65 {
66  const int LEVELS = cuda::ProjectiveICP::MAX_PYRAMID_LEVELS;
67 
68  int cols = params_.cols;
69  int rows = params_.rows;
70 
71  dists_.create(rows, cols);
72 
73  curr_.depth_pyr.resize(LEVELS);
74  curr_.normals_pyr.resize(LEVELS);
75  prev_.depth_pyr.resize(LEVELS);
76  prev_.normals_pyr.resize(LEVELS);
77 
78  curr_.points_pyr.resize(LEVELS);
79  prev_.points_pyr.resize(LEVELS);
80 
81  for(int i = 0; i < LEVELS; ++i)
82  {
83  curr_.depth_pyr[i].create(rows, cols);
84  curr_.normals_pyr[i].create(rows, cols);
85 
86  prev_.depth_pyr[i].create(rows, cols);
87  prev_.normals_pyr[i].create(rows, cols);
88 
89  curr_.points_pyr[i].create(rows, cols);
90  prev_.points_pyr[i].create(rows, cols);
91 
92  cols /= 2;
93  rows /= 2;
94  }
95 
96  depths_.create(params_.rows, params_.cols);
97  normals_.create(params_.rows, params_.cols);
98  points_.create(params_.rows, params_.cols);
99 }
100 
102 {
103  if (frame_counter_)
104  cout << "Reset" << endl;
105  //initialPose.translate(Vec3f(0, 0, params_.distance_camera_target));
106  frame_counter_ = 0;
107  poses_.clear();
108  //poses_.reserve(60000);
109  poses_.push_back(initialPose);
110  volume_->clear();
111  volume_->setPose(params_.volume_pose);
112  cyclical_.resetBuffer (volume_);
113  cyclical_.resetMesh();
114  has_shifted_=false;
115 }
116 
118 {
119  if (time > (int)poses_.size () || time < 0)
120  time = (int)poses_.size () - 1;
121  return poses_[time];
122 }
123 
125 {
126  const KinFuParams& p = params_;
127  const int LEVELS = icp_->getUsedLevelsNum();
128 
129  cuda::computeDists(depth, dists_, p.intr);
130  cuda::depthBilateralFilter(depth, curr_.depth_pyr[0], p.bilateral_kernel_size, p.bilateral_sigma_spatial, p.bilateral_sigma_depth);
131 
132  if (p.icp_truncate_depth_dist > 0)
133  kfusion::cuda::depthTruncation(curr_.depth_pyr[0], p.icp_truncate_depth_dist);
134 
135  for (int i = 1; i < LEVELS; ++i)
136  cuda::depthBuildPyramid(curr_.depth_pyr[i-1], curr_.depth_pyr[i], p.bilateral_sigma_depth);
137 
138  for (int i = 0; i < LEVELS; ++i)
139 #if defined USE_DEPTH
140  cuda::computeNormalsAndMaskDepth(p.intr, curr_.depth_pyr[i], curr_.normals_pyr[i]);
141 #else
142  cuda::computePointNormals(p.intr(i), curr_.depth_pyr[i], curr_.points_pyr[i], curr_.normals_pyr[i]);
143 #endif
144 
146 
147  //can't perform more on first frame
148  if (frame_counter_ == 0)
149  {
150  volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(), p.intr);
151 #if defined USE_DEPTH
152  curr_.depth_pyr.swap(prev_.depth_pyr);
153 #else
154  curr_.points_pyr.swap(prev_.points_pyr);
155 #endif
156  curr_.normals_pyr.swap(prev_.normals_pyr);
157  return ++frame_counter_, false;
158  }
159 
161  // ICP
162  Affine3f affine; // cuur -> prev
163  {
164  //ScopeTime time("icp");
165 #if defined USE_DEPTH
166  bool ok = icp_->estimateTransform(affine, p.intr, curr_.depth_pyr, curr_.normals_pyr, prev_.depth_pyr, prev_.normals_pyr);
167 #else
168  bool ok = icp_->estimateTransform(affine, p.intr, curr_.points_pyr, curr_.normals_pyr, prev_.points_pyr, prev_.normals_pyr);
169 #endif
170  if (!ok)
171  return reset(), false;
172  }
173  poses_.push_back(poses_.back() * affine); // curr -> global
174 
175  // check if we need to shift
176  if(checkForShift_)
177  has_shifted_ = cyclical_.checkForShift(volume_, getCameraPose(), params_.distance_camera_target , perform_shift_, perform_last_scan_, record_mode_);
178 
179  perform_shift_ = false;
181  // Volume integration
182  Affine3f local_pose = Affine3f().translate(poses_.back().translation() - volume_->getPose().translation());
183  local_pose.rotate(poses_.back().rotation());
184  // We do not integrate volume if camera does not move.
185  float rnorm = (float)cv::norm(affine.rvec());
186  float tnorm = (float)cv::norm(affine.translation());
187  bool integrate = (rnorm + tnorm)/2 >= p.tsdf_min_camera_movement;
188  if (integrate)
189  {
190  //ScopeTime time("tsdf");
191  volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(), p.intr);
192  }
193 
195  // Ray casting
196  {
197  //ScopeTime time("ray-cast-all");
198 #if defined USE_DEPTH
199  volume_->raycast(poses_.back(), cyclical_.getBuffer(), p.intr, prev_.depth_pyr[0], prev_.normals_pyr[0]);
200  for (int i = 1; i < LEVELS; ++i)
201  resizeDepthNormals(prev_.depth_pyr[i-1], prev_.normals_pyr[i-1], prev_.depth_pyr[i], prev_.normals_pyr[i]);
202 #else
203  //volume_->raycast(local_pose, p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]);
204  volume_->raycast(poses_.back(), cyclical_.getBuffer(), p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]);
205  for (int i = 1; i < LEVELS; ++i)
206  resizePointsNormals(prev_.points_pyr[i-1], prev_.normals_pyr[i-1], prev_.points_pyr[i], prev_.normals_pyr[i]);
207 #endif
209  }
210 
211  return ++frame_counter_, true;
212 }
213 
215 {
216  const KinFuParams& p = params_;
217  image.create(p.rows, flag != 3 ? p.cols : p.cols * 2);
218 
219 #if defined USE_DEPTH
220  #define PASS1 prev_.depth_pyr
221 #else
222  #define PASS1 prev_.points_pyr
223 #endif
224 
225  if (flag < 1 || flag > 3)
226  cuda::renderImage(PASS1[0], prev_.normals_pyr[0], params_.intr, params_.light_pose, image);
227  else if (flag == 2)
228  cuda::renderTangentColors(prev_.normals_pyr[0], image);
229  else /* if (flag == 3) */
230  {
231  DeviceArray2D<RGB> i1(p.rows, p.cols, image.ptr(), image.step());
232  DeviceArray2D<RGB> i2(p.rows, p.cols, image.ptr() + p.cols, image.step());
233 
234  cuda::renderImage(PASS1[0], prev_.normals_pyr[0], params_.intr, params_.light_pose, i1);
235  cuda::renderTangentColors(prev_.normals_pyr[0], i2);
236  }
237 #undef PASS1
238 }
239 
240 
241 void kfusion::KinFu::renderImage(cuda::Image& image, const Affine3f& pose, int flag)
242 {
243  const KinFuParams& p = params_;
244  image.create(p.rows, flag != 3 ? p.cols : p.cols * 2);
245  depths_.create(p.rows, p.cols);
246  normals_.create(p.rows, p.cols);
247  points_.create(p.rows, p.cols);
248 
249 #if defined USE_DEPTH
250  #define PASS1 depths_
251 #else
252  #define PASS1 points_
253 #endif
254 
255  volume_->raycast(pose, cyclical_.getBuffer(), p.intr, PASS1, normals_);
256 
257  if (flag < 1 || flag > 3)
258  cuda::renderImage(PASS1, normals_, params_.intr, params_.light_pose, image);
259  else if (flag == 2)
260  cuda::renderTangentColors(normals_, image);
261  else /* if (flag == 3) */
262  {
263  DeviceArray2D<RGB> i1(p.rows, p.cols, image.ptr(), image.step());
264  DeviceArray2D<RGB> i2(p.rows, p.cols, image.ptr() + p.cols, image.step());
265 
266  cuda::renderImage(PASS1, normals_, params_.intr, params_.light_pose, i1);
267  cuda::renderTangentColors(normals_, i2);
268  }
269 #undef PASS1
270 }
271 
272 
273 void kfusion::KinFu::renderImage(cuda::Image& image, const Affine3f& pose, Intr cameraIntrinsics, cv::Size size, int flag)
274 {
275  int rows = size.height;
276  int cols = size.width;
277  image.create(rows, flag != 3 ? cols : cols * 2);
278  depths_.create(rows, cols);
279  normals_.create(rows, cols);
280  points_.create(rows, cols);
281 
282 #if defined USE_DEPTH
283  #define PASS1 depths_
284 #else
285  #define PASS1 points_
286 #endif
287 
288  volume_->raycast(pose, cyclical_.getBuffer(), cameraIntrinsics, PASS1, normals_);
289 
290  if (flag < 1 || flag > 3)
291  cuda::renderImage(PASS1, normals_, cameraIntrinsics, params_.light_pose, image);
292  else if (flag == 2)
293  cuda::renderTangentColors(normals_, image);
294  else /* if (flag == 3) */
295  {
296  DeviceArray2D<RGB> i1(rows, cols, image.ptr(), image.step());
297  DeviceArray2D<RGB> i2(rows, cols, image.ptr() + cols, image.step());
298 
299  cuda::renderImage(PASS1, normals_, cameraIntrinsics, params_.light_pose, i1);
300  cuda::renderTangentColors(normals_, i2);
301  }
302 #undef PASS1
303 }
304 
305 
kfusion::KinFu::icp_
cv::Ptr< cuda::ProjectiveICP > icp_
Definition: kinfu.hpp:97
kfusion::KinFu::getCameraPose
Affine3f getCameraPose(int time=-1) const
Definition: kinfu.cpp:117
kfusion::cuda::DeviceArray2D::create
void create(int rows, int cols)
Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates...
Definition: device_array.hpp:274
kfusion::KinFu::volume_
cv::Ptr< cuda::TsdfVolume > volume_
Definition: kinfu.hpp:94
kfusion::cuda::depthTruncation
KF_EXPORTS void depthTruncation(Depth &depth, float threshold)
Definition: imgproc.cpp:9
kfusion::cuda::TsdfVolume
Definition: tsdf_volume.hpp:11
internal.hpp
kfusion::cuda::ProjectiveICP::MAX_PYRAMID_LEVELS
@ MAX_PYRAMID_LEVELS
Definition: projective_icp.hpp:12
kfusion::cuda::computeDists
KF_EXPORTS void computeDists(const Depth &depth, Dists &dists, const Intr &intr)
Definition: imgproc.cpp:44
kfusion::cuda::ProjectiveICP
Definition: projective_icp.hpp:9
kfusion::KinFu::params_
KinFuParams params_
Definition: kinfu.hpp:78
kfusion::KinFu::operator()
bool operator()(const cuda::Depth &dpeth, const cuda::Image &image=cuda::Image())
Definition: kinfu.cpp:124
kfusion::cuda::waitAllDefaultStream
KF_EXPORTS void waitAllDefaultStream()
Definition: imgproc.cpp:18
kfusion::cuda::resizePointsNormals
KF_EXPORTS void resizePointsNormals(const Cloud &points, const Normals &normals, Cloud &points_out, Normals &normals_out)
Definition: imgproc.cpp:61
kfusion::cuda::CyclicalBuffer::initBuffer
void initBuffer(cv::Ptr< cuda::TsdfVolume > tsdf_volume)
Initializes memory pointers of the cyclical buffer (start, end, current origin)
Definition: cyclical_buffer.h:209
kfusion::cuda::depthBilateralFilter
KF_EXPORTS void depthBilateralFilter(const Depth &in, Depth &out, int ksz, float sigma_spatial, float sigma_depth)
Definition: imgproc.cpp:3
precomp.hpp
p
SharedPointer p
Definition: ConvertShared.hpp:42
kfusion::Affine3f
cv::Affine3f Affine3f
Definition: types.hpp:18
kfusion::KinFu::allocate_buffers
void allocate_buffers()
Definition: kinfu.cpp:64
kinfu.hpp
kfusion::cuda::CyclicalBuffer
CyclicalBuffer implements a cyclical TSDF buffer. The class offers a simple interface,...
Definition: cyclical_buffer.h:64
kfusion::cuda::depthBuildPyramid
KF_EXPORTS void depthBuildPyramid(const Depth &depth, Depth &pyramid, float sigma_depth)
Definition: imgproc.cpp:12
kfusion::cuda::DeviceArray2D::ptr
T * ptr(int y=0)
Returns pointer to given row in internal buffer.
Definition: device_array.hpp:294
kfusion::cuda
Definition: device_array.hpp:10
kfusion
Utility.
Definition: capture.hpp:8
kfusion::KinFu::renderImage
void renderImage(cuda::Image &image, int flags=0)
Definition: kinfu.cpp:214
PASS1
#define PASS1
kfusion::KinFuParams::raycast_step_factor
float raycast_step_factor
Definition: types.hpp:157
kfusion::KinFu::cyclical
const cuda::CyclicalBuffer & cyclical() const
Definition: kinfu.cpp:52
kfusion::KinFuParams::icp_dist_thres
float icp_dist_thres
Definition: types.hpp:149
kfusion::KinFuParams
Definition: types.hpp:127
kfusion::KinFu::KinFu
KinFu(const KinFuParams &params)
Definition: kinfu.cpp:9
kfusion::KinFuParams::volume_size
Vec3f volume_size
Definition: types.hpp:138
kfusion::KinFuParams::volume_dims
Vec3i volume_dims
Definition: types.hpp:137
kfusion::KinFu::params
const KinFuParams & params() const
Definition: kinfu.cpp:35
kfusion::cuda::computeNormalsAndMaskDepth
KF_EXPORTS void computeNormalsAndMaskDepth(const Intr &intr, Depth &depth, Normals &normals)
Definition: imgproc.cpp:21
kfusion::KinFu::cyclical_
cuda::CyclicalBuffer cyclical_
Cyclical buffer object.
Definition: kinfu.hpp:96
kfusion::KinFu::reset
void reset(Affine3f initialPose=Affine3f::Identity())
Definition: kinfu.cpp:101
kfusion::KinFuParams::volume_pose
Affine3f volume_pose
Definition: types.hpp:139
std
Definition: HalfEdge.hpp:124
kfusion::cuda::renderTangentColors
KF_EXPORTS void renderTangentColors(const Normals &normals, Image &image)
Definition: imgproc.cpp:104
kfusion::KinFu::icp
const cuda::ProjectiveICP & icp() const
Definition: kinfu.cpp:58
kfusion::KinFuParams::tsdf_max_weight
int tsdf_max_weight
Definition: types.hpp:155
kfusion::KinFu::tsdf
const cuda::TsdfVolume & tsdf() const
Definition: kinfu.cpp:46
kfusion::KinFuParams::gradient_delta_factor
float gradient_delta_factor
Definition: types.hpp:158
kfusion::device::norm
__kf_device__ float norm(const float3 &v)
Definition: temp_utils.hpp:87
kfusion::KinFu::performLastScan
void performLastScan()
Definition: kinfu.cpp:41
kfusion::device::integrate
void integrate(const Dists &depth, TsdfVolume &volume, tsdf_buffer &buffer, const Aff3f &aff, const Projector &proj)
kfusion::KinFuParams::tsdf_trunc_dist
float tsdf_trunc_dist
Definition: types.hpp:154
kfusion::KinFuParams::icp_angle_thres
float icp_angle_thres
Definition: types.hpp:150
kfusion::KinFuParams::icp_iter_num
std::vector< int > icp_iter_num
Definition: types.hpp:151
kfusion::cuda::computePointNormals
KF_EXPORTS void computePointNormals(const Intr &intr, const Depth &depth, Cloud &points, Normals &normals)
Definition: imgproc.cpp:31
kfusion::cuda::resizeDepthNormals
KF_EXPORTS void resizeDepthNormals(const Depth &depth, const Normals &normals, Depth &depth_out, Normals &normals_out)
Definition: imgproc.cpp:50
kfusion::cuda::DeviceArray2D< unsigned short >
kfusion::cuda::renderImage
KF_EXPORTS void renderImage(const Depth &depth, const Normals &normals, const Intr &intr, const Vec3f &light_pose, Image &image)
Definition: imgproc.cpp:76
kfusion::Intr
Definition: types.hpp:20
kfusion::cuda::DeviceMemory2D::step
size_t step() const
Returns stride between two consecutive rows in bytes for internal buffer. Step is stored always and e...
Definition: device_memory.cpp:258


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:23