| 86 | } |
| 87 | |
| 88 | bool OPTCalibration::analyzeData(const cb::PinholeSensor::Ptr & color_sensor, |
| 89 | const cb::DepthSensor::Ptr & depth_sensor, |
| 90 | const cv::Mat & image, |
| 91 | const cb::PCLCloud3::Ptr & cloud, |
| 92 | CheckerboardView::Ptr & color_cb_view, |
| 93 | CheckerboardView::Ptr & depth_cb_view) |
| 94 | { |
| 95 | OPTCheckerboardExtraction ex; |
| 96 | ex.setImage(image); |
| 97 | ex.setCloud(cloud); |
| 98 | ex.setColorSensor(color_sensor); |
| 99 | ex.setDepthSensor(depth_sensor); |
| 100 | ex.setDepthTransform(depth_sensor->pose()); |
| 101 | ex.setCheckerboard(checkerboard_); |
| 102 | |
| 103 | cb::PinholeView<cb::Checkerboard>::Ptr color_view; |
| 104 | cb::DepthViewPCL<cb::PlanarObject>::Ptr depth_view; |
| 105 | cb::Checkerboard::Ptr extracted_checkerboard; |
| 106 | cb::PlanarObject::Ptr extracted_plane; |
| 107 | if (ex.perform(color_view, depth_view, extracted_checkerboard, extracted_plane)) |
| 108 | { |
| 109 | visualization_msgs::Marker checkerboard_marker; |
| 110 | checkerboard_marker.ns = "checkerboard"; |
| 111 | checkerboard_marker.id = node_map_[color_sensor]->id(); |
| 112 | extracted_checkerboard->toMarker(checkerboard_marker); |
| 113 | marker_pub_.publish(checkerboard_marker); |
| 114 | |
| 115 | visualization_msgs::Marker plane_marker; |
| 116 | plane_marker.ns = "plane"; |
| 117 | plane_marker.id = node_map_[color_sensor]->id(); |
| 118 | extracted_plane->toMarker(plane_marker); |
| 119 | marker_pub_.publish(plane_marker); |
| 120 | |
| 121 | geometry_msgs::TransformStamped transform_msg; |
| 122 | extracted_checkerboard->toTF(transform_msg); |
| 123 | tf_pub_.sendTransform(transform_msg); |
| 124 | extracted_plane->toTF(transform_msg); |
| 125 | tf_pub_.sendTransform(transform_msg); |
| 126 | |
| 127 | color_cb_view = boost::make_shared<CheckerboardView>(color_view, |
| 128 | extracted_checkerboard, |
| 129 | extracted_checkerboard->center(), |
| 130 | floorAcquisition()); |
| 131 | |
| 132 | depth_cb_view = boost::make_shared<CheckerboardView>(depth_view, |
| 133 | extracted_plane, |
| 134 | depth_view->centroid(), |
| 135 | floorAcquisition()); |
| 136 | |
| 137 | // analyzeData(color_sensor, color_view, extracted_checkerboard, extracted_checkerboard->center()); |
| 138 | // analyzeData(depth_sensor, depth_view, extracted_plane, depth_view->centroid()); |
| 139 | |
| 140 | return true; |
| 141 | } |
| 142 | return false; |
| 143 | } |
| 144 | |
| 145 | bool OPTCalibration::analyzeData(const cb::PinholeSensor::Ptr & color_sensor, |
no test coverage detected