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();
}
