| 161 | } |
| 162 | |
| 163 | ColorOcTreeNode* ColorOcTree::integrateNodeColor(const OcTreeKey& key, |
| 164 | uint8_t r, |
| 165 | uint8_t g, |
| 166 | uint8_t b) { |
| 167 | ColorOcTreeNode* n = search (key); |
| 168 | if (n != 0) { |
| 169 | if (n->isColorSet()) { |
| 170 | ColorOcTreeNode::Color prev_color = n->getColor(); |
| 171 | double node_prob = n->getOccupancy(); |
| 172 | uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob |
| 173 | + (double) r * (0.99-node_prob)); |
| 174 | uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob |
| 175 | + (double) g * (0.99-node_prob)); |
| 176 | uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob |
| 177 | + (double) b * (0.99-node_prob)); |
| 178 | n->setColor(new_r, new_g, new_b); |
| 179 | } |
| 180 | else { |
| 181 | n->setColor(r, g, b); |
| 182 | } |
| 183 | } |
| 184 | return n; |
| 185 | } |
| 186 | |
| 187 | |
| 188 | void ColorOcTree::updateInnerOccupancy() { |
nothing calls this directly
no test coverage detected