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(); }