//==============================================================================
void VoxelGridShape::updateOccupancy(
const Eigen::Vector3d& from, const Eigen::Vector3d& to)
{
mOctree->insertRay(toPoint3d(from), toPoint3d(to));
incrementVersion();
}
//==============================================================================
void VoxelGridShape::updateOccupancy(
const Eigen::Vector3d& point, bool occupied)
{
mOctree->updateNode(toPoint3d(point), occupied);
incrementVersion();
}
//==============================================================================
void VoxelGridShape::updateOccupancy(
const octomap::Pointcloud& pointCloud,
const Eigen::Vector3d& sensorOrigin,
const Eigen::Isometry3d& relativeTo)
{
mOctree->insertPointCloud(
pointCloud, toPoint3d(sensorOrigin), toPose6d(relativeTo));
incrementVersion();
}
//==============================================================================
void VoxelGridShape::updateOccupancy(
const octomap::Pointcloud& pointCloud,
const Eigen::Vector3d& sensorOrigin,
const Frame* relativeTo)
{
if (relativeTo == Frame::World())
{
mOctree->insertPointCloud(pointCloud, toPoint3d(sensorOrigin));
incrementVersion();
}
else
{
updateOccupancy(pointCloud, sensorOrigin, relativeTo->getWorldTransform());
}
}
void toMsgPointCloud(const sensor_msgs::ImagePtr& image,
const CameraIntrinsics& camera_intrinsics,
sensor_msgs::PointCloud2Ptr points)
{
points->header.frame_id = image->header.frame_id;
points->header.stamp = image->header.stamp;
points->height = image->height;
points->width = image->width;
points->is_dense = false;
points->is_bigendian = false;
points->fields.resize(3);
points->fields[0].name = "x";
points->fields[1].name = "y";
points->fields[2].name = "z";
int offset = 0;
for (size_t d = 0; d < points->fields.size(); ++d, offset += sizeof(float))
{
points->fields[d].offset = offset;
points->fields[d].datatype = sensor_msgs::PointField::FLOAT32;
points->fields[d].count = 1;
}
points->point_step = offset;
points->row_step = points->point_step * points->width;
points->data.resize(points->width * points->height * points->point_step);
const float* depth_data = reinterpret_cast<const float*>(&image->data[0]);
float* point_data = reinterpret_cast<float*>(&points->data[0]);
Point3d p;
for(int v = 0, k = 0; v < image->height; ++v)
{
for(int u = 0; u < image->width; ++u, ++k, ++point_data)
{
p = toPoint3d(u, v, depth_data[k], camera_intrinsics);
*point_data++ = p.x;
*point_data++ = p.y;
*point_data = p.z;
}
}
}