$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #include <srs_env_model/but_server/plugins/octomap_plugin_tools/octomap_filter_raycast.h> 00029 #include <pcl/sample_consensus/method_types.h> 00030 #include <pcl/sample_consensus/model_types.h> 00031 #include <pcl/segmentation/sac_segmentation.h> 00032 #include <pcl/filters/extract_indices.h> 00033 #include <pcl/filters/passthrough.h> 00034 #include <pcl_ros/transforms.h> 00035 #include <pcl/io/io.h> 00036 #include <visualization_msgs/Marker.h> 00037 00038 #include <srs_env_model/topics_list.h> 00039 00040 #define SHOW_VISUALIZATION 00041 //#define NEW_RAYCAST 00042 //#define PUBLISH_GRID 00043 00047 srs_env_model::COcFilterRaycast::COcFilterRaycast(const std::string & octree_frame_id, ERunMode mode /*= FILTER_ALLWAYS*/) 00048 : COcTreeFilterBase( octree_frame_id, mode ) 00049 , m_bFilterInitialized(false) 00050 , m_camera_stereo_offset_left(128) 00051 , m_camera_stereo_offset_right(0) 00052 , m_bCamModelInitialized(false) 00053 , m_camera_info_topic(CAMERA_INFO_TOPIC_NAME) 00054 , m_numLeafsRemoved(0) 00055 , m_filtered_cloud( new tPointCloud()) 00056 , m_miss_speedup( 2.0f ) 00057 { 00058 00059 } 00060 00064 void srs_env_model::COcFilterRaycast::init(ros::NodeHandle & node_handle) 00065 { 00066 00067 // stereo cam params for sensor cone: 00068 node_handle.param<int> ("camera_stereo_offset_left", 00069 m_camera_stereo_offset_left, 0); 00070 node_handle.param<int> ("camera_stereo_offset_right", 00071 m_camera_stereo_offset_right, 0); 00072 00073 // Used camera info topic 00074 node_handle.param("camera_info_topic", m_camera_info_topic, 00075 m_camera_info_topic); 00076 00077 #ifdef SHOW_VISUALIZATION 00078 std::cerr << "Initializing Raytrace filter visualization marker publisher" << std::endl; 00079 marker_pub_ = node_handle.advertise<visualization_msgs::Marker>("visualization_marker", 5); 00080 #endif 00081 00082 #ifdef PUBLISH_GRID 00083 grid_pub_ = node_handle.advertise<sensor_msgs::PointCloud2> ("grid_cloud", 5); 00084 #endif 00085 00086 // Add camera info subscriber 00087 m_ciSubscriber = node_handle.subscribe(m_camera_info_topic, 1, 00088 &COcFilterRaycast::cameraInfoCB, this); 00089 } 00090 00094 void srs_env_model::COcFilterRaycast::setCloud(tPointCloudConstPtr cloud) 00095 { 00096 assert( cloud != 0 ); 00097 m_lockData.lock(); 00098 m_cloudPtr = cloud; 00099 m_lockData.unlock(); 00100 } 00101 00103 void srs_env_model::COcFilterRaycast::writeLastRunInfo() 00104 { 00105 ROS_DEBUG_STREAM("COcFilterRaycast: Number of leafs removed: " << m_numLeafsRemoved); 00106 ROS_DEBUG_STREAM("COcFilterRaycast: Number of leafs out of cone: " << m_numLeafsOutOfCone); 00107 ROS_DEBUG_STREAM("Camera position: " << m_sensor_origin.x() << ", " << m_sensor_origin.y() << ", " << m_sensor_origin.z()); 00108 } 00109 00113 void srs_env_model::COcFilterRaycast::cameraInfoCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info) { 00114 00115 boost::mutex::scoped_lock lock( m_lockCamera ); 00116 00117 // Get camera model from camera info 00118 m_camera_model.fromCameraInfo(*cam_info); 00119 m_camera_size = m_camera_model.fullResolution(); 00120 m_sensor_header = cam_info->header; 00121 00122 // HACK 00123 m_sensor_header.frame_id = "/head_cam3d_link"; 00124 00125 // Set flag 00126 m_bCamModelInitialized = true; 00127 } 00128 00129 #ifndef NEW_RAYCAST 00130 00133 void srs_env_model::COcFilterRaycast::filterInternal( tButServerOcTree & tree ) 00134 { 00135 assert( m_cloudPtr != 0 ); 00136 00137 m_numLeafsRemoved = 0; 00138 m_numLeafsOutOfCone = 0; 00139 00140 //octomap::pose6d sensor_pose(m_sensor_origin.x(), m_sensor_origin.y(), m_sensor_origin.z(), 0, 0, 0); 00141 00142 // Is camera model initialized? 00143 if (!m_bCamModelInitialized) 00144 { 00145 ROS_ERROR("ERROR: camera model not initialized."); 00146 return; 00147 } 00148 00149 // Get sensor origin 00150 m_sensor_origin = getSensorOrigin(m_sensor_header); 00151 00152 tf::StampedTransform trans; 00153 try 00154 { 00155 m_tfListener.lookupTransform(m_sensor_header.frame_id, m_cloudPtr->header.frame_id, m_cloudPtr->header.stamp, trans); 00156 } 00157 catch (tf::TransformException& ex) 00158 { 00159 ROS_ERROR_STREAM("ERROR: Cannot find transform: " << m_cloudPtr->header.frame_id << " -> " << m_sensor_header.frame_id); 00160 return; 00161 } 00162 00163 tf::Transform to_sensor = trans; 00164 00165 // compute bbx from sensor cone 00166 octomap::point3d min; 00167 octomap::point3d max; 00168 computeBBX(m_sensor_header, min, max); 00169 00170 double resolution(tree.getResolution()); 00171 float probMissLog(tree.getProbMissLog() * m_miss_speedup); 00172 00173 boost::mutex::scoped_lock lock(m_lockCamera); 00174 00175 float probDeleted(tree.getProbMiss() * 1.0f); 00176 unsigned query_time = time(NULL); 00177 unsigned max_update_time = 1; 00178 for (tButServerOcTree::leaf_bbx_iterator it = 00179 tree.begin_leafs_bbx(min, max), end = tree.end_leafs_bbx(); it 00180 != end; ++it) { 00181 if (tree.isNodeOccupied(*it) && ((query_time - it->getTimestamp()) 00182 > max_update_time)) { 00183 tf::Point pos(it.getX(), it.getY(), it.getZ()); 00184 tf::Point posRel = to_sensor(pos); 00185 cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( 00186 posRel.x(), posRel.y(), posRel.z())); 00187 00188 // ignore point if not in sensor cone 00189 if (!inSensorCone(uv)) 00190 { 00191 ++m_numLeafsOutOfCone; 00192 continue; 00193 } 00194 00195 // ignore point if it is occluded in the map 00196 if (isOccludedMap(m_sensor_origin, it.getCoordinate(), resolution, tree)) 00197 continue; 00198 00199 // otherwise: degrade node 00200 // it->setColor(255, 0, 0); 00201 // it->setValue(probMiss); 00202 // tree.integrateMissNoTime(&(*it)); 00203 tree.updateNodeLogOdds(&(*it), probMissLog); 00204 00205 ++m_numLeafsRemoved; 00206 } 00207 } 00208 } 00209 00210 #else 00211 00215 void srs_env_model::COcFilterRaycast::filterInternal( tButServerOcTree & tree ) 00216 { 00217 assert( m_cloudPtr != 0 ); 00218 00219 // Is camera model initialized? 00220 if (!m_bCamModelInitialized) 00221 { 00222 ROS_ERROR("ERROR: camera model not initialized."); 00223 return; 00224 } 00225 00226 // compute bbx from sensor cone 00227 octomap::point3d min; 00228 octomap::point3d max; 00229 computeBBX(m_sensor_header, min, max); 00230 00231 m_lockData.lock(); 00232 00233 m_numLeafsRemoved = 0; 00234 m_numLeafsOutOfCone = 0; 00235 00236 // Get sensor origin 00237 m_sensor_origin = getSensorOrigin(m_sensor_header); 00238 00239 float resolution(tree.getResolution()); 00240 00241 m_vgfilter.setLeafSize(resolution, resolution, resolution); 00242 m_vgfilter.setInputCloud(m_cloudPtr); 00243 00244 // std::cerr << "Voxelgrid filter. Resolution: " << resolution << " Input cloud size: " << m_cloudPtr->size() << std::endl; 00245 // std::cerr << "Output cloud: " << m_filtered_cloud << ", size: " << m_filtered_cloud->size() << std::endl; 00246 00247 m_vgfilter.filter(*m_filtered_cloud); 00248 00249 00250 // std::cerr << "Raytracing. Grid size: " << m_filtered_cloud->size() << std::endl; 00251 00252 #ifdef PUBLISH_GRID 00253 tPointCloud::Ptr ends_cloud(new tPointCloud ); 00254 #endif 00255 00256 float probMiss(tree.getProbMissLog()); 00257 00258 // For all points in cloud 00259 tPointCloud::VectorType::const_iterator it, itEnd( m_filtered_cloud->points.end()); 00260 00261 for( it = m_filtered_cloud->points.begin(); it != itEnd; ++it) 00262 { 00263 // Cast ray through octomap 00264 octomap::point3d direction(octomap::point3d(it->x, it->y, it->z) - m_sensor_origin); 00265 // std::cerr << "D: " << direction.x() << ", " << direction.y() << ", " << direction.z() << std::endl; 00266 00267 octomap::point3d obstacle; 00268 double range = direction.norm() - resolution; 00269 00270 octomap::point3d ray_end( m_sensor_origin + direction.normalized() * range); 00271 00272 //* 00273 #ifdef PUBLISH_GRID 00274 tPclPoint point; 00275 point.x = ray_end.x(); point.y = ray_end.y(); point.z = ray_end.z(); 00276 point.r = point.g = point.b = 255; 00277 00278 ends_cloud->points.push_back(point); 00279 #endif 00280 //*/ 00281 00282 octomap::KeyRay keyray; 00283 00284 if(tree.computeRayKeys(m_sensor_origin, ray_end, keyray)) 00285 { 00286 // For all found cells 00287 octomap::KeyRay::const_iterator itc, itcEnd( keyray.end()); 00288 for ( itc = keyray.begin(); itc != itcEnd; ++itc) 00289 { 00290 tButServerOcTree::NodeType * node(0); 00291 node = tree.search(*itc); 00292 00293 if( node != 0 && tree.isNodeOccupied(node) && (!node->hasChildren())) 00294 { 00295 octomap::point3d point; 00296 tree.genCoords(*itc, tree.getTreeDepth(), point); 00297 /* 00298 #ifdef PUBLISH_GRID 00299 tPclPoint pclpoint; 00300 pclpoint.x = point.x(); pclpoint.y = point.y(); pclpoint.z = point.z(); 00301 pclpoint.r = pclpoint.g = pclpoint.b = 255; 00302 00303 ends_cloud->points.push_back(pclpoint); 00304 #endif 00305 */ 00306 node->setValue(probMiss); 00307 /* 00308 float node_range( (point-m_sensor_origin).norm() ); 00309 if( node_range > range) 00310 node->setColor(255, 0, 0 ); 00311 else 00312 node->setColor(0, 255.0*float(node_range/(0.1 + range)), 0 ); 00313 00314 min = -(node_range-range) < min ? range : min; 00315 max = -(node_range-range) > max ? range : max; 00316 00317 */ 00318 ++m_numLeafsRemoved; 00319 } 00320 } 00321 00322 } 00323 00324 //*/ 00325 00326 } 00327 00328 #ifdef PUBLISH_GRID 00329 sensor_msgs::PointCloud2 cloud; 00330 // pcl::toROSMsg< tPclPoint >(*m_filtered_cloud, cloud); 00331 pcl::toROSMsg< tPclPoint >(*ends_cloud, cloud); 00332 00333 // Set message parameters and publish 00334 cloud.header.frame_id = m_cloudPtr->header.frame_id; 00335 cloud.header.stamp = m_cloudPtr->header.stamp; 00336 00337 grid_pub_.publish(cloud); 00338 #endif 00339 00340 m_lockData.unlock(); 00341 } 00342 00343 #endif 00344 00347 octomap::point3d srs_env_model::COcFilterRaycast::getSensorOrigin(const std_msgs::Header& sensor_header) 00348 { 00349 geometry_msgs::PointStamped stamped_in; 00350 geometry_msgs::PointStamped stamped_out; 00351 stamped_in.header = sensor_header; 00352 00353 std::string fixed_frame_(m_treeFrameId); 00354 00355 // HACK: laser origin 00356 if (sensor_header.frame_id == "base_footprint") 00357 { 00358 stamped_in.header.frame_id = "laser_tilt_link"; 00359 } 00360 00361 geometry_msgs::Point p; 00362 p.x = p.y = p.z = 0; 00363 try { 00364 m_tfListener.transformPoint(fixed_frame_, stamped_in, stamped_out); 00365 } catch (tf::TransformException& ex) { 00366 ros::Time t; 00367 std::string err_string; 00368 ROS_INFO_STREAM("Transforming sensor origin using latest common time because there's a tf problem"); 00369 if (m_tfListener.getLatestCommonTime(fixed_frame_, stamped_in.header.frame_id, 00370 stamped_in.header.stamp, &err_string) == tf::NO_ERROR) 00371 { 00372 try { 00373 m_tfListener.transformPoint(fixed_frame_, stamped_in, stamped_out); 00374 } catch (...) { 00375 ROS_WARN_STREAM("Still can't transform sensor origin between " << fixed_frame_ << " and " << stamped_in.header.frame_id); 00376 } 00377 } 00378 else 00379 { 00380 ROS_WARN_STREAM("No common time between " << fixed_frame_ << " and " << stamped_in.header.frame_id); 00381 } 00382 } 00383 00384 octomap::point3d retval(stamped_out.point.x, stamped_out.point.y, 00385 stamped_out.point.z); 00386 00387 return retval; 00388 } 00392 bool srs_env_model::COcFilterRaycast::inSensorCone(const cv::Point2d& uv) const { 00393 // Check if projected 2D coordinate in pixel range. 00394 // This check is a little more restrictive than it should be by using 00395 // 1 pixel less to account for rounding / discretization errors. 00396 // Otherwise points on the corner are accounted to be in the sensor cone. 00397 return ( (uv.x > m_camera_stereo_offset_left + 1) 00398 && (uv.x < m_camera_size.width + m_camera_stereo_offset_right - 2) 00399 && (uv.y > 1) 00400 && (uv.y < m_camera_size.height - 2)); 00401 } 00402 00406 bool srs_env_model::COcFilterRaycast::isOccludedMap(const octomap::point3d& sensor_origin, const octomap::point3d& p, double resolution, tButServerOcTree & tree) const 00407 { 00408 octomap::point3d direction(p - sensor_origin); 00409 octomap::point3d obstacle; 00410 double range = direction.norm() - resolution; 00411 00412 if (tree.castRay(sensor_origin, direction, obstacle, true, range)) 00413 { 00414 // fprintf(stderr, "<%.2f , %.2f , %.2f> -> <%.2f , %.2f , %.2f> // obs at: <%.2f , %.2f , %.2f>, range: %.2f\n", 00415 // sensor_origin.x(), sensor_origin.y(), sensor_origin.z(), 00416 // p.x(), p.y(), p.z(), 00417 // obstacle.x(), obstacle.y(), obstacle.z(), (obstacle-p).norm()); 00418 return true; 00419 } 00420 return false; 00421 } 00422 00423 00427 void srs_env_model::COcFilterRaycast::computeBBX(const std_msgs::Header& sensor_header, octomap::point3d& bbx_min, octomap::point3d& bbx_max) 00428 { 00429 std::string sensor_frame = sensor_header.frame_id; 00430 00431 // transform sensor FOV 00432 geometry_msgs::PointStamped stamped_in; 00433 geometry_msgs::PointStamped stamped_out; 00434 stamped_in.header = sensor_header; 00435 stamped_in.header.frame_id = sensor_frame; 00436 00437 // get max 3d points from camera at 0.5m and 5m. 00438 geometry_msgs::Point p[8]; 00439 00440 // define min/max 2d points 00441 cv::Point2d uv[4]; 00442 uv[0].x = m_camera_stereo_offset_left; 00443 uv[0].y = 0; 00444 uv[1].x = m_camera_size.width + m_camera_stereo_offset_right; 00445 uv[1].y = 0; 00446 uv[2].x = m_camera_size.width + m_camera_stereo_offset_right; 00447 uv[2].y = m_camera_size.height; 00448 uv[3].x = m_camera_stereo_offset_left; 00449 uv[3].y = m_camera_size.height; 00450 00451 // transform to 3d space 00452 cv::Point3d xyz[4]; 00453 for (int i = 0; i < 4; i++) { 00454 xyz[i] = m_camera_model.projectPixelTo3dRay(uv[i]); 00455 cv::Point3d xyz_05 = xyz[i] * 0.5; 00456 xyz[i] *= 5.; // 5meters 00457 p[i].x = xyz[i].x; 00458 p[i].y = xyz[i].y; 00459 p[i].z = xyz[i].z; 00460 p[i + 4].x = xyz_05.x; 00461 p[i + 4].y = xyz_05.y; 00462 p[i + 4].z = xyz_05.z; 00463 } 00464 00465 // transform to world coodinates and find axis-aligned bbx 00466 bbx_min.x() = bbx_min.y() = bbx_min.z() = 1e6; 00467 bbx_max.x() = bbx_max.y() = bbx_max.z() = -1e6; 00468 for (int i = 0; i < 8; i++) { 00469 stamped_in.point = p[i]; 00470 m_tfListener.transformPoint(m_treeFrameId, stamped_in, 00471 stamped_out); 00472 p[i].x = stamped_out.point.x; 00473 p[i].y = stamped_out.point.y; 00474 p[i].z = stamped_out.point.z; 00475 if (p[i].x < bbx_min.x()) 00476 bbx_min.x() = p[i].x; 00477 if (p[i].y < bbx_min.y()) 00478 bbx_min.y() = p[i].y; 00479 if (p[i].z < bbx_min.z()) 00480 bbx_min.z() = p[i].z; 00481 if (p[i].x > bbx_max.x()) 00482 bbx_max.x() = p[i].x; 00483 if (p[i].y > bbx_max.y()) 00484 bbx_max.y() = p[i].y; 00485 if (p[i].z > bbx_max.z()) 00486 bbx_max.z() = p[i].z; 00487 } 00488 00489 #ifdef SHOW_VISUALIZATION 00490 // std::cerr << "Publishing visualization marker publisher" << std::endl; 00491 std::string fixed_frame_(m_treeFrameId); 00492 // // visualize axis-aligned querying bbx 00493 visualization_msgs::Marker bbx; 00494 bbx.header.frame_id = fixed_frame_; 00495 bbx.header.stamp = ros::Time::now(); 00496 bbx.ns = "collider"; 00497 bbx.id = 1; 00498 bbx.action = visualization_msgs::Marker::ADD; 00499 bbx.type = visualization_msgs::Marker::CUBE; 00500 bbx.pose.orientation.w = 1.0; 00501 bbx.pose.position.x = (bbx_min.x() + bbx_max.x()) / 2.; 00502 bbx.pose.position.y = (bbx_min.y() + bbx_max.y()) / 2.; 00503 bbx.pose.position.z = (bbx_min.z() + bbx_max.z()) / 2.; 00504 bbx.scale.x = bbx_max.x()-bbx_min.x(); 00505 bbx.scale.y = bbx_max.y()-bbx_min.y(); 00506 bbx.scale.z = bbx_max.z()-bbx_min.z(); 00507 bbx.color.g = 1; 00508 bbx.color.a = 0.3; 00509 marker_pub_.publish(bbx); 00510 00511 00512 // visualize sensor cone 00513 visualization_msgs::Marker bbx_points; 00514 bbx_points.header.frame_id = fixed_frame_; 00515 bbx_points.header.stamp = ros::Time::now(); 00516 bbx_points.ns = "collider"; 00517 bbx_points.id = 2; 00518 bbx_points.action = visualization_msgs::Marker::ADD; 00519 bbx_points.type = visualization_msgs::Marker::LINE_STRIP; 00520 bbx_points.pose.orientation.w = 1.0; 00521 bbx_points.scale.x = 0.02; 00522 bbx_points.scale.y = 0.02; 00523 bbx_points.color.g = 1; 00524 bbx_points.color.a = 0.3; 00525 bbx_points.points.push_back(p[0]); 00526 bbx_points.points.push_back(p[1]); 00527 bbx_points.points.push_back(p[2]); 00528 bbx_points.points.push_back(p[3]); 00529 bbx_points.points.push_back(p[0]); 00530 bbx_points.points.push_back(p[4]); 00531 bbx_points.points.push_back(p[5]); 00532 bbx_points.points.push_back(p[6]); 00533 bbx_points.points.push_back(p[7]); 00534 bbx_points.points.push_back(p[4]); 00535 bbx_points.points.push_back(p[7]); 00536 bbx_points.points.push_back(p[3]); 00537 bbx_points.points.push_back(p[2]); 00538 bbx_points.points.push_back(p[6]); 00539 bbx_points.points.push_back(p[5]); 00540 bbx_points.points.push_back(p[1]); 00541 marker_pub_.publish(bbx_points); 00542 #endif 00543 00544 }