void WorldDownloadManager::findExtraCubesForBoundingBox(const Eigen::Vector3f& current_cube_min,
const Eigen::Vector3f& current_cube_max,const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,Vector3fVector& cubes_centers,
bool& extract_current)
{
const Eigen::Vector3f & cube_size = current_cube_max - current_cube_min;
cubes_centers.clear();
extract_current = false;
const Eigen::Vector3f relative_act_bbox_min = bbox_min - current_cube_min;
const Eigen::Vector3f relative_act_bbox_max = bbox_max - current_cube_min;
const Eigen::Vector3i num_cubes_plus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_max.array() / cube_size.array())
- (Eigen::Vector3f::Ones() * 0.0001))).cast<int>();
const Eigen::Vector3i num_cubes_minus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_min.array() / cube_size.array())
+ (Eigen::Vector3f::Ones() * 0.0001))).cast<int>();
for (int z = num_cubes_minus.z(); z <= num_cubes_plus.z(); z++)
for (int y = num_cubes_minus.y(); y <= num_cubes_plus.y(); y++)
for (int x = num_cubes_minus.x(); x <= num_cubes_plus.x(); x++)
{
const Eigen::Vector3i cube_index(x,y,z);
if ((cube_index.array() == Eigen::Vector3i::Zero().array()).all())
{
extract_current = true;
continue;
}
const Eigen::Vector3f relative_cube_origin = cube_index.cast<float>().array() * cube_size.array();
const Eigen::Vector3f cube_center = relative_cube_origin + current_cube_min + (cube_size * 0.5);
cubes_centers.push_back(cube_center);
}
}
void transformMeans(
Vector3fVector& means,
const AffineTransform& transform)
{
Matrix3f R = transform.rotation();
Vector3f t = transform.translation();
unsigned int size = means.size();
for(unsigned int i = 0; i < size; ++i)
{
Vector3f& m = means[i];
m = R * m + t;
}
}
void removeInvalidMeans(
const Vector3fVector& means,
const BoolVector& valid,
Vector3fVector& means_f)
{
unsigned int size = valid.size();
for(unsigned int i = 0; i < size; ++i)
{
if (valid[i])
{
const Vector3f& mean = means[i];
means_f.push_back(mean);
}
}
}
void transformDistributions(
Vector3fVector& means,
Matrix3fVector& covariances,
const AffineTransform& transform)
{
Matrix3f R = transform.rotation();
Vector3f t = transform.translation();
Matrix3f R_T = R.transpose();
unsigned int size = means.size();
for(unsigned int i = 0; i < size; ++i)
{
Vector3f& m = means[i];
Matrix3f& c = covariances[i];
m = R * m + t;
c = R * c * R_T;
}
}
void removeInvalidDistributions(
const Vector3fVector& means,
const Matrix3fVector& covariances,
const BoolVector& valid,
Vector3fVector& means_f,
Matrix3fVector& covariances_f)
{
unsigned int size = valid.size();
for(unsigned int i = 0; i < size; ++i)
{
if (valid[i])
{
const Vector3f& mean = means[i];
const Matrix3f& cov = covariances[i];
means_f.push_back(mean);
covariances_f.push_back(cov);
}
}
}
void pointCloudFromMeans(
const Vector3fVector& means,
PointCloudFeature& cloud)
{
unsigned int size = means.size();
cloud.points.resize(size);
for(unsigned int i = 0; i < size; ++i)
{
const Vector3f& m = means[i];
PointFeature& p = cloud.points[i];
p.x = m(0,0);
p.y = m(1,0);
p.z = m(2,0);
}
cloud.height = 1;
cloud.width = size;
cloud.is_dense = true;
}