10 , cyclical_(params), checkForShift_(true)
43 perform_last_scan_ =
true;
68 int cols = params_.cols;
69 int rows = params_.rows;
71 dists_.create(rows, cols);
73 curr_.depth_pyr.resize(LEVELS);
74 curr_.normals_pyr.resize(LEVELS);
75 prev_.depth_pyr.resize(LEVELS);
76 prev_.normals_pyr.resize(LEVELS);
78 curr_.points_pyr.resize(LEVELS);
79 prev_.points_pyr.resize(LEVELS);
81 for(
int i = 0; i < LEVELS; ++i)
83 curr_.depth_pyr[i].create(rows, cols);
84 curr_.normals_pyr[i].create(rows, cols);
86 prev_.depth_pyr[i].create(rows, cols);
87 prev_.normals_pyr[i].create(rows, cols);
89 curr_.points_pyr[i].create(rows, cols);
90 prev_.points_pyr[i].create(rows, cols);
96 depths_.create(params_.rows, params_.cols);
97 normals_.create(params_.rows, params_.cols);
98 points_.create(params_.rows, params_.cols);
104 cout <<
"Reset" << endl;
109 poses_.push_back(initialPose);
111 volume_->setPose(params_.volume_pose);
112 cyclical_.resetBuffer (volume_);
113 cyclical_.resetMesh();
119 if (time > (
int)poses_.size () || time < 0)
120 time = (int)poses_.size () - 1;
127 const int LEVELS = icp_->getUsedLevelsNum();
132 if (
p.icp_truncate_depth_dist > 0)
135 for (
int i = 1; i < LEVELS; ++i)
138 for (
int i = 0; i < LEVELS; ++i)
139 #
if defined USE_DEPTH
148 if (frame_counter_ == 0)
150 volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(),
p.intr);
151 #if defined USE_DEPTH
152 curr_.depth_pyr.swap(prev_.depth_pyr);
154 curr_.points_pyr.swap(prev_.points_pyr);
156 curr_.normals_pyr.swap(prev_.normals_pyr);
157 return ++frame_counter_,
false;
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);
168 bool ok = icp_->estimateTransform(affine,
p.intr, curr_.points_pyr, curr_.normals_pyr, prev_.points_pyr, prev_.normals_pyr);
171 return reset(),
false;
173 poses_.push_back(poses_.back() * affine);
177 has_shifted_ = cyclical_.checkForShift(volume_, getCameraPose(), params_.distance_camera_target , perform_shift_, perform_last_scan_, record_mode_);
179 perform_shift_ =
false;
182 Affine3f local_pose =
Affine3f().translate(poses_.back().translation() - volume_->getPose().translation());
183 local_pose.rotate(poses_.back().rotation());
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;
191 volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(),
p.intr);
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]);
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]);
211 return ++frame_counter_,
true;
217 image.
create(
p.rows, flag != 3 ?
p.cols :
p.cols * 2);
219 #if defined USE_DEPTH
220 #define PASS1 prev_.depth_pyr
222 #define PASS1 prev_.points_pyr
225 if (flag < 1 || flag > 3)
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);
249 #if defined USE_DEPTH
250 #define PASS1 depths_
252 #define PASS1 points_
255 volume_->raycast(pose, cyclical_.getBuffer(),
p.intr,
PASS1, normals_);
257 if (flag < 1 || flag > 3)
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);
282 #if defined USE_DEPTH
283 #define PASS1 depths_
285 #define PASS1 points_
288 volume_->raycast(pose, cyclical_.getBuffer(), cameraIntrinsics,
PASS1, normals_);
290 if (flag < 1 || flag > 3)