Exemplo n.º 1
0
//==============================================================================
void VoxelGridShape::updateOccupancy(
    const Eigen::Vector3d& from, const Eigen::Vector3d& to)
{
  mOctree->insertRay(toPoint3d(from), toPoint3d(to));

  incrementVersion();
}
Exemplo n.º 2
0
//==============================================================================
void VoxelGridShape::updateOccupancy(
    const Eigen::Vector3d& point, bool occupied)
{
  mOctree->updateNode(toPoint3d(point), occupied);

  incrementVersion();
}
Exemplo n.º 3
0
//==============================================================================
void VoxelGridShape::updateOccupancy(
    const octomap::Pointcloud& pointCloud,
    const Eigen::Vector3d& sensorOrigin,
    const Eigen::Isometry3d& relativeTo)
{
  mOctree->insertPointCloud(
      pointCloud, toPoint3d(sensorOrigin), toPose6d(relativeTo));

  incrementVersion();
}
Exemplo n.º 4
0
//==============================================================================
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());
  }
}
Exemplo n.º 5
0
    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;
            }
        }
    }