예제 #1
0
 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;
 }
예제 #2
0
 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;
 }