47 cv::Vec3f targetPoint(0,0, distance_camera_target);
48 targetPoint = cam_pose * targetPoint;
49 targetPoint[1] = cam_pose.translation()[1];
54 double dist =
norm(targetPoint, center_cube, cv::NORM_L2);
60 if (result || last_shift || perform_shift)
62 performShift (volume, targetPoint, cam_pose, last_shift, record_mode);
79 computeAndSetNewCubeMetricOrigin (volume, target_point, offset);
89 minBounds[0] = minBounds[1] = minBounds[2] = 0;
90 maxBounds[0] = maxBounds[1] = maxBounds[2] = 512;
94 calcBounds(offset, minBounds, maxBounds);
97 cloud = volume->fetchSliceAsCloud(cloud_buffer_device_, &buffer_, minBounds, maxBounds, global_shift_ );
99 cloud_slice_ = cv::Mat(1, (
int)cloud.
size(), CV_32FC4);
107 std::cout <<
"#### Performing slice number: " << slice_count_ <<
" with " << cloud.
size() <<
" TSDF values ####" << std::endl;
108 Vec3i fusionShift = global_shift_;
109 Vec3i fusionBackShift = global_shift_;
110 for(
int i = 0; i < 3; i++)
112 if(minBounds[i] == 1 && !last_shift)
114 fusionShift[i] += maxBounds[i];
115 fusionBackShift[i] += minBounds[i];
119 fusionShift[i] -= 1000000000;
120 fusionBackShift[i] += maxBounds[i];
124 fusionShift[i] += minBounds[i];
125 fusionBackShift[i] += maxBounds[i];
133 pl_.addTSDFSlice(slice, last_shift);
140 volume->clearSlice(&buffer_, offset);
143 shiftOrigin (volume, offset);
153 float3 new_cube_origin_meters;
154 new_cube_origin_meters.x = target_point[0] - buffer_.volume_size.x/2.0f;
155 new_cube_origin_meters.y = target_point[1] - buffer_.volume_size.y/2.0f;
156 new_cube_origin_meters.z = target_point[2] - buffer_.volume_size.z/2.0f;
161 offset[0] = calcIndex((new_cube_origin_meters.x - buffer_.origin_metric.x) * ( buffer_.voxels_size.x / (
float) (buffer_.volume_size.x) ));
162 offset[1] = calcIndex((new_cube_origin_meters.y - buffer_.origin_metric.y) * ( buffer_.voxels_size.y / (
float) (buffer_.volume_size.y) ));
163 offset[2] = calcIndex((new_cube_origin_meters.z - buffer_.origin_metric.z) * ( buffer_.voxels_size.z / (
float) (buffer_.volume_size.z) ));
167 buffer_.origin_metric = new_cube_origin_meters;
168 volume->setPose(
Affine3f().translate(
Vec3f(new_cube_origin_meters.x, new_cube_origin_meters.y, new_cube_origin_meters.z)));
175 int newX = buffer_.origin_GRID.x + offset[0];
176 int newY = buffer_.origin_GRID.y + offset[1];
177 int newZ = buffer_.origin_GRID.z + offset[2];
182 minBounds[0] = buffer_.origin_GRID.x;
187 minBounds[0] = newX + buffer_.voxels_size.x;
188 maxBounds[0] = buffer_.origin_GRID.x + buffer_.voxels_size.x;
191 if (minBounds[0] > maxBounds[0])
197 minBounds[1] = buffer_.origin_GRID.y;
202 minBounds[1] = newY + buffer_.voxels_size.y;
203 maxBounds[1] = buffer_.origin_GRID.y + buffer_.voxels_size.y;
206 if(minBounds[1] > maxBounds[1])
212 minBounds[2] = buffer_.origin_GRID.z;
217 minBounds[2] = newZ + buffer_.voxels_size.z;
218 maxBounds[2] = buffer_.origin_GRID.z + buffer_.voxels_size.z;
221 if (minBounds[2] > maxBounds[2])
224 minBounds[0] -= buffer_.origin_GRID.x;
225 maxBounds[0] -= buffer_.origin_GRID.x;
227 minBounds[1] -= buffer_.origin_GRID.y;
228 maxBounds[1] -= buffer_.origin_GRID.y;
230 minBounds[2] -= buffer_.origin_GRID.z;
231 maxBounds[2] -= buffer_.origin_GRID.z;
233 if (minBounds[0] < 0)
235 minBounds[0] += buffer_.voxels_size.x;
236 maxBounds[0] += buffer_.voxels_size.x;
240 if (minBounds[1] < 0)
242 minBounds[1] += buffer_.voxels_size.y;
243 maxBounds[1] += buffer_.voxels_size.y;
246 if (minBounds[2] < 0)
248 minBounds[2] += buffer_.voxels_size.z;
249 maxBounds[2] += buffer_.voxels_size.z;
251 for(
int i = 0; i < 3; i++)
255 if(minBounds[i] == 0)
261 if(maxBounds[i] == 512)