| 298 | } |
| 299 | |
| 300 | void OPTCalibrationNode::spin() |
| 301 | { |
| 302 | ros::Rate rate(5.0); |
| 303 | |
| 304 | while (ros::ok()) |
| 305 | { |
| 306 | ros::spinOnce(); |
| 307 | calibration_->nextAcquisition(); |
| 308 | |
| 309 | int count = 0; |
| 310 | |
| 311 | try |
| 312 | { |
| 313 | #pragma omp parallel for |
| 314 | for (size_t i = 0; i < pinhole_vec_.size(); ++i) |
| 315 | { |
| 316 | const PinholeRGBDevice::Ptr & device = pinhole_vec_[i]; |
| 317 | if (device->hasNewMessages()) |
| 318 | { |
| 319 | device->convertLastMessages(); |
| 320 | PinholeRGBDevice::Data::Ptr data = device->lastData(); |
| 321 | OPTCalibration::CheckerboardView::Ptr cb_view; |
| 322 | ROS_DEBUG_STREAM("[" << device->frameId() << "] analysing image generated at: " << device->lastMessages().image_msg->header.stamp); |
| 323 | #pragma omp critical |
| 324 | if (calibration_->analyzeData(device->sensor(), data->image, cb_view)) |
| 325 | { |
| 326 | calibration_->addData(device->sensor(), cb_view); |
| 327 | images_acquired_map_[device->frameId()]++; |
| 328 | ROS_INFO_STREAM("[" << device->frameId() << "] checkerboard detected"); |
| 329 | ++count; |
| 330 | } |
| 331 | } |
| 332 | } |
| 333 | for (size_t i = 0; i < kinect_vec_.size(); ++i) |
| 334 | { |
| 335 | const KinectDevice::Ptr & device = kinect_vec_[i]; |
| 336 | if (device->hasNewMessages()) |
| 337 | { |
| 338 | device->convertLastMessages(); |
| 339 | KinectDevice::Data::Ptr data = device->lastData(); |
| 340 | OPTCalibration::CheckerboardView::Ptr color_cb_view; |
| 341 | OPTCalibration::CheckerboardView::Ptr depth_cb_view; |
| 342 | ROS_DEBUG_STREAM("[" << device->colorFrameId() << "] analysing image generated at: " << device->lastMessages().image_msg->header.stamp); |
| 343 | ROS_DEBUG_STREAM("[" << device->depthFrameId() << "] analysing cloud generated at: " << device->lastMessages().cloud_msg->header.stamp); |
| 344 | if (calibration_->analyzeData(device->colorSensor(), device->depthSensor(), |
| 345 | data->image, data->cloud, |
| 346 | color_cb_view, depth_cb_view)) |
| 347 | { |
| 348 | calibration_->addData(device->colorSensor(), color_cb_view); |
| 349 | calibration_->addData(device->depthSensor(), depth_cb_view); |
| 350 | images_acquired_map_[device->colorFrameId()]++; |
| 351 | ROS_INFO_STREAM("[" << device->colorFrameId() << "] checkerboard detected"); |
| 352 | ++count; |
| 353 | } |
| 354 | } |
| 355 | } |
| 356 | for (size_t i = 0; i < swiss_ranger_vec_.size(); ++i) |
| 357 | { |
no test coverage detected