Ejemplo n.º 1
0
void KeyframeMapper::buildOctomap(octomap::OcTree& tree)
{
  ROS_INFO("Building Octomap...");
  
  octomap::point3d sensor_origin(0.0, 0.0, 0.0);  

  for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx)
  {
    ROS_INFO("Processing keyframe %u", kf_idx);
    const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx];
    
    PointCloudT cloud;
    keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_);
           
    octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose));

    // build octomap cloud from pcl cloud
    octomap::Pointcloud octomap_cloud;
    for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx)
    {
      const PointT& p = cloud.points[pt_idx];
      if (!std::isnan(p.z))
        octomap_cloud.push_back(p.x, p.y, p.z);
    }
    
    tree.insertScan(octomap_cloud, sensor_origin, frame_origin);
  }
}
void keypoint_map::get_octree(octomap::OcTree & tree) {

	tree.clear();

	for (size_t i = 0; i < depth_imgs.size(); i++) {

		octomap::Pointcloud octomap_cloud;
		octomap::point3d sensor_origin(0.0, 0.0, 0.0);

		Eigen::Vector3f trans(camera_positions[i].translation());
		Eigen::Quaternionf rot(camera_positions[i].rotation());

		octomap::pose6d frame_origin(
				octomath::Vector3(trans.x(), trans.y(), trans.z()),
				octomath::Quaternion(rot.w(), rot.x(), rot.y(), rot.z()));

		for (int v = 0; v < depth_imgs[i].rows; v++) {
			for (int u = 0; u < depth_imgs[i].cols; u++) {
				if (depth_imgs[i].at<unsigned short>(v, u) != 0) {
					pcl::PointXYZ p;
					p.z = depth_imgs[i].at<unsigned short>(v, u) / 1000.0f;
					p.x = (u - intrinsics[2]) * p.z / intrinsics[0];
					p.y = (v - intrinsics[3]) * p.z / intrinsics[1];

					octomap_cloud.push_back(p.x, p.y, p.z);

				}
			}
		}

		tree.insertScan(octomap_cloud, sensor_origin, frame_origin);
	}

	tree.updateInnerOccupancy();

}