Octomap wiki - icsl-Jeon/image-tracking GitHub Wiki

Octomap Basics

octomap support occupancy value (clamped probability value in [minThres,maxThres]) / LogOdd (some log operation to prob) for known if the occupancy value is bigger than 0.5, then it is labeled as occupied.

https://github.com/ethz-asl/volumetric_mapping

for(OcTree::leaf_bbx_iterator it=octree->begin_leafs_bbx(bb_lower,bb_upper);
        it!=octree->end_leafs_bbx();it++){

            count++;
            if (it->getOccupancy()>0.4 && it->getOccupancy()<0.6) {
                unknown_count++;
                // basically L(n)>0.5 -> occupied
                std::cout<< it->getOccupancy() <<std::endl;
                std::cout<<octree->isNodeOccupied(*it)<<std::endl;

            }
    }

for unknown node, we cannot access iterator. we access then with

    point3d_list unknown_cells;
    // normally max_depth is 16
    octree->getUnknownLeafCenters(unknown_cells,bb_lower,bb_upper,16);

Installation

sudo apt-get install ros-indigo-octomap ros-indigo-octomap-mapping
rosdep install octomap_mapping
rosmake octomap_mapping

in CMakeLists.txt of our package

find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})
link_libraries(${OCTOMAP_LIBRARIES})

in package.xml

<build_depend>octomap</build_depend>
<run_depend>octomap</run_depend>

then let's modify octomap_server octomap_tracking_server.launch

<param name="frame_id" type="string" value="map" />
...
<!--remap from="cloud_in" to="/rgbdslam/batch_clouds" /-->

to

<param name="frame_id" type="string" value="world" />
...
<remap from="cloud_in" to="/firefly/vi_sensor/camera_depth/depth/points" />

then run the command

roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch  mav_name:=firefly
rviz
roslaunch octomap_server octomap_tracking_server.launch

original source code https://github.com/OctoMap/octomap_mapping/tree/kinetic-devel/octomap_server/src

Octomap save

rosrun octomap_server octomap_saver mapfile.bt //to request a compressed binary octomap via service call and save it to mapfile.bt. 

rosrun octomap_server octomap_saver -f mapfile.ot // to request a full probability octomap

Octomap retrieve

from msg

basically, octomap msg is some serialized information about "octree". the msg cannot be used directly. it needs to be deserialized using :

AbstractOcTree* tree = octomap_msgs::msgToMap(map_msg);
 OcTree octree* = dynamic_cast<OcTree*>(tree);
 if (octree){ // can be NULL
 ...
 }

Here we highly recommend to delete the octree pointer at the end of this callback function as octomap::msgToMap create new Tree object. we have to access and delete

then we can make query like

octree.castRay(...)

castRay details(https://docs.ros.org/api/octomap/html/classoctomap_1_1OccupancyOcTreeBase.html#a22956f418e0a63ec3dfb4821f5300a25) or

OcTreeNode* n = octree.search(x,y,z);
 if (n){
 std::cout << "Value: " << n->getValue() << "\n";
 }

Code example

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h> 
#include<stdio.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include<octomap/OcTreeBase.h>
using namespace std;
using namespace octomap;
double v=0;

int main( int argc, char** argv )

{
  //initialize the node

ros::init(argc, argv, "volume_calc");
ros::NodeHandle node;

AbstractOcTree* tree = AbstractOcTree::read("/home/metal/fuerte_workspace/sandbox/volume_eval/sample.ot");
OcTree* octree = dynamic_cast<OcTree*>(tree);

     for(OcTree::leaf_iterator it = octree->begin_leafs(),
       end=octree->end_leafs(); it!= end; ++it)
{
  //manipulate node, e.g.:
  std::cout << "Node center: " << it.getCoordinate() << std::endl;
  std::cout << "Node size: " << it.getSize() << std::endl;
  std::cout << "Node value: " << it->getValue() << std::endl;
  v=v+(pow(it.getSize(),3));
}

std::cout<<"VOLUME::::"<<v<<endl;
exit(0);


  return 0;
}//main

iterating in some given box

 bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){
     point3d min = pointMsgToOctomap(req.min);
     point3d max = pointMsgToOctomap(req.max);
   
     double thresMin = m_octree->getClampingThresMin();
     for(OcTreeT::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min,max),
         end=m_octree->end_leafs_bbx(); it!= end; ++it){
   
       it->setLogOdds(octomap::logodds(thresMin));
       
       m_octree->updateInnerOccupancy();

octomap filter implementation https://github.com/lorenzoriano/octomap_filters.git

refer also https://github.com/introlab/rtabmap_ros/issues/193

Octovis Usage

installation(its not a ros-dependent package)

sudo apt-get install ros-kinetic-octovis

usage

octovis ~/filename.bt

Stand alone test in c++

CMake Settings

set(octomap_DIR /opt/ros/kinetic/share/octomap)
find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})
include_directories(/opt/ros/kinetic/include)

add_executable(CLion main.cpp )
target_link_libraries(CLion ${OCTOMAP_LIBRARIES})
⚠️ **GitHub.com Fallback** ⚠️