ColorOcTreeNode* ColorOcTree::averageNodeColor(const OcTreeKey& key,
const unsigned char& r,
const unsigned char& g,
const unsigned char& b) {
ColorOcTreeNode* n = search (key);
if (n != 0) {
if (n->isColorSet()) {
ColorOcTreeNode::Color prev_color = n->getColor();
n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
}
else {
n->setColor(r, g, b);
}
}
return n;
}
ColorOcTreeNode* ColorOcTree::integrateNodeColor(const OcTreeKey& key,
const unsigned char& r,
const unsigned char& g,
const unsigned char& b) {
ColorOcTreeNode* n = search (key);
if (n != 0) {
if (n->isColorSet()) {
ColorOcTreeNode::Color prev_color = n->getColor();
double node_prob = n->getOccupancy();
unsigned char new_r = (unsigned char) ((double) prev_color.r * node_prob
+ (double) r * (0.99-node_prob));
unsigned char new_g = (unsigned char) ((double) prev_color.g * node_prob
+ (double) g * (0.99-node_prob));
unsigned char new_b = (unsigned char) ((double) prev_color.b * node_prob
+ (double) b * (0.99-node_prob));
n->setColor(new_r, new_g, new_b);
}
else {
n->setColor(r, g, b);
}
}
return n;
}