void KeyframeMapper::buildOctomap(octomap::OcTree& tree) { ROS_INFO("Building Octomap..."); octomap::point3d sensor_origin(0.0, 0.0, 0.0); for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) { ROS_INFO("Processing keyframe %u", kf_idx); const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; PointCloudT cloud; keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_); octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose)); // build octomap cloud from pcl cloud octomap::Pointcloud octomap_cloud; for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx) { const PointT& p = cloud.points[pt_idx]; if (!std::isnan(p.z)) octomap_cloud.push_back(p.x, p.y, p.z); } tree.insertScan(octomap_cloud, sensor_origin, frame_origin); } }
void KeyframeMapper::buildColorOctomap(octomap::ColorOcTree& tree) { ROS_INFO("Building Octomap with color..."); octomap::point3d sensor_origin(0.0, 0.0, 0.0); for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) { ROS_INFO("Processing keyframe %u", kf_idx); const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; // construct the cloud PointCloudT::Ptr cloud_unf(new PointCloudT()); keyframe.constructDensePointCloud(*cloud_unf, max_range_, max_stdev_); // perform filtering for max z pcl::transformPointCloud(*cloud_unf, *cloud_unf, keyframe.pose); PointCloudT cloud; pcl::PassThrough<PointT> pass; pass.setInputCloud (cloud_unf); pass.setFilterFieldName ("z"); pass.setFilterLimits (-std::numeric_limits<double>::infinity(), max_map_z_); pass.filter(cloud); pcl::transformPointCloud(cloud, cloud, keyframe.pose.inverse()); octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose)); // build octomap cloud from pcl cloud octomap::Pointcloud octomap_cloud; for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx) { const PointT& p = cloud.points[pt_idx]; if (!std::isnan(p.z)) octomap_cloud.push_back(p.x, p.y, p.z); } // insert scan (only xyz considered, no colors) tree.insertScan(octomap_cloud, sensor_origin, frame_origin); // insert colors PointCloudT cloud_tf; pcl::transformPointCloud(cloud, cloud_tf, keyframe.pose); for (unsigned int pt_idx = 0; pt_idx < cloud_tf.points.size(); ++pt_idx) { const PointT& p = cloud_tf.points[pt_idx]; if (!std::isnan(p.z)) { octomap::point3d endpoint(p.x, p.y, p.z); octomap::ColorOcTreeNode* n = tree.search(endpoint); if (n) n->setColor(p.r, p.g, p.b); } } tree.updateInnerOccupancy(); } }
void keypoint_map::get_octree(octomap::OcTree & tree) { tree.clear(); for (size_t i = 0; i < depth_imgs.size(); i++) { octomap::Pointcloud octomap_cloud; octomap::point3d sensor_origin(0.0, 0.0, 0.0); Eigen::Vector3f trans(camera_positions[i].translation()); Eigen::Quaternionf rot(camera_positions[i].rotation()); octomap::pose6d frame_origin( octomath::Vector3(trans.x(), trans.y(), trans.z()), octomath::Quaternion(rot.w(), rot.x(), rot.y(), rot.z())); for (int v = 0; v < depth_imgs[i].rows; v++) { for (int u = 0; u < depth_imgs[i].cols; u++) { if (depth_imgs[i].at<unsigned short>(v, u) != 0) { pcl::PointXYZ p; p.z = depth_imgs[i].at<unsigned short>(v, u) / 1000.0f; p.x = (u - intrinsics[2]) * p.z / intrinsics[0]; p.y = (v - intrinsics[3]) * p.z / intrinsics[1]; octomap_cloud.push_back(p.x, p.y, p.z); } } } tree.insertScan(octomap_cloud, sensor_origin, frame_origin); } tree.updateInnerOccupancy(); }
void KeyframeLiveMapper::buildColorOctomap(octomap::ColorOcTree& tree,int current_keyframes_size) { int i; octomap::point3d sensor_origin(0,0,0); for(i = 0; i < current_keyframes_size; i++) { const rgbdtools::RGBDKeyframe& keyframe = _keyframes[i]; octomap::pose6d frame_origin; octomap::Pointcloud octomap_cloud; PointCloudXYZRGB::Ptr cloud_unf(new PointCloudXYZRGB()); PointCloudXYZRGB cloud; keyframe.constructDensePointCloud(*cloud_unf, _max_range,_max_stddev); // perform filtering for max z pcl::transformPointCloud(*cloud_unf,*cloud_unf,keyframe.pose); _pass.setInputCloud(cloud_unf); _pass.filter(cloud); pcl::transformPointCloud(cloud,cloud,keyframe.pose.inverse()); // build octomap cloud from pcl cloud frame_origin = octomap::poseTfToOctomap(ccny_rgbd::tfFromEigenAffine(keyframe.pose)); // insert points buildOctoCloud(octomap_cloud,cloud); tree.insertScan(octomap_cloud, sensor_origin, frame_origin); // insert colors insertColor(tree,cloud,keyframe.pose); tree.updateInnerOccupancy(); } }
void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg) { ROS_DEBUG("Received a new point cloud message"); ros::WallTime start = ros::WallTime::now(); if (monitor_->getMapFrame().empty()) monitor_->setMapFrame(cloud_msg->header.frame_id); /* get transform for cloud into map frame */ tf::StampedTransform map_H_sensor; if (monitor_->getMapFrame() == cloud_msg->header.frame_id) map_H_sensor.setIdentity(); else { if (tf_) { try { tf_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp, map_H_sensor); } catch (tf::TransformException& ex) { ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback"); return; } } else return; } /* convert cloud message to pcl cloud object */ pcl::PointCloud<pcl::PointXYZ> cloud; pcl::fromROSMsg(*cloud_msg, cloud); /* compute sensor origin in map frame */ const tf::Vector3 &sensor_origin_tf = map_H_sensor.getOrigin(); octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ()); Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ()); if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp)) ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail."); /* mask out points on the robot */ shape_mask_->maskContainment(cloud, sensor_origin_eigen, 0.0, max_range_, mask_); octomap::KeySet free_cells, occupied_cells, model_cells; tree_->lockRead(); try { /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates * should be occupied */ for (unsigned int row = 0; row < cloud.height; row += point_subsample_) { unsigned int row_c = row * cloud.width; for (unsigned int col = 0; col < cloud.width; col += point_subsample_) { if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP) continue; const pcl::PointXYZ &p = cloud(col, row); /* check for NaN */ if ((p.x == p.x) && (p.y == p.y) && (p.z == p.z)) { /* transform to map frame */ tf::Vector3 point_tf = map_H_sensor * tf::Vector3(p.x, p.y, p.z); /* occupied cell at ray endpoint if ray is shorter than max range and this point isn't on a part of the robot*/ if (mask_[row_c + col] == point_containment_filter::ShapeMask::INSIDE) model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); else occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } } } /* compute the free cells along each ray that ends at an occupied cell */ for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_)) free_cells.insert(key_ray_.begin(), key_ray_.end()); /* compute the free cells along each ray that ends at a model cell */ for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it) if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_)) free_cells.insert(key_ray_.begin(), key_ray_.end()); } catch (...) { tree_->unlockRead(); return; } tree_->unlockRead(); /* cells that overlap with the model are not occupied */ for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it) occupied_cells.erase(*it); /* occupied cells are not free */ for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) free_cells.erase(*it); tree_->lockWrite(); try { /* mark free cells only if not seen occupied in this cloud */ for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it) tree_->updateNode(*it, false); /* now mark all occupied cells */ for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) tree_->updateNode(*it, true); // set the logodds to the minimum for the cells that are part of the model const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog(); for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it) tree_->updateNode(*it, lg); } catch (...) { ROS_ERROR("Internal error while updating octree"); } tree_->unlockWrite(); ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); tree_->triggerUpdateCallback(); }
void keypoint_map::extract_surface() { octomap::ColorOcTree tree(0.05f); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr point_cloud( new pcl::PointCloud<pcl::PointXYZRGBA>); for (size_t i = 0; i < depth_imgs.size(); i++) { cv::imwrite("rgb/" + boost::lexical_cast<std::string>(i) + ".png", rgb_imgs[i]); cv::imwrite("depth/" + boost::lexical_cast<std::string>(i) + ".png", depth_imgs[i]); octomap::Pointcloud octomap_cloud; octomap::point3d sensor_origin(0.0, 0.0, 0.0); Eigen::Vector3f trans(camera_positions[i].translation()); Eigen::Quaternionf rot(camera_positions[i].rotation()); octomap::pose6d frame_origin( octomath::Vector3(trans.x(), trans.y(), trans.z()), octomath::Quaternion(rot.w(), rot.x(), rot.y(), rot.z())); //std::cerr << camera_positions[i].matrix() << std::endl; for (int v = 0; v < depth_imgs[i].rows; v++) { for (int u = 0; u < depth_imgs[i].cols; u++) { if (depth_imgs[i].at<unsigned short>(v, u) != 0) { pcl::PointXYZRGBA p; p.z = depth_imgs[i].at<unsigned short>(v, u) / 1000.0f; p.x = (u - intrinsics[2]) * p.z / intrinsics[0]; p.y = (v - intrinsics[3]) * p.z / intrinsics[1]; cv::Vec3b brg = rgb_imgs[i].at<cv::Vec3b>(v, u); p.r = brg[2]; p.g = brg[1]; p.b = brg[0]; p.a = 255; Eigen::Vector4f tmp = camera_positions[i] * p.getVector4fMap(); if (tmp[2] < 2.0) { octomap_cloud.push_back(p.x, p.y, p.z); p.getVector4fMap() = tmp; octomap::point3d endpoint(p.x, p.y, p.z); octomap::ColorOcTreeNode* n = tree.search(endpoint); if (n) { n->setColor(p.r, p.g, p.b); } //ROS_INFO("Point %f %f %f from %f %f ", p.x, p.y, p.z, keypoints[i].pt.x, keypoints[i].pt.y); point_cloud->push_back(p); } } } } tree.insertScan(octomap_cloud, sensor_origin, frame_origin); tree.updateInnerOccupancy(); } tree.write("room.ot"); pcl::io::savePCDFileASCII("room.pcd", *point_cloud); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr point_cloud_filtered( new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::VoxelGrid<pcl::PointXYZRGBA> sor; sor.setInputCloud(point_cloud); sor.setLeafSize(0.05f, 0.05f, 0.05f); sor.filter(*point_cloud_filtered); pcl::io::savePCDFileASCII("room_sub.pcd", *point_cloud_filtered); }
void DepthImageOccupancyMapUpdater::depthImageCallback(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { ROS_DEBUG("Received a new depth image message"); ros::WallTime start = ros::WallTime::now(); if (monitor_->getMapFrame().empty()) monitor_->setMapFrame(depth_msg->header.frame_id); /* get transform for cloud into map frame */ tf::StampedTransform map_H_sensor; if (monitor_->getMapFrame() == depth_msg->header.frame_id) map_H_sensor.setIdentity(); else { if (tf_) { try { tf_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, depth_msg->header.stamp, map_H_sensor); } catch (tf::TransformException& ex) { ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback"); return; } } else return; } if (!updateTransformCache(depth_msg->header.frame_id, depth_msg->header.stamp)) ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail."); if (depth_msg->is_bigendian && !HOST_IS_BIG_ENDIAN) ROS_ERROR_THROTTLE(1, "endian problem: received image data does not match host"); const int w = depth_msg->width; const int h = depth_msg->height; // call the mesh filter mesh_filter::StereoCameraModel::Parameters& params = mesh_filter_->parameters(); params.setCameraParameters (info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]); params.setImageSize(w, h); const bool is_u_short = depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1; if (is_u_short) mesh_filter_->filter(&depth_msg->data[0], GL_UNSIGNED_SHORT); else mesh_filter_->filter(&depth_msg->data[0], GL_FLOAT); // Use correct principal point from calibration const double px = info_msg->K[2]; const double py = info_msg->K[5]; const double inv_fx = 1.0 / info_msg->K[0]; const double inv_fy = 1.0 / info_msg->K[4]; // Pre-compute some constants if (x_cache_.size() < w) x_cache_.resize(w); if (y_cache_.size() < h) y_cache_.resize(h); for (int x = 0; x < w; ++x) x_cache_[x] = (x - px) * inv_fx; for (int y = 0; y < h; ++y) y_cache_[y] = (y - py) * inv_fy; octomap::point3d sensor_origin(map_H_sensor.getOrigin().getX(), map_H_sensor.getOrigin().getY(), map_H_sensor.getOrigin().getZ()); OccMapTreePtr tree = monitor_->getOcTreePtr(); octomap::KeySet free_cells, occupied_cells, model_cells; // allocate memory if needed std::size_t img_size = h * w; if (filtered_data_.size() < img_size) filtered_data_.resize(img_size); const float* filtered_row = reinterpret_cast<const float*>(&filtered_data_[0]); mesh_filter_->getFilteredDepth(&filtered_data_[0]); if (debug_info_) { sensor_msgs::Image debug_msg; debug_msg.header = depth_msg->header; debug_msg.height = depth_msg->height; debug_msg.width = depth_msg->width; debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; debug_msg.is_bigendian = depth_msg->is_bigendian; debug_msg.step = depth_msg->step; debug_msg.data.resize(img_size * sizeof(float)); mesh_filter_->getModelDepth(reinterpret_cast<float*>(&debug_msg.data[0])); pub_model_depth_image_.publish(debug_msg, *info_msg); memcpy(&debug_msg.data[0], &filtered_data_[0], sizeof(float) * img_size); pub_filtered_depth_image_.publish(debug_msg, *info_msg); } tree->lockRead(); try { const int h_bound = h - skip_vertical_pixels_; const int w_bound = w - skip_horizontal_pixels_; if (is_u_short) { const uint16_t *input_row = reinterpret_cast<const uint16_t*>(&depth_msg->data[0]); for (int y = skip_vertical_pixels_ ; y < h_bound ; ++y, filtered_row += w, input_row += w) if (y_cache_[y] == y_cache_[y]) // if not NaN for (int x = skip_horizontal_pixels_ ; x < w_bound ; ++x) { float zz = filtered_row[x]; uint16_t zz0 = input_row[x]; if (zz0 != 0) { if (zz == 0.0) { float zz1 = (float)zz0 * 1e-3; // scale from mm to m float yy = y_cache_[y] * zz1; float xx = x_cache_[x] * zz1; /* transform to map frame */ tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz1); // add to the list of model cells model_cells.insert(tree->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } else { if (zz == zz) { float yy = y_cache_[y] * zz; float xx = x_cache_[x] * zz; /* transform to map frame */ tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz); // add to the list of occupied cells occupied_cells.insert(tree->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } } } } } else { const float *input_row = reinterpret_cast<const float*>(&depth_msg->data[0]); for (int y = skip_vertical_pixels_ ; y < h_bound ; ++y, filtered_row += w, input_row += w) if (y_cache_[y] == y_cache_[y]) // if not NaN for (int x = skip_horizontal_pixels_ ; x < w_bound ; ++x) { float zz = filtered_row[x]; float zz0 = input_row[x]; if (zz0 == zz0 && zz == zz) // check for NaN { float yy = y_cache_[y] * zz0; float xx = x_cache_[x] * zz0; if (xx == xx) { /* transform to map frame */ tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz0); if (zz == 0.0) // add to the list of model cells model_cells.insert(tree->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); else // add to the list of occupied cells occupied_cells.insert(tree->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } } } } /* compute the free cells along each ray that ends at an occupied cell */ for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) if (tree->computeRayKeys(sensor_origin, tree->keyToCoord(*it), key_ray_)) free_cells.insert(key_ray_.begin(), key_ray_.end()); /* compute the free cells along each ray that ends at a model cell */ for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it) if (tree->computeRayKeys(sensor_origin, tree->keyToCoord(*it), key_ray_)) free_cells.insert(key_ray_.begin(), key_ray_.end()); } catch (...) { tree->unlockRead(); return; } tree->unlockRead(); /* cells that overlap with the model are not occupied */ for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it) occupied_cells.erase(*it); /* occupied cells are not free */ for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) free_cells.erase(*it); tree->lockWrite(); try { /* mark free cells only if not seen occupied in this cloud */ for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it) tree->updateNode(*it, false); /* now mark all occupied cells */ for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) tree->updateNode(*it, true); // set the logodds to the minimum for the cells that are part of the model const float lg = tree->getClampingThresMinLog() - tree->getClampingThresMaxLog(); for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it) tree->updateNode(*it, lg); } catch (...) { ROS_ERROR("Internal error while updating octree"); } tree->unlockWrite(); ROS_DEBUG("Processed depth image in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); triggerUpdateCallback(); }