Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

WIP #2

Open
wants to merge 3 commits into
base: feature/temporal_window
Choose a base branch
from
Open

WIP #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
53b71bd
add service
alessandromorra Apr 16, 2021
8faca84
Add freespace point cloud deintetration to voxblox.
alessandromorra Jul 19, 2021
c8e2043
Clean up logging
alessandromorra Jul 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,11 @@ class ProjectiveTsdfIntegrator : public voxblox::TsdfIntegratorBase {
void updateTsdfBlocks(const Transformation& T_G_C,
const Eigen::MatrixXf& range_image,
const voxblox::IndexSet& touched_block_indices,
const bool freespace_points = false,
const bool deintegrate = false);
inline void updateTsdfVoxel(const Eigen::MatrixXf& range_image,
const Point& t_C_voxel, TsdfVoxel* tsdf_voxel,
const bool freespace_points = false,
const bool deintegrate = false);

template <typename T>
Expand Down
32 changes: 24 additions & 8 deletions voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,11 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::integratePointCloud(
const Colors & /*colors*/, const bool freespace_points,
const bool deintegrate) {
// Freespace points are not yet supported
CHECK(!freespace_points) << "Freespace points are not yet supported for the "
"Projective TSDF Integrator";
LOG_IF_EVERY_N(WARNING, freespace_points, 100)
<< "Freespace points are not yet supported "
"for the Projective TSDF Integrator ";
LOG_IF(ERROR, deintegrate && freespace_points)
<< "deintegrating freespace_points # " << points_C.size();

// Construct the range image and select the blocks it affects
voxblox::IndexSet touched_block_indices;
Expand All @@ -58,7 +61,8 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::integratePointCloud(
// Process all blocks
timing::Timer integration_timer("integration_timer");
if (config_.integrator_threads == 1) {
updateTsdfBlocks(T_G_C, range_image_, touched_block_indices, deintegrate);
updateTsdfBlocks(T_G_C, range_image_, touched_block_indices,
freespace_points, deintegrate);
} else {
std::vector<voxblox::IndexSet> block_index_subsets(
config_.integrator_threads);
Expand All @@ -72,7 +76,7 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::integratePointCloud(
for (size_t i = 0; i < config_.integrator_threads; ++i) {
integration_threads.emplace_back(
&ProjectiveTsdfIntegrator::updateTsdfBlocks, this, T_G_C,
range_image_, block_index_subsets[i], deintegrate);
range_image_, block_index_subsets[i], freespace_points, deintegrate);
}
for (std::thread &integration_thread : integration_threads) {
integration_thread.join();
Expand Down Expand Up @@ -189,7 +193,8 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::parsePointcloud(
template <InterpolationScheme interpolation_scheme>
void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfBlocks(
const Transformation &T_G_C, const Eigen::MatrixXf &range_image,
const voxblox::IndexSet &touched_block_indices, const bool deintegrate) {
const voxblox::IndexSet &touched_block_indices, const bool freespace_points,
const bool deintegrate) {
for (const voxblox::BlockIndex &block_index : touched_block_indices) {
voxblox::Block<TsdfVoxel>::Ptr block_ptr =
layer_->getBlockPtrByIndex(block_index);
Expand All @@ -201,15 +206,17 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfBlocks(
const Point t_C_voxel =
T_G_C.inverse() *
block_ptr->computeCoordinatesFromLinearIndex(linear_index);
updateTsdfVoxel(range_image, t_C_voxel, tsdf_voxel, deintegrate);
updateTsdfVoxel(range_image, t_C_voxel, tsdf_voxel, freespace_points,
deintegrate);
}
}
}

template <InterpolationScheme interpolation_scheme>
void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
const Eigen::MatrixXf &range_image, const Point &t_C_voxel,
TsdfVoxel *tsdf_voxel, const bool deintegrate) {
TsdfVoxel *tsdf_voxel, const bool freespace_points,
const bool deintegrate) {
// Skip voxels that are too far or too close
const float distance_to_voxel = t_C_voxel.norm();
if (distance_to_voxel < config_.min_ray_length_m ||
Expand All @@ -227,12 +234,21 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
const float distance_to_surface = interpolate(range_image, h, w);
const float sdf = distance_to_surface - distance_to_voxel;

// TODO: add to config, this parameter should represen the max thickness of
// something.
constexpr float sdf_threshold_m = 0.2;
// It point is closer to sdf than threshold, set to zero.
if (freespace_points && std::abs(sdf) < sdf_threshold_m) {
tsdf_voxel->distance = 0.0f;
tsdf_voxel->weight = 0.0f;
return;
}
// Approximate how many rays would have updated the voxel
// NOTE: We do this to reflect that we are more certain about
// the updates applied to nearer voxels
const float num_rays_intersecting_voxel =
ray_intersections_per_distance_squared_ /
std::max(distance_to_voxel * distance_to_voxel, 1.f);
std::max(distance_to_voxel * distance_to_voxel, 1.f);

// Skip voxels that fall outside the TSDF truncation distance
if (sdf < -config_.default_truncation_distance) {
Expand Down
11 changes: 11 additions & 0 deletions voxblox_ros/include/voxblox_ros/tsdf_server.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,11 @@ class TsdfServer {
std_srvs::Empty::Response& response); // NOLINT
bool publishTsdfMapCallback(std_srvs::Empty::Request& request, // NOLINT
std_srvs::Empty::Response& response); // NOLINT
bool startVoxbloxCallback(std_srvs::Empty::Request& request, // NOLINT
std_srvs::Empty::Response& response); // NOLINT
bool stopVoxbloxCallback(std_srvs::Empty::Request& request, // NOLINT
std_srvs::Empty::Response& response); // NOLINT
void startSubscribers();

void updateMeshEvent(const ros::TimerEvent& event);
void publishMapEvent(const ros::TimerEvent& event);
Expand Down Expand Up @@ -170,6 +175,8 @@ class TsdfServer {
ros::ServiceServer load_map_srv_;
ros::ServiceServer publish_pointclouds_srv_;
ros::ServiceServer publish_tsdf_map_srv_;
ros::ServiceServer start_voxblox_srv_;
ros::ServiceServer stop_voxblox_srv_;

/// Tools for broadcasting TFs.
tf::TransformBroadcaster tf_broadcaster_;
Expand All @@ -180,6 +187,10 @@ class TsdfServer {

bool verbose_;

bool voxblox_is_running_ = true; // Default is on.

bool wait_for_service_to_start_ = false;

/**
* Global/map coordinate frame. Will always look up TF transforms to this
* frame.
Expand Down
45 changes: 43 additions & 2 deletions voxblox_ros/src/tsdf_server.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ TsdfServer::TsdfServer(const ros::NodeHandle& nh,
: nh_(nh),
nh_private_(nh_private),
verbose_(true),
wait_for_service_to_start_(false),
world_frame_("world"),
icp_corrected_frame_("icp_corrected"),
pose_corrected_frame_("pose_corrected"),
Expand Down Expand Up @@ -68,8 +69,10 @@ TsdfServer::TsdfServer(const ros::NodeHandle& nh,

nh_private_.param("pointcloud_queue_size", pointcloud_queue_size_,
pointcloud_queue_size_);
pointcloud_sub_ = nh_.subscribe("pointcloud", pointcloud_queue_size_,
&TsdfServer::insertPointcloud, this);
if (!wait_for_service_to_start_) {
pointcloud_sub_ = nh_.subscribe("pointcloud", pointcloud_queue_size_,
&TsdfServer::insertPointcloud, this);
}

mesh_pub_ = nh_private_.advertise<voxblox_msgs::Mesh>("mesh", 1, true);

Expand Down Expand Up @@ -110,6 +113,7 @@ TsdfServer::TsdfServer(const ros::NodeHandle& nh,
nh_private_.param("method", method, method);
tsdf_integrator_ = TsdfIntegratorFactory::create(
method, integrator_config, tsdf_map_->getTsdfLayerPtr());
LOG(ERROR) << integrator_config.print();

mesh_layer_.reset(new MeshLayer(tsdf_map_->block_size()));

Expand All @@ -131,6 +135,10 @@ TsdfServer::TsdfServer(const ros::NodeHandle& nh,
"publish_pointclouds", &TsdfServer::publishPointcloudsCallback, this);
publish_tsdf_map_srv_ = nh_private_.advertiseService(
"publish_map", &TsdfServer::publishTsdfMapCallback, this);
start_voxblox_srv_ = nh_private_.advertiseService(
"start_voxblox", &TsdfServer::startVoxbloxCallback, this);
stop_voxblox_srv_ = nh_private_.advertiseService(
"stop_voxblox", &TsdfServer::stopVoxbloxCallback, this);

// If set, use a timer to progressively integrate the mesh.
double update_mesh_every_n_sec = 1.0;
Expand Down Expand Up @@ -186,6 +194,8 @@ void TsdfServer::getServerConfigFromRosParam(
accumulate_icp_corrections_);

nh_private.param("verbose", verbose_, verbose_);
nh_private.param("wait_for_service_to_start", wait_for_service_to_start_,
wait_for_service_to_start_);

// Pointcloud deintegration settings
{
Expand Down Expand Up @@ -405,6 +415,11 @@ bool TsdfServer::getNextPointcloudFromQueue(

void TsdfServer::insertPointcloud(
const sensor_msgs::PointCloud2::Ptr& pointcloud_msg_in) {
if (!voxblox_is_running_) {
LOG_EVERY_N(ERROR, 100)
<< "Voxblox is stopped. Start with ROS service .../start_voxblox";
return;
}
if (pointcloud_msg_in->header.stamp - last_msg_time_ptcloud_ >
min_time_between_msgs_) {
last_msg_time_ptcloud_ = pointcloud_msg_in->header.stamp;
Expand Down Expand Up @@ -601,6 +616,7 @@ void TsdfServer::publishMap(bool reset_remote_map) {
if (!publish_tsdf_map_) {
return;
}

int subscribers = this->tsdf_map_pub_.getNumSubscribers();
if (subscribers > 0) {
if (num_subscribers_tsdf_map_ < subscribers) {
Expand All @@ -617,9 +633,13 @@ void TsdfServer::publishMap(bool reset_remote_map) {
if (reset_remote_map) {
layer_msg.action = static_cast<uint8_t>(MapDerializationAction::kReset);
}

LOG(ERROR) << this->tsdf_map_->getTsdfLayer().voxel_size();

if (publish_map_with_trajectory_) {
voxblox_msgs::LayerWithTrajectory layer_with_trajectory_msg;
layer_with_trajectory_msg.layer = layer_msg;

for (const PointcloudDeintegrationPacket& pointcloud_queue_packet :
pointcloud_deintegration_queue_) {
geometry_msgs::PoseStamped pose_msg;
Expand Down Expand Up @@ -780,6 +800,27 @@ bool TsdfServer::publishTsdfMapCallback(std_srvs::Empty::Request& /*request*/,
return true;
}

bool TsdfServer::startVoxbloxCallback(std_srvs::Empty::Request& /*request*/,
std_srvs::Empty::Response&
/*response*/) { // NOLINT
startSubscribers();
voxblox_is_running_ = true;
return true;
}

bool TsdfServer::stopVoxbloxCallback(std_srvs::Empty::Request& /*request*/,
std_srvs::Empty::Response&
/*response*/) { // NOLINT
LOG(INFO) << "Stopping voxblox call";
voxblox_is_running_ = false;
return true;
}

void TsdfServer::startSubscribers() {
pointcloud_sub_ = nh_.subscribe("pointcloud", pointcloud_queue_size_,
&TsdfServer::insertPointcloud, this);
}

void TsdfServer::updateMeshEvent(const ros::TimerEvent& /*event*/) {
updateMesh();
}
Expand Down