| 177 | } |
| 178 | |
| 179 | void OPTCalibration::perform() |
| 180 | { |
| 181 | const ViewMap & view_map = view_map_vec_.back(); |
| 182 | |
| 183 | if (initialization_) |
| 184 | { |
| 185 | if (not tree_initialized_ and not view_map.empty()) // Set /world |
| 186 | { |
| 187 | ViewMap::const_iterator it = view_map.begin(); |
| 188 | |
| 189 | while (it != view_map.end() and it->first->type() == TreeNode::DEPTH) |
| 190 | ++it; |
| 191 | |
| 192 | if (it == view_map.end()) |
| 193 | { |
| 194 | view_map_vec_.resize(view_map_vec_.size() - 1); |
| 195 | return; |
| 196 | } |
| 197 | |
| 198 | TreeNode::Ptr tree_node = it->first; |
| 199 | tree_node->sensor()->setParent(world_); |
| 200 | tree_node->setLevel(0); |
| 201 | |
| 202 | ROS_INFO_STREAM(tree_node->sensor()->frameId() << " added to the tree."); |
| 203 | tree_initialized_ = true; |
| 204 | } |
| 205 | |
| 206 | if (view_map.empty()) |
| 207 | { |
| 208 | view_map_vec_.resize(view_map_vec_.size() - 1); // Remove data |
| 209 | } |
| 210 | else if (view_map.size() >= 2) // At least 2 cameras |
| 211 | { |
| 212 | int min_level = TreeNode::MAX_LEVEL; |
| 213 | TreeNode::Ptr min_sensor_node; |
| 214 | for (ViewMap::const_iterator it = view_map.begin(); it != view_map.end(); ++it) |
| 215 | { |
| 216 | TreeNode::Ptr tree_node = it->first; |
| 217 | if (tree_node->level() < min_level) |
| 218 | { |
| 219 | min_level = tree_node->level(); |
| 220 | min_sensor_node = tree_node; |
| 221 | } |
| 222 | } |
| 223 | |
| 224 | if (min_level < TreeNode::MAX_LEVEL) // At least one already in tree |
| 225 | { |
| 226 | for (ViewMap::const_iterator it = view_map.begin(); it != view_map.end(); ++it) |
| 227 | { |
| 228 | TreeNode::Ptr tree_node = it->first; |
| 229 | |
| 230 | if (tree_node != min_sensor_node) |
| 231 | { |
| 232 | const CheckerboardView::Ptr & view = it->second; |
| 233 | cb::PlanarObject::Ptr planar_object = view->object; |
| 234 | cb::Point3 center = view->center; |
| 235 | |
| 236 | if (tree_node->type() == TreeNode::INTENSITY) |