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();
}
MSP3D::MSP3D(octomap::OcTree &tree, int max_depth):m_tree(tree),m_path_found(false),m_alpha(1.0),m_eps(tree.getResolution()/10.0),m_max_tree_depth(max_depth),m_lambda1(0.999),m_lambda2(0.001) {
m_M=100*pow(8,max_depth);
m_epsilon=pow(0.5,1+3*m_max_tree_depth);
}