Joey
/
24navigation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
24navigation/install/octomap-distribution/include/octomap/OccupancyOcTreeBase.hxx

1135 lines
40 KiB
C++
Raw Blame History

/*
* OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
* https://octomap.github.io/
*
* Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
* All rights reserved.
* License: New BSD
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the University of Freiburg nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <bitset>
#include <algorithm>
#include <octomap/MCTables.h>
namespace octomap {
template <class NODE>
OccupancyOcTreeBase<NODE>::OccupancyOcTreeBase(double in_resolution)
: OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>(in_resolution), use_bbx_limit(false), use_change_detection(false)
{
}
template <class NODE>
OccupancyOcTreeBase<NODE>::OccupancyOcTreeBase(double in_resolution, unsigned int in_tree_depth, unsigned int in_tree_max_val)
: OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>(in_resolution, in_tree_depth, in_tree_max_val), use_bbx_limit(false), use_change_detection(false)
{
}
template <class NODE>
OccupancyOcTreeBase<NODE>::~OccupancyOcTreeBase(){
}
template <class NODE>
OccupancyOcTreeBase<NODE>::OccupancyOcTreeBase(const OccupancyOcTreeBase<NODE>& rhs) :
OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>(rhs), use_bbx_limit(rhs.use_bbx_limit),
bbx_min(rhs.bbx_min), bbx_max(rhs.bbx_max),
bbx_min_key(rhs.bbx_min_key), bbx_max_key(rhs.bbx_max_key),
use_change_detection(rhs.use_change_detection), changed_keys(rhs.changed_keys)
{
this->clamping_thres_min = rhs.clamping_thres_min;
this->clamping_thres_max = rhs.clamping_thres_max;
this->prob_hit_log = rhs.prob_hit_log;
this->prob_miss_log = rhs.prob_miss_log;
this->occ_prob_thres_log = rhs.occ_prob_thres_log;
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::insertPointCloud(const ScanNode& scan, double maxrange, bool lazy_eval, bool discretize) {
// performs transformation to data and sensor origin first
Pointcloud& cloud = *(scan.scan);
pose6d frame_origin = scan.pose;
point3d sensor_origin = frame_origin.inv().transform(scan.pose.trans());
insertPointCloud(cloud, sensor_origin, frame_origin, maxrange, lazy_eval, discretize);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin,
double maxrange, bool lazy_eval, bool discretize) {
KeySet free_cells, occupied_cells;
if (discretize)
computeDiscreteUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
else
computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
// insert data into tree -----------------------
for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
updateNode(*it, false, lazy_eval);
}
for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
updateNode(*it, true, lazy_eval);
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::insertPointCloud(const Pointcloud& pc, const point3d& sensor_origin, const pose6d& frame_origin,
double maxrange, bool lazy_eval, bool discretize) {
// performs transformation to data and sensor origin first
Pointcloud transformed_scan (pc);
transformed_scan.transform(frame_origin);
point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval, discretize);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool lazy_eval) {
if (pc.size() < 1)
return;
#ifdef _OPENMP
omp_set_num_threads(this->keyrays.size());
#pragma omp parallel for
#endif
for (int i = 0; i < (int)pc.size(); ++i) {
const point3d& p = pc[i];
unsigned threadIdx = 0;
#ifdef _OPENMP
threadIdx = omp_get_thread_num();
#endif
KeyRay* keyray = &(this->keyrays.at(threadIdx));
if (this->computeRayKeys(origin, p, *keyray)){
#ifdef _OPENMP
#pragma omp critical
#endif
{
for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) {
updateNode(*it, false, lazy_eval); // insert freespace measurement
}
updateNode(p, true, lazy_eval); // update endpoint to be occupied
}
}
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin,
KeySet& free_cells, KeySet& occupied_cells,
double maxrange)
{
Pointcloud discretePC;
discretePC.reserve(scan.size());
KeySet endpoints;
for (int i = 0; i < (int)scan.size(); ++i) {
OcTreeKey k = this->coordToKey(scan[i]);
std::pair<KeySet::iterator,bool> ret = endpoints.insert(k);
if (ret.second){ // insertion took place => k was not in set
discretePC.push_back(this->keyToCoord(k));
}
}
computeUpdate(discretePC, origin, free_cells, occupied_cells, maxrange);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
KeySet& free_cells, KeySet& occupied_cells,
double maxrange)
{
#ifdef _OPENMP
omp_set_num_threads(this->keyrays.size());
#pragma omp parallel for schedule(guided)
#endif
for (int i = 0; i < (int)scan.size(); ++i) {
const point3d& p = scan[i];
unsigned threadIdx = 0;
#ifdef _OPENMP
threadIdx = omp_get_thread_num();
#endif
KeyRay* keyray = &(this->keyrays.at(threadIdx));
if (!use_bbx_limit) { // no BBX specified
if ((maxrange < 0.0) || ((p - origin).norm() <= maxrange) ) { // is not maxrange meas.
// free cells
if (this->computeRayKeys(origin, p, *keyray)){
#ifdef _OPENMP
#pragma omp critical (free_insert)
#endif
{
free_cells.insert(keyray->begin(), keyray->end());
}
}
// occupied endpoint
OcTreeKey key;
if (this->coordToKeyChecked(p, key)){
#ifdef _OPENMP
#pragma omp critical (occupied_insert)
#endif
{
occupied_cells.insert(key);
}
}
} else { // user set a maxrange and length is above
point3d direction = (p - origin).normalized ();
point3d new_end = origin + direction * (float) maxrange;
if (this->computeRayKeys(origin, new_end, *keyray)){
#ifdef _OPENMP
#pragma omp critical (free_insert)
#endif
{
free_cells.insert(keyray->begin(), keyray->end());
}
}
} // end if maxrange
} else { // BBX was set
// endpoint in bbx and not maxrange?
if ( inBBX(p) && ((maxrange < 0.0) || ((p - origin).norm () <= maxrange) ) ) {
// occupied endpoint
OcTreeKey key;
if (this->coordToKeyChecked(p, key)){
#ifdef _OPENMP
#pragma omp critical (occupied_insert)
#endif
{
occupied_cells.insert(key);
}
}
} // end if in BBX and not maxrange
// truncate the end point to the max range if the max range is exceeded
point3d new_end = p;
if ((maxrange >= 0.0) && ((p - origin).norm() > maxrange)) {
const point3d direction = (p - origin).normalized();
new_end = origin + direction * (float) maxrange;
}
// update freespace, break as soon as bbx limit is reached
if (this->computeRayKeys(origin, new_end, *keyray)){
for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) {
if (inBBX(*it)) {
#ifdef _OPENMP
#pragma omp critical (free_insert)
#endif
{
free_cells.insert(*it);
}
}
else break;
}
} // end if compute ray
} // end bbx case
} // end for all points, end of parallel OMP loop
// prefer occupied cells over free ones (and make sets disjunct)
for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ){
if (occupied_cells.find(*it) != occupied_cells.end()){
it = free_cells.erase(it);
} else {
++it;
}
}
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval) {
// clamp log odds within range:
log_odds_value = std::min(std::max(log_odds_value, this->clamping_thres_min), this->clamping_thres_max);
bool createdRoot = false;
if (this->root == NULL){
this->root = new NODE();
this->tree_size++;
createdRoot = true;
}
return setNodeValueRecurs(this->root, createdRoot, key, 0, log_odds_value, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval) {
OcTreeKey key;
if (!this->coordToKeyChecked(value, key))
return NULL;
return setNodeValue(key, log_odds_value, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval) {
OcTreeKey key;
if (!this->coordToKeyChecked(x, y, z, key))
return NULL;
return setNodeValue(key, log_odds_value, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval) {
// early abort (no change will happen).
// may cause an overhead in some configuration, but more often helps
NODE* leaf = this->search(key);
// no change: node already at threshold
if (leaf
&& ((log_odds_update >= 0 && leaf->getLogOdds() >= this->clamping_thres_max)
|| ( log_odds_update <= 0 && leaf->getLogOdds() <= this->clamping_thres_min)))
{
return leaf;
}
bool createdRoot = false;
if (this->root == NULL){
this->root = new NODE();
this->tree_size++;
createdRoot = true;
}
return updateNodeRecurs(this->root, createdRoot, key, 0, log_odds_update, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, float log_odds_update, bool lazy_eval) {
OcTreeKey key;
if (!this->coordToKeyChecked(value, key))
return NULL;
return updateNode(key, log_odds_update, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval) {
OcTreeKey key;
if (!this->coordToKeyChecked(x, y, z, key))
return NULL;
return updateNode(key, log_odds_update, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval) {
float logOdds = this->prob_miss_log;
if (occupied)
logOdds = this->prob_hit_log;
return updateNode(key, logOdds, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, bool occupied, bool lazy_eval) {
OcTreeKey key;
if (!this->coordToKeyChecked(value, key))
return NULL;
return updateNode(key, occupied, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(double x, double y, double z, bool occupied, bool lazy_eval) {
OcTreeKey key;
if (!this->coordToKeyChecked(x, y, z, key))
return NULL;
return updateNode(key, occupied, lazy_eval);
}
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
unsigned int depth, const float& log_odds_update, bool lazy_eval) {
bool created_node = false;
assert(node);
// follow down to last level
if (depth < this->tree_depth) {
unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
if (!this->nodeChildExists(node, pos)) {
// child does not exist, but maybe it's a pruned node?
if (!this->nodeHasChildren(node) && !node_just_created ) {
// current node does not have children AND it is not a new node
// -> expand pruned node
this->expandNode(node);
}
else {
// not a pruned node, create requested child
this->createNodeChild(node, pos);
created_node = true;
}
}
if (lazy_eval)
return updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
else {
NODE* retval = updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
// prune node if possible, otherwise set own probability
// note: combining both did not lead to a speedup!
if (this->pruneNode(node)){
// return pointer to current parent (pruned), the just updated node no longer exists
retval = node;
} else{
node->updateOccupancyChildren();
}
return retval;
}
}
// at last level, update node, end of recursion
else {
if (use_change_detection) {
bool occBefore = this->isNodeOccupied(node);
updateNodeLogOdds(node, log_odds_update);
if (node_just_created){ // new node
changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
} else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it
KeyBoolMap::iterator it = changed_keys.find(key);
if (it == changed_keys.end())
changed_keys.insert(std::pair<OcTreeKey,bool>(key, false));
else if (it->second == false)
changed_keys.erase(it);
}
} else {
updateNodeLogOdds(node, log_odds_update);
}
return node;
}
}
// TODO: mostly copy of updateNodeRecurs => merge code or general tree modifier / traversal
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
unsigned int depth, const float& log_odds_value, bool lazy_eval) {
bool created_node = false;
assert(node);
// follow down to last level
if (depth < this->tree_depth) {
unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
if (!this->nodeChildExists(node, pos)) {
// child does not exist, but maybe it's a pruned node?
if (!this->nodeHasChildren(node) && !node_just_created ) {
// current node does not have children AND it is not a new node
// -> expand pruned node
this->expandNode(node);
}
else {
// not a pruned node, create requested child
this->createNodeChild(node, pos);
created_node = true;
}
}
if (lazy_eval)
return setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
else {
NODE* retval = setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
// prune node if possible, otherwise set own probability
// note: combining both did not lead to a speedup!
if (this->pruneNode(node)){
// return pointer to current parent (pruned), the just updated node no longer exists
retval = node;
} else{
node->updateOccupancyChildren();
}
return retval;
}
}
// at last level, update node, end of recursion
else {
if (use_change_detection) {
bool occBefore = this->isNodeOccupied(node);
node->setLogOdds(log_odds_value);
if (node_just_created){ // new node
changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
} else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it
KeyBoolMap::iterator it = changed_keys.find(key);
if (it == changed_keys.end())
changed_keys.insert(std::pair<OcTreeKey,bool>(key, false));
else if (it->second == false)
changed_keys.erase(it);
}
} else {
node->setLogOdds(log_odds_value);
}
return node;
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::updateInnerOccupancy(){
if (this->root)
this->updateInnerOccupancyRecurs(this->root, 0);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::updateInnerOccupancyRecurs(NODE* node, unsigned int depth){
assert(node);
// only recurse and update for inner nodes:
if (this->nodeHasChildren(node)){
// return early for last level:
if (depth < this->tree_depth){
for (unsigned int i=0; i<8; i++) {
if (this->nodeChildExists(node, i)) {
updateInnerOccupancyRecurs(this->getNodeChild(node, i), depth+1);
}
}
}
node->updateOccupancyChildren();
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::toMaxLikelihood() {
if (this->root == NULL)
return;
// convert bottom up
for (unsigned int depth=this->tree_depth; depth>0; depth--) {
toMaxLikelihoodRecurs(this->root, 0, depth);
}
// convert root
nodeToMaxLikelihood(this->root);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::toMaxLikelihoodRecurs(NODE* node, unsigned int depth,
unsigned int max_depth) {
assert(node);
if (depth < max_depth) {
for (unsigned int i=0; i<8; i++) {
if (this->nodeChildExists(node, i)) {
toMaxLikelihoodRecurs(this->getNodeChild(node, i), depth+1, max_depth);
}
}
}
else { // max level reached
nodeToMaxLikelihood(node);
}
}
template <class NODE>
bool OccupancyOcTreeBase<NODE>::getNormals(const point3d& point, std::vector<point3d>& normals,
bool unknownStatus) const {
normals.clear();
OcTreeKey init_key;
if ( !OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>::coordToKeyChecked(point, init_key) ) {
OCTOMAP_WARNING_STR("Voxel out of bounds");
return false;
}
// OCTOMAP_WARNING("Normal for %f, %f, %f\n", point.x(), point.y(), point.z());
int vertex_values[8];
OcTreeKey current_key;
NODE* current_node;
// There is 8 neighbouring sets
// The current cube can be at any of the 8 vertex
int x_index[4][4] = {{1, 1, 0, 0}, {1, 1, 0, 0}, {0, 0 -1, -1}, {0, 0 -1, -1}};
int y_index[4][4] = {{1, 0, 0, 1}, {0, -1, -1, 0}, {0, -1, -1, 0}, {1, 0, 0, 1}};
int z_index[2][2] = {{0, 1}, {-1, 0}};
// Iterate over the 8 neighboring sets
for(int m = 0; m < 2; ++m){
for(int l = 0; l < 4; ++l){
int k = 0;
// Iterate over the cubes
for(int j = 0; j < 2; ++j){
for(int i = 0; i < 4; ++i){
current_key[0] = init_key[0] + x_index[l][i];
current_key[1] = init_key[1] + y_index[l][i];
current_key[2] = init_key[2] + z_index[m][j];
current_node = this->search(current_key);
if(current_node){
vertex_values[k] = this->isNodeOccupied(current_node);
// point3d coord = this->keyToCoord(current_key);
// OCTOMAP_WARNING_STR("vertex " << k << " at " << coord << "; value " << vertex_values[k]);
}else{
// Occupancy of unknown cells
vertex_values[k] = unknownStatus;
}
++k;
}
}
int cube_index = 0;
if (vertex_values[0]) cube_index |= 1;
if (vertex_values[1]) cube_index |= 2;
if (vertex_values[2]) cube_index |= 4;
if (vertex_values[3]) cube_index |= 8;
if (vertex_values[4]) cube_index |= 16;
if (vertex_values[5]) cube_index |= 32;
if (vertex_values[6]) cube_index |= 64;
if (vertex_values[7]) cube_index |= 128;
// OCTOMAP_WARNING_STR("cubde_index: " << cube_index);
// All vertices are occupied or free resulting in no normal
if (edgeTable[cube_index] == 0)
return true;
// No interpolation is done yet, we use vertexList in <MCTables.h>.
for(int i = 0; triTable[cube_index][i] != -1; i += 3){
point3d p1 = vertexList[triTable[cube_index][i ]];
point3d p2 = vertexList[triTable[cube_index][i+1]];
point3d p3 = vertexList[triTable[cube_index][i+2]];
point3d v1 = p2 - p1;
point3d v2 = p3 - p1;
// OCTOMAP_WARNING("Vertex p1 %f, %f, %f\n", p1.x(), p1.y(), p1.z());
// OCTOMAP_WARNING("Vertex p2 %f, %f, %f\n", p2.x(), p2.y(), p2.z());
// OCTOMAP_WARNING("Vertex p3 %f, %f, %f\n", p3.x(), p3.y(), p3.z());
// Right hand side cross product to retrieve the normal in the good
// direction (pointing to the free nodes).
normals.push_back(v1.cross(v2).normalize());
}
}
}
return true;
}
template <class NODE>
bool OccupancyOcTreeBase<NODE>::castRay(const point3d& origin, const point3d& directionP, point3d& end,
bool ignoreUnknown, double maxRange) const {
/// ---------- see OcTreeBase::computeRayKeys -----------
// Initialization phase -------------------------------------------------------
OcTreeKey current_key;
if ( !OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>::coordToKeyChecked(origin, current_key) ) {
OCTOMAP_WARNING_STR("Coordinates out of bounds during ray casting");
return false;
}
NODE* startingNode = this->search(current_key);
if (startingNode){
if (this->isNodeOccupied(startingNode)){
// Occupied node found at origin
// (need to convert from key, since origin does not need to be a voxel center)
end = this->keyToCoord(current_key);
return true;
}
} else if(!ignoreUnknown){
end = this->keyToCoord(current_key);
return false;
}
point3d direction = directionP.normalized();
bool max_range_set = (maxRange > 0.0);
int step[3];
double tMax[3];
double tDelta[3];
for(unsigned int i=0; i < 3; ++i) {
// compute step direction
if (direction(i) > 0.0) step[i] = 1;
else if (direction(i) < 0.0) step[i] = -1;
else step[i] = 0;
// compute tMax, tDelta
if (step[i] != 0) {
// corner point of voxel (in direction of ray)
double voxelBorder = this->keyToCoord(current_key[i]);
voxelBorder += double(step[i] * this->resolution * 0.5);
tMax[i] = ( voxelBorder - origin(i) ) / direction(i);
tDelta[i] = this->resolution / fabs( direction(i) );
}
else {
tMax[i] = std::numeric_limits<double>::max();
tDelta[i] = std::numeric_limits<double>::max();
}
}
if (step[0] == 0 && step[1] == 0 && step[2] == 0){
OCTOMAP_ERROR("Raycasting in direction (0,0,0) is not possible!");
return false;
}
// for speedup:
double maxrange_sq = maxRange *maxRange;
// Incremental phase ---------------------------------------------------------
bool done = false;
while (!done) {
unsigned int dim;
// find minimum tMax:
if (tMax[0] < tMax[1]){
if (tMax[0] < tMax[2]) dim = 0;
else dim = 2;
}
else {
if (tMax[1] < tMax[2]) dim = 1;
else dim = 2;
}
// check for overflow:
if ((step[dim] < 0 && current_key[dim] == 0)
|| (step[dim] > 0 && current_key[dim] == 2* this->tree_max_val-1))
{
OCTOMAP_WARNING("Coordinate hit bounds in dim %d, aborting raycast\n", dim);
// return border point nevertheless:
end = this->keyToCoord(current_key);
return false;
}
// advance in direction "dim"
current_key[dim] += step[dim];
tMax[dim] += tDelta[dim];
// generate world coords from key
end = this->keyToCoord(current_key);
// check for maxrange:
if (max_range_set){
double dist_from_origin_sq(0.0);
for (unsigned int j = 0; j < 3; j++) {
dist_from_origin_sq += ((end(j) - origin(j)) * (end(j) - origin(j)));
}
if (dist_from_origin_sq > maxrange_sq)
return false;
}
NODE* currentNode = this->search(current_key);
if (currentNode){
if (this->isNodeOccupied(currentNode)) {
done = true;
break;
}
// otherwise: node is free and valid, raycasting continues
} else if (!ignoreUnknown){ // no node found, this usually means we are in "unknown" areas
return false;
}
} // end while
return true;
}
template <class NODE>
bool OccupancyOcTreeBase<NODE>::getRayIntersection (const point3d& origin, const point3d& direction, const point3d& center,
point3d& intersection, double delta/*=0.0*/) const {
// We only need three normals for the six planes
octomap::point3d normalX(1, 0, 0);
octomap::point3d normalY(0, 1, 0);
octomap::point3d normalZ(0, 0, 1);
// One point on each plane, let them be the center for simplicity
octomap::point3d pointXNeg(center(0) - float(this->resolution / 2.0), center(1), center(2));
octomap::point3d pointXPos(center(0) + float(this->resolution / 2.0), center(1), center(2));
octomap::point3d pointYNeg(center(0), center(1) - float(this->resolution / 2.0), center(2));
octomap::point3d pointYPos(center(0), center(1) + float(this->resolution / 2.0), center(2));
octomap::point3d pointZNeg(center(0), center(1), center(2) - float(this->resolution / 2.0));
octomap::point3d pointZPos(center(0), center(1), center(2) + float(this->resolution / 2.0));
double lineDotNormal = 0.0;
double d = 0.0;
double outD = std::numeric_limits<double>::max();
octomap::point3d intersect;
bool found = false;
// Find the intersection (if any) with each place
// Line dot normal will be zero if they are parallel, in which case no intersection can be the entry one
// if there is an intersection does it occur in the bounded plane of the voxel
// if yes keep only the closest (smallest distance to sensor origin).
if((lineDotNormal = normalX.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
d = (pointXNeg - origin).dot(normalX) / lineDotNormal;
intersect = direction * float(d) + origin;
if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
outD = std::min(outD, d);
found = true;
}
d = (pointXPos - origin).dot(normalX) / lineDotNormal;
intersect = direction * float(d) + origin;
if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
outD = std::min(outD, d);
found = true;
}
}
if((lineDotNormal = normalY.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
d = (pointYNeg - origin).dot(normalY) / lineDotNormal;
intersect = direction * float(d) + origin;
if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
outD = std::min(outD, d);
found = true;
}
d = (pointYPos - origin).dot(normalY) / lineDotNormal;
intersect = direction * float(d) + origin;
if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
outD = std::min(outD, d);
found = true;
}
}
if((lineDotNormal = normalZ.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
d = (pointZNeg - origin).dot(normalZ) / lineDotNormal;
intersect = direction * float(d) + origin;
if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
outD = std::min(outD, d);
found = true;
}
d = (pointZPos - origin).dot(normalZ) / lineDotNormal;
intersect = direction * float(d) + origin;
if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
outD = std::min(outD, d);
found = true;
}
}
// Substract (add) a fraction to ensure no ambiguity on the starting voxel
// Don't start on a boundary.
if(found)
intersection = direction * float(outD + delta) + origin;
return found;
}
template <class NODE> inline bool
OccupancyOcTreeBase<NODE>::integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval) {
if (!this->computeRayKeys(origin, end, this->keyrays.at(0))) {
return false;
}
for(KeyRay::iterator it=this->keyrays[0].begin(); it != this->keyrays[0].end(); it++) {
updateNode(*it, false, lazy_eval); // insert freespace measurement
}
return true;
}
template <class NODE> bool
OccupancyOcTreeBase<NODE>::insertRay(const point3d& origin, const point3d& end, double maxrange, bool lazy_eval)
{
// cut ray at maxrange
if ((maxrange > 0) && ((end - origin).norm () > maxrange))
{
point3d direction = (end - origin).normalized ();
point3d new_end = origin + direction * (float) maxrange;
return integrateMissOnRay(origin, new_end,lazy_eval);
}
// insert complete ray
else
{
if (!integrateMissOnRay(origin, end,lazy_eval))
return false;
updateNode(end, true, lazy_eval); // insert hit cell
return true;
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::setBBXMin (const point3d& min) {
bbx_min = min;
if (!this->coordToKeyChecked(bbx_min, bbx_min_key)) {
OCTOMAP_ERROR("ERROR while generating bbx min key.\n");
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::setBBXMax (const point3d& max) {
bbx_max = max;
if (!this->coordToKeyChecked(bbx_max, bbx_max_key)) {
OCTOMAP_ERROR("ERROR while generating bbx max key.\n");
}
}
template <class NODE>
bool OccupancyOcTreeBase<NODE>::inBBX(const point3d& p) const {
return ((p.x() >= bbx_min.x()) && (p.y() >= bbx_min.y()) && (p.z() >= bbx_min.z()) &&
(p.x() <= bbx_max.x()) && (p.y() <= bbx_max.y()) && (p.z() <= bbx_max.z()) );
}
template <class NODE>
bool OccupancyOcTreeBase<NODE>::inBBX(const OcTreeKey& key) const {
return ((key[0] >= bbx_min_key[0]) && (key[1] >= bbx_min_key[1]) && (key[2] >= bbx_min_key[2]) &&
(key[0] <= bbx_max_key[0]) && (key[1] <= bbx_max_key[1]) && (key[2] <= bbx_max_key[2]) );
}
template <class NODE>
point3d OccupancyOcTreeBase<NODE>::getBBXBounds () const {
octomap::point3d obj_bounds = (bbx_max - bbx_min);
obj_bounds /= 2.;
return obj_bounds;
}
template <class NODE>
point3d OccupancyOcTreeBase<NODE>::getBBXCenter () const {
octomap::point3d obj_bounds = (bbx_max - bbx_min);
obj_bounds /= 2.;
return bbx_min + obj_bounds;
}
// -- I/O -----------------------------------------
template <class NODE>
std::istream& OccupancyOcTreeBase<NODE>::readBinaryData(std::istream &s){
// tree needs to be newly created or cleared externally
if (this->root) {
OCTOMAP_ERROR_STR("Trying to read into an existing tree.");
return s;
}
this->root = new NODE();
this->readBinaryNode(s, this->root);
this->size_changed = true;
this->tree_size = OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>::calcNumNodes(); // compute number of nodes
return s;
}
template <class NODE>
std::ostream& OccupancyOcTreeBase<NODE>::writeBinaryData(std::ostream &s) const{
OCTOMAP_DEBUG("Writing %zu nodes to output stream...", this->size());
if (this->root)
this->writeBinaryNode(s, this->root);
return s;
}
template <class NODE>
std::istream& OccupancyOcTreeBase<NODE>::readBinaryNode(std::istream &s, NODE* node){
assert(node);
char child1to4_char;
char child5to8_char;
s.read((char*)&child1to4_char, sizeof(char));
s.read((char*)&child5to8_char, sizeof(char));
std::bitset<8> child1to4 ((unsigned long long) child1to4_char);
std::bitset<8> child5to8 ((unsigned long long) child5to8_char);
// std::cout << "read: "
// << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
// << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
// inner nodes default to occupied
node->setLogOdds(this->clamping_thres_max);
for (unsigned int i=0; i<4; i++) {
if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 0)) {
// child is free leaf
this->createNodeChild(node, i);
this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_min);
}
else if ((child1to4[i*2] == 0) && (child1to4[i*2+1] == 1)) {
// child is occupied leaf
this->createNodeChild(node, i);
this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_max);
}
else if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 1)) {
// child has children
this->createNodeChild(node, i);
this->getNodeChild(node, i)->setLogOdds(-200.); // child is unkown, we leave it uninitialized
}
}
for (unsigned int i=0; i<4; i++) {
if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 0)) {
// child is free leaf
this->createNodeChild(node, i+4);
this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_min);
}
else if ((child5to8[i*2] == 0) && (child5to8[i*2+1] == 1)) {
// child is occupied leaf
this->createNodeChild(node, i+4);
this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_max);
}
else if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 1)) {
// child has children
this->createNodeChild(node, i+4);
this->getNodeChild(node, i+4)->setLogOdds(-200.); // set occupancy when all children have been read
}
// child is unkown, we leave it uninitialized
}
// read children's children and set the label
for (unsigned int i=0; i<8; i++) {
if (this->nodeChildExists(node, i)) {
NODE* child = this->getNodeChild(node, i);
if (fabs(child->getLogOdds() + 200.)<1e-3) {
readBinaryNode(s, child);
child->setLogOdds(child->getMaxChildLogOdds());
}
} // end if child exists
} // end for children
return s;
}
template <class NODE>
std::ostream& OccupancyOcTreeBase<NODE>::writeBinaryNode(std::ostream &s, const NODE* node) const{
assert(node);
// 2 bits for each children, 8 children per node -> 16 bits
std::bitset<8> child1to4;
std::bitset<8> child5to8;
// 10 : child is free node
// 01 : child is occupied node
// 00 : child is unkown node
// 11 : child has children
// speedup: only set bits to 1, rest is init with 0 anyway,
// can be one logic expression per bit
for (unsigned int i=0; i<4; i++) {
if (this->nodeChildExists(node, i)) {
const NODE* child = this->getNodeChild(node, i);
if (this->nodeHasChildren(child)) { child1to4[i*2] = 1; child1to4[i*2+1] = 1; }
else if (this->isNodeOccupied(child)) { child1to4[i*2] = 0; child1to4[i*2+1] = 1; }
else { child1to4[i*2] = 1; child1to4[i*2+1] = 0; }
}
else {
child1to4[i*2] = 0; child1to4[i*2+1] = 0;
}
}
for (unsigned int i=0; i<4; i++) {
if (this->nodeChildExists(node, i+4)) {
const NODE* child = this->getNodeChild(node, i+4);
if (this->nodeHasChildren(child)) { child5to8[i*2] = 1; child5to8[i*2+1] = 1; }
else if (this->isNodeOccupied(child)) { child5to8[i*2] = 0; child5to8[i*2+1] = 1; }
else { child5to8[i*2] = 1; child5to8[i*2+1] = 0; }
}
else {
child5to8[i*2] = 0; child5to8[i*2+1] = 0;
}
}
// std::cout << "wrote: "
// << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
// << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
char child1to4_char = (char) child1to4.to_ulong();
char child5to8_char = (char) child5to8.to_ulong();
s.write((char*)&child1to4_char, sizeof(char));
s.write((char*)&child5to8_char, sizeof(char));
// write children's children
for (unsigned int i=0; i<8; i++) {
if (this->nodeChildExists(node, i)) {
const NODE* child = this->getNodeChild(node, i);
if (this->nodeHasChildren(child)) {
writeBinaryNode(s, child);
}
}
}
return s;
}
//-- Occupancy queries on nodes:
template <class NODE>
void OccupancyOcTreeBase<NODE>::updateNodeLogOdds(NODE* occupancyNode, const float& update) const {
occupancyNode->addValue(update);
if (occupancyNode->getLogOdds() < this->clamping_thres_min) {
occupancyNode->setLogOdds(this->clamping_thres_min);
return;
}
if (occupancyNode->getLogOdds() > this->clamping_thres_max) {
occupancyNode->setLogOdds(this->clamping_thres_max);
}
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::integrateHit(NODE* occupancyNode) const {
updateNodeLogOdds(occupancyNode, this->prob_hit_log);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::integrateMiss(NODE* occupancyNode) const {
updateNodeLogOdds(occupancyNode, this->prob_miss_log);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::nodeToMaxLikelihood(NODE* occupancyNode) const{
if (this->isNodeOccupied(occupancyNode))
occupancyNode->setLogOdds(this->clamping_thres_max);
else
occupancyNode->setLogOdds(this->clamping_thres_min);
}
template <class NODE>
void OccupancyOcTreeBase<NODE>::nodeToMaxLikelihood(NODE& occupancyNode) const{
if (this->isNodeOccupied(occupancyNode))
occupancyNode.setLogOdds(this->clamping_thres_max);
else
occupancyNode.setLogOdds(this->clamping_thres_min);
}
} // namespace
Reference in New Issue View Git Blame Copy Permalink