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 WorldDownloadManager::transformBoundingBoxAndExpand(const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,
const Eigen::Affine3f& transform,Eigen::Vector3f& bbox_min_out,Eigen::Vector3f& bbox_max_out)
{
const int SIZE = 2;
Eigen::Vector3f inv[SIZE];
inv[0] = bbox_min;
inv[1] = bbox_max;
Eigen::Vector3f comb;
for (uint ix = 0; ix < SIZE; ix++)
{
comb.x() = inv[ix].x();
for (uint iy = 0; iy < SIZE; iy++)
{
comb.y() = inv[iy].y();
for (uint iz = 0; iz < SIZE; iz++)
{
comb.z() = inv[iz].z();
const Eigen::Vector3f t_comb = transform * comb;
if (!ix && !iy && !iz) // first iteration
bbox_min_out = bbox_max_out = t_comb;
else
{
bbox_min_out = bbox_min_out.array().min(t_comb.array());
bbox_max_out = bbox_max_out.array().max(t_comb.array());
}
}
}
}
}
TIncompletePointsListener::PointXYZNormalCloud::Ptr TIncompletePointsListener::ClearBBoxCloud(
const PointXYZNormalCloud::ConstPtr cloud, const Eigen::Vector3f & bbox_min, const Eigen::Vector3f & bbox_max)
{
PointXYZNormalCloud::Ptr result(new PointXYZNormalCloud);
const size_t size = cloud->size();
result->reserve(size);
for (uint i = 0; i < size; i++)
{
const pcl::PointNormal & pt = (*cloud)[i];
const Eigen::Vector3f ept(pt.x,pt.y,pt.z);
if ((ept.array() < bbox_min.array()).any() || (ept.array() > bbox_max.array()).any())
result->push_back(pt);
}
return result;
}
Transform<float, 3, Affine, AutoAlign> Plane::get2DArbitraryRefSystem() const
{
Eigen::Vector3f n = normal_.getUnitNormal();
/// seek for a good unit axis to project on plane
/// and then use it as X direction of the 2d local system
size_t min_id;
n.array().abs().minCoeff(&min_id);
DLOG(INFO) << "min id is " << min_id;
Vector3f proj_axis = Vector3f::Zero();
proj_axis(min_id) = 1; // unity on that axis
// project the selected axis on the plane
Vector3f second_ax = projectVectorOnPlane(proj_axis);
second_ax.normalize();
Vector3f first_ax = n.cross(second_ax);
first_ax.normalize();
Transform<float, 3, Affine, AutoAlign> T;
// T.matrix().fill(0);
T.matrix().col(0).head(3) = first_ax;
T.matrix().col(1).head(3) = second_ax;
T.matrix().col(2).head(3) = n;
// T.matrix()(3, 3) = 1;
DLOG(INFO) << "Transform computed \n " << T.inverse().matrix() << "normal was " << n;
DLOG(INFO) << "In fact T*n " <<T.inverse() *n;
return T.inverse();
}
png::image<png::rgb_pixel_16> bumpier::model::calculate_normals(const png::image<png::gray_pixel_16>& input, double phi, space_type space) const
{
int width = input.get_width(), height = input.get_height();
png::image<png::rgb_pixel_16> output(width, height);
for (int y = 0; y < height; y++)
for (int x = 0; x < width; x++)
{
int xmin = (x + width - 1) % width,
ymin = (y + height - 1) % height,
xmax = (x + 1) % width,
ymax = (y + 1) % height;
Eigen::Vector3f normal = (position(input, x, ymax) - position(input, x, ymin)).cross(
position(input, xmax, y) - position(input, xmin, y));
if(space == tangent_space)
normal = tangentspace(Eigen::Vector2f((float)x / width, (float)y / height)) * normal;
normal.normalize();
normal = (normal.array() * 0.5 + 0.5) * 0xFFFF;
output.set_pixel(x, y, png::rgb_pixel_16(normal[0], normal[1], normal[2]));
}
return output;
}
void WorldDownloadManager::initRaycaster(bool has_intrinsics,const kinfu_msgs::KinfuCameraIntrinsics & intr,
bool has_bounding_box_view,const kinfu_msgs::KinfuCloudPoint & bbox_min,const kinfu_msgs::KinfuCloudPoint & bbox_max)
{
const uint rows = has_intrinsics ? intr.size_y : m_kinfu->rows();
const uint cols = has_intrinsics ? intr.size_x : m_kinfu->cols();
float cx,cy,fx,fy;
float min_range = 0.0;
if (has_intrinsics)
{
ROS_INFO("kinfu: custom intrinsics will be used.");
cx = intr.center_x;
cy = intr.center_y;
fx = intr.focal_x;
fy = intr.focal_y;
min_range = intr.min_range;
}
else
m_kinfu->getDepthIntrinsics(fx,fy,cx,cy);
if (!m_raycaster || (uint(m_raycaster->rows) != rows) || (uint(m_raycaster->cols) != cols))
{
ROS_INFO("kinfu: initializing raycaster...");
m_raycaster = RayCaster::Ptr(new RayCaster(rows,cols));
}
m_raycaster->setRaycastStep(m_kinfu->volume().getTsdfTruncDist() * 0.6);
m_raycaster->setMinRange(min_range);
m_raycaster->setIntrinsics(fx,fy,cx,cy);
if (has_bounding_box_view)
{
const Eigen::Vector3f im(bbox_min.x,bbox_min.y,bbox_min.z);
const Eigen::Vector3f iM(bbox_max.x,bbox_max.y,bbox_max.z);
ROS_INFO("kinfu: raycaster will be limited to bounding box: %f %f %f - %f %f %f",im.x(),im.y(),im.z(),iM.x(),iM.y(),iM.z());
const Eigen::Vector3f m = m_reverse_initial_transformation.inverse() * im;
const Eigen::Vector3f M = m_reverse_initial_transformation.inverse() * iM;
const Eigen::Vector3f mmin = m.array().min(M.array());
const Eigen::Vector3f mmax = m.array().max(M.array());
m_raycaster->setBoundingBox(mmin,mmax);
}
else
m_raycaster->clearBoundingBox();
}
/** \brief Constructor
* param[in] volume_size size of the volume in mm
* param[in] volume_res volume grid resolution (typically device::VOLUME_X x device::VOLUME_Y x device::VOLUME_Z)
*/
DeviceVolume (const Eigen::Vector3f &volume_size, const Eigen::Vector3i &volume_res)
: volume_size_ (volume_size)
{
// initialize GPU
device_volume_.create (volume_res[1] * volume_res[2], volume_res[0]); // (device::VOLUME_Y * device::VOLUME_Z, device::VOLUME_X)
pcl::device::initVolume (device_volume_);
// truncation distance
Eigen::Vector3f voxel_size = volume_size.array() / volume_res.array().cast<float>();
trunc_dist_ = max ((float)min_trunc_dist, 2.1f * max (voxel_size[0], max (voxel_size[1], voxel_size[2])));
};
void onGMM(const gaussian_mixture_model::GaussianMixture & mix)
{
visualization_msgs::MarkerArray msg;
ROS_INFO("gmm_rviz_converter: Received message.");
uint i;
for (i = 0; i < mix.gaussians.size(); i++)
{
visualization_msgs::Marker marker;
marker.header.frame_id = m_frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = m_rviz_namespace;
marker.id = i;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration();
Eigen::Vector3f coords;
for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
coords[ir] = m_conversion_mask[ir]->GetMean(m_conversion_mask,mix.gaussians[i]);
marker.pose.position.x = coords.x();
marker.pose.position.y = coords.y();
marker.pose.position.z = coords.z();
Eigen::Matrix3f covmat;
for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
for (uint ic = 0; ic < NUM_OUTPUT_COORDINATES; ic++)
covmat(ir,ic) = m_conversion_mask[ir]->GetCov(m_conversion_mask,mix.gaussians[i],ic);
Eigen::EigenSolver<Eigen::Matrix3f> evsolver(covmat);
Eigen::Matrix3f eigenvectors = evsolver.eigenvectors().real();
if (eigenvectors.determinant() < 0.0)
eigenvectors.col(0) = - eigenvectors.col(0);
Eigen::Matrix3f rotation = eigenvectors;
Eigen::Quaternionf quat = Eigen::Quaternionf(Eigen::AngleAxisf(rotation));
marker.pose.orientation.x = quat.x();
marker.pose.orientation.y = quat.y();
marker.pose.orientation.z = quat.z();
marker.pose.orientation.w = quat.w();
Eigen::Vector3f eigenvalues = evsolver.eigenvalues().real();
Eigen::Vector3f scale = Eigen::Vector3f(eigenvalues.array().abs().sqrt());
if (m_normalize)
scale.normalize();
marker.scale.x = mix.weights[i] * scale.x() * m_scale;
marker.scale.y = mix.weights[i] * scale.y() * m_scale;
marker.scale.z = mix.weights[i] * scale.z() * m_scale;
marker.color.a = 1.0;
rainbow(float(i) / float(mix.gaussians.size()),marker.color.r,marker.color.g,marker.color.b);
msg.markers.push_back(marker);
}
// this a waste of resources, but we need to delete old markers in some way
// someone should add a "clear all" command to rviz
// (using expiration time is not an option, here)
for ( ; i < m_max_markers; i++)
{
visualization_msgs::Marker marker;
marker.id = i;
marker.action = visualization_msgs::Marker::DELETE;
marker.lifetime = ros::Duration();
marker.header.frame_id = m_frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = m_rviz_namespace;
msg.markers.push_back(marker);
}
m_pub.publish(msg);
ROS_INFO("gmm_rviz_converter: Sent message.");
}
void SemanticLabelsVisualization::multiplyColors(Eigen::Vector3i& rgb, Eigen::Vector3f label)
{
rgb = (rgb.cast<float>().array() * label.array()).cast<int>().matrix();
}
