From 2e54c1c3ece306f04eb68960b423a0ddd0c72d16 Mon Sep 17 00:00:00 2001
From: Colin Lewis <clewis@swri.org>
Date: Tue, 17 Nov 2020 22:19:09 -0600
Subject: [PATCH] initial commit; translation

---
 yak_ros/CMakeLists.txt          |  2 ++
 yak_ros/include/yak_ros/utils.h | 10 ++++++---
 yak_ros/package.xml             |  1 +
 yak_ros/src/yak_node.cpp        | 36 +++++++++++++++++++++------------
 yak_ros_msgs/CMakeLists.txt     |  1 +
 yak_ros_msgs/package.xml        |  1 +
 6 files changed, 35 insertions(+), 16 deletions(-)

diff --git a/yak_ros/CMakeLists.txt b/yak_ros/CMakeLists.txt
index b80bace..b6a4a29 100644
--- a/yak_ros/CMakeLists.txt
+++ b/yak_ros/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(Eigen3 REQUIRED)
 
 find_package(catkin REQUIRED COMPONENTS
      roscpp
+     eigen_conversions
      pcl_ros
      tf2
      tf2_ros
@@ -37,6 +38,7 @@ find_package(catkin REQUIRED COMPONENTS
      ${cv_bridge_LIBRARIES}
    CATKIN_DEPENDS
      cv_bridge
+     eigen_conversions
      geometry_msgs
      pcl_ros
      sensor_msgs
diff --git a/yak_ros/include/yak_ros/utils.h b/yak_ros/include/yak_ros/utils.h
index 1dcb3cb..9f45b4f 100644
--- a/yak_ros/include/yak_ros/utils.h
+++ b/yak_ros/include/yak_ros/utils.h
@@ -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
 {
 /**
@@ -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)
     {
diff --git a/yak_ros/package.xml b/yak_ros/package.xml
index 5beb760..51cace7 100644
--- a/yak_ros/package.xml
+++ b/yak_ros/package.xml
@@ -14,6 +14,7 @@
 
   <depend>libpcl-all-dev</depend>
   <depend>eigen</depend>
+  <depend>eigen_conversions</depend>
   <depend>yak</depend>
   <depend>yak_ros_msgs</depend>
 
diff --git a/yak_ros/src/yak_node.cpp b/yak_ros/src/yak_node.cpp
index 1337458..63c0378 100644
--- a/yak_ros/src/yak_node.cpp
+++ b/yak_ros/src/yak_node.cpp
@@ -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)
@@ -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;
@@ -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;
diff --git a/yak_ros_msgs/CMakeLists.txt b/yak_ros_msgs/CMakeLists.txt
index 54ae9ea..8b28ad5 100644
--- a/yak_ros_msgs/CMakeLists.txt
+++ b/yak_ros_msgs/CMakeLists.txt
@@ -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
diff --git a/yak_ros_msgs/package.xml b/yak_ros_msgs/package.xml
index e247648..2b866f2 100644
--- a/yak_ros_msgs/package.xml
+++ b/yak_ros_msgs/package.xml
@@ -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>