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

WIP: Volume Config #1

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

WIP: Volume Config #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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 yak_ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ find_package(Eigen3 REQUIRED)

find_package(catkin REQUIRED COMPONENTS
roscpp
eigen_conversions
pcl_ros
tf2
tf2_ros
Expand All @@ -37,6 +38,7 @@ find_package(catkin REQUIRED COMPONENTS
${cv_bridge_LIBRARIES}
CATKIN_DEPENDS
cv_bridge
eigen_conversions
geometry_msgs
pcl_ros
sensor_msgs
Expand Down
10 changes: 7 additions & 3 deletions yak_ros/include/yak_ros/utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,8 @@
#define YAK_ROS_UTILS_H
#include <yak/kfusion/kinfu.hpp>
#include <yak_ros_msgs/UpdateKinFuParamsRequest.h>

#include <eigen_conversions/eigen_msg.h>
#include <opencv2/core/eigen.hpp>
namespace yak_ros
{
/**
Expand Down Expand Up @@ -69,8 +70,11 @@ bool updateParams(kfusion::KinFuParams& params, yak_ros_msgs::UpdateKinFuParamsR
}
else if (param == request.VOLUME_POSE)
{
// I think it is a bad idea to let people change this.
return false;
Eigen::Isometry3d mid;
tf::poseMsgToEigen(request.kinfu_params.volume_pose, mid);
params.volume_pose= cv::Affine3f::Identity().translate(cv::Vec3f(mid.translation().x(), mid.translation().y(), mid.translation().z()));
// cv::eigen2cv(mid.matrix(),why);
// params.volume_pose.matrix = why;
}
else if (param == request.BILATERAL_SIGMA_DEPTH)
{
Expand Down
1 change: 1 addition & 0 deletions yak_ros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

<depend>libpcl-all-dev</depend>
<depend>eigen</depend>
<depend>eigen_conversions</depend>
<depend>yak</depend>
<depend>yak_ros_msgs</depend>

Expand Down
36 changes: 23 additions & 13 deletions yak_ros/src/yak_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,12 +50,18 @@ OnlineFusionServer::OnlineFusionServer(ros::NodeHandle& nh,
update_params_service_ = nh.advertiseService("update_params", &OnlineFusionServer::onUpdateParams, this);

// Publish the TSDF Volume
visualizer_.setBoundingBox(0,
0,
0,
params.volume_dims[0] * params.volume_resolution,
params.volume_dims[1] * params.volume_resolution,
params.volume_dims[2] * params.volume_resolution);
cv::Vec3f neg = params_.volume_pose.translation();
cv::Vec3f pos = params_.volume_pose.translation() + cv::Vec3f(params_.volume_dims[0] * params_.volume_resolution, params_.volume_dims[1] * params_.volume_resolution,params_.volume_dims[2] * params_.volume_resolution);

// Publish the TSDF Volume
visualizer_.setBoundingBox(neg[0],
neg[1],
neg[2],
pos[0],
pos[1],
pos[2]);

fusion_.resetWithNewParams(params);
}

void OnlineFusionServer::onReceivedPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_in)
Expand Down Expand Up @@ -242,13 +248,17 @@ bool OnlineFusionServer::onUpdateParams(yak_ros_msgs::UpdateKinFuParamsRequest&
}
else
{
cv::Vec3f neg = params_.volume_pose.translation();
cv::Vec3f pos = params_.volume_pose.translation() + cv::Vec3f(params_.volume_dims[0] * params_.volume_resolution, params_.volume_dims[1] * params_.volume_resolution,params_.volume_dims[2] * params_.volume_resolution);

// Publish the TSDF Volume
visualizer_.setBoundingBox(0,
0,
0,
params_.volume_dims[0] * params_.volume_resolution,
params_.volume_dims[1] * params_.volume_resolution,
params_.volume_dims[2] * params_.volume_resolution);
visualizer_.setBoundingBox(neg[0],
neg[1],
neg[2],
pos[0],
pos[1],
pos[2]);

ROS_INFO_STREAM("TSDF volume has been reset and " << req.params_to_update.size()
<< " parameters have been updated.");
res.success = true;
Expand All @@ -273,7 +283,7 @@ bool OnlineFusionServer::transformPolygonMesh(pcl::PolygonMesh& input_mesh, cons
ros::Time stamp;
pcl_conversions::fromPCL(input_mesh.header.stamp, stamp);
auto tsdf_to_target_tf = tf_buffer_.lookupTransform(target_frame, input_mesh.header.frame_id, stamp);
Eigen::Affine3d tsdf_to_target = tf2::transformToEigen(tsdf_to_target_tf);
Eigen::Affine3d tsdf_to_target = tf2::transformToEigen(tsdf_to_target_tf).translate(Eigen::Vector3d(params_.volume_pose.translation()[0],params_.volume_pose.translation()[1],params_.volume_pose.translation()[2]));

// Tranform the point cloud to the correct frame
pcl::PointCloud<pcl::PointXYZ> cloud;
Expand Down
1 change: 1 addition & 0 deletions yak_ros_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(yak_ros_msgs)

find_package(catkin REQUIRED COMPONENTS
eigen_conversions
geometry_msgs
message_generation
std_msgs
Expand Down
1 change: 1 addition & 0 deletions yak_ros_msgs/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

<depend>roscpp</depend>

<depend>eigen_conversions</depend>
<build_depend>geometry_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>

Expand Down