Merge pull request #44 from rsoussan/soundsee_depth_tool

commit 0a1a8d6
Merge: 5be6863 e19ab71
Author: Ryan Soussan <ryan.soussan@gmail.com>
Date:   Mon Apr 4 13:38:34 2022 -0700

    Merge pull request #44 from rsoussan/soundsee_depth_tool

    * Added soundsee depth tool
    * fixed td mesh writing
    * fixed depth print out
    * updated timestamp error
    * updated save depth data fcn
    * updated load sensor rays fcn
    * added output o abbreviation
    * loading multiple cams
    * updated test depth utils
    * added test at angle
    * added utils file for depth mesh utils
    * added output filename
    * added saving depth data
    * added file loading
    * added getdepth data fcn
    * added make groundtruth pose interplator fcn

dense_map/geometry_mapper/CMakeLists.txt

@@ -39,6 +39,7 @@ find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   image_transport
   config_reader
+  localization_common
 )
 
 # The quantity CMAKE_PREFIX_PATH can be set as an environmental
@@ -117,7 +118,7 @@ set(TEXRECON_INCLUDE_DIRS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES geometry_mapper
-  CATKIN_DEPENDS roscpp std_msgs nodelet camera interest_point sparse_mapping ff_util isaac_hw_msgs eigen_conversions rosbag cv_bridge image_transport config_reader
+  CATKIN_DEPENDS roscpp std_msgs nodelet camera interest_point sparse_mapping ff_util isaac_hw_msgs eigen_conversions rosbag cv_bridge image_transport config_reader localization_common
 # DEPENDS system_lib
 )
 
@@ -141,6 +142,7 @@ add_definitions(${PCL_DEFINITIONS})
 
 # Declare a C++ library
 add_library(geometry_mapper_lib
+    src/depth_from_mesh_utils.cc
     src/dense_map_utils.cc
     src/dense_map_ros_utils.cc
     src/TinyEXIF.cc
@@ -180,6 +182,10 @@ add_executable(camera_refiner_old tools/camera_refiner_old.cc)
 target_link_libraries(camera_refiner_old
     geometry_mapper_lib gflags glog)
 
+add_executable(get_depth_data_from_mesh tools/get_depth_data_from_mesh.cc)
+target_link_libraries(get_depth_data_from_mesh
+    geometry_mapper_lib gflags glog)
+
 add_executable(extract_pc_from_bag tools/extract_pc_from_bag.cc)
 target_link_libraries(extract_pc_from_bag
     geometry_mapper_lib gflags glog)
@@ -220,6 +226,18 @@ add_executable(streaming_mapper tools/streaming_mapper.cc)
 target_link_libraries(streaming_mapper
     geometry_mapper_lib gflags glog)
 
+if(CATKIN_ENABLE_TESTING)
+ find_package(rostest REQUIRED)
+  add_rostest_gtest(test_depth_from_mesh_utils
+    test/test_depth_from_mesh_utils.test
+    test/test_depth_from_mesh_utils.cc
+  )
+  target_link_libraries(test_depth_from_mesh_utils
+    ${PROJECT_NAME}_lib glog ${catkin_LIBRARIES} 
+  )
+
+endif()
+
 #############
 ## Install ##
 #############

dense_map/geometry_mapper/include/depth_from_mesh_utils.h

@@ -0,0 +1,73 @@
+/* Copyright (c) 2017, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ *
+ * All rights reserved.
+ *
+ * The Astrobee platform is licensed under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef DEPTH_FROM_MESH_UTILS_H_
+#define DEPTH_FROM_MESH_UTILS_H_
+
+#include <localization_common/pose_interpolater.h>
+#include <localization_common/time.h>
+
+#include <acc/bvh_tree.h>
+#include <mve/mesh.h>
+#include <math/vector.h>
+
+#include <Eigen/Geometry>
+
+#include <boost/optional.hpp>
+
+#include <string>
+#include <vector>
+
+namespace dense_map {
+typedef acc::BVHTree<unsigned int, math::Vec3f> BVHTree;
+
+boost::optional<double> Depth(const Eigen::Vector3d& sensor_t_ray, const Eigen::Isometry3d& world_T_sensor,
+                              const BVHTree& bvh_tree, double min_depth = 1e-3, double max_depth = 1e2);
+
+localization_common::Time LoadTimestamp(const std::string& filename, const double timestamp_offset = 0.0);
+
+void LoadTimestampsAndPoses(const std::string& directory, const std::string& sensor_name,
+                            std::vector<localization_common::Time>& timestamps, std::vector<Eigen::Isometry3d>& poses,
+                            const Eigen::Isometry3d& poses_sensor_T_sensor = Eigen::Isometry3d::Identity(),
+                            const double timestamp_offset = 0.0);
+
+// Assumes each timestamp is on a newline of the file
+std::vector<localization_common::Time> LoadTimestamps(const std::string& timestamps_filename);
+
+localization_common::PoseInterpolater MakePoseInterpolater(
+  const std::string& directory, const Eigen::Isometry3d& body_T_sensor,
+  const std::vector<std::string>& groundtruth_sensor_names,
+  const std::vector<Eigen::Isometry3d>& body_T_groundtruth_sensor_vec, const std::vector<double>& timestamp_offsets);
+
+// Assumes each point is sampled from an y, z grid with an x offset of 1.0
+// Points are entered as "y z" values with new points on a newline in the file
+std::vector<Eigen::Vector3d> LoadSensorRays(const std::string& sensor_rays_filename, int& rows);
+
+std::shared_ptr<BVHTree> LoadMeshTree(const std::string& mesh_file);
+
+std::vector<boost::optional<double>> GetDepthData(
+  const std::vector<localization_common::Time>& timestamps, const std::vector<Eigen::Vector3d>& sensor_t_rays,
+  const localization_common::PoseInterpolater& groundtruth_pose_interpolater, const BVHTree& bvh_tree);
+
+void SaveDepthData(const std::vector<localization_common::Time>& timestamps,
+                   const std::vector<Eigen::Vector3d>& sensor_t_rays,
+                   const std::vector<boost::optional<double>>& depths, const std::string& output_filename,
+                   const int rows);
+}  // namespace dense_map
+
+#endif  // DEPTH_FROM_MESH_UTILS_H_

dense_map/geometry_mapper/include/texture_processing.h

@@ -159,7 +159,7 @@ inline IsaacTexturePatch::IsaacTexturePatch(int64_t label, std::vector<std::size
                                             int64_t width, int64_t height)
     : label(label), faces(faces), texcoords(texcoords), width(width), height(height) {}
 
-IsaacTexturePatch::IsaacTexturePatch(IsaacTexturePatch const& texture_patch) {
+inline IsaacTexturePatch::IsaacTexturePatch(IsaacTexturePatch const& texture_patch) {
   label = texture_patch.label;
   faces = std::vector<std::size_t>(texture_patch.faces);
   texcoords = std::vector<Eigen::Vector2d>(texture_patch.texcoords);

dense_map/geometry_mapper/package.xml

@@ -29,6 +29,7 @@
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>config_reader</build_depend>
+  <build_depend>localization_common</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
@@ -43,7 +44,7 @@
   <build_export_depend>rosbag</build_export_depend>
   <build_export_depend>cv_bridge</build_export_depend>
   <build_export_depend>image_transport</build_export_depend>
-  <build_export_depend>config_reader</build_export_depend>
+  <build_export_depend>localization_common</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
@@ -59,6 +60,7 @@
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>image_transport</exec_depend>
   <exec_depend>config_reader</exec_depend>
+  <exec_depend>localization_common</exec_depend>
 
   <export>
 <!--

dense_map/geometry_mapper/src/depth_from_mesh_utils.cc

@@ -0,0 +1,183 @@
+/* Copyright (c) 2017, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ *
+ * All rights reserved.
+ *
+ * The Astrobee platform is licensed under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <dense_map_utils.h>
+#include <depth_from_mesh_utils.h>
+#include <localization_common/utilities.h>
+#include <texture_processing.h>
+
+#include <mve/mesh.h>
+
+#include <boost/algorithm/string/predicate.hpp>
+#include <boost/filesystem.hpp>
+
+#include <glog/logging.h>
+
+namespace dm = dense_map;
+namespace fs = boost::filesystem;
+namespace lc = localization_common;
+
+namespace dense_map {
+
+boost::optional<double> Depth(const Eigen::Vector3d& sensor_t_ray, const Eigen::Isometry3d& world_T_sensor,
+                              const BVHTree& bvh_tree, double min_depth, double max_depth) {
+  const Eigen::Vector3d world_F_sensor_t_ray = world_T_sensor.linear() * sensor_t_ray;
+  // Create ray centered at world_t_sensor
+  BVHTree::Ray bvh_ray;
+  bvh_ray.origin = dm::eigen_to_vec3f(world_T_sensor.translation());
+  bvh_ray.dir = dm::eigen_to_vec3f(world_F_sensor_t_ray);
+  bvh_ray.dir.normalize();
+  bvh_ray.tmin = min_depth;
+  bvh_ray.tmax = max_depth;
+
+  // Get intersection if it exists
+  BVHTree::Hit hit;
+  if (bvh_tree.intersect(bvh_ray, &hit))
+    return hit.t;
+  else
+    VLOG(2) << "Failed to get mesh intersection.";
+  return boost::none;
+}
+
+lc::Time LoadTimestamp(const std::string& filename, const double timestamp_offset) {
+  const std::string timestamp_string = filename.substr(0, filename.find("_"));
+  if (timestamp_string.empty()) LOG(FATAL) << "Failed to load timestamp from filename.";
+  return std::stod(timestamp_string) + timestamp_offset;
+}
+
+void LoadTimestampsAndPoses(const std::string& directory, const std::string& sensor_name,
+                            std::vector<lc::Time>& timestamps, std::vector<Eigen::Isometry3d>& poses,
+                            const Eigen::Isometry3d& poses_sensor_T_sensor, const double timestamp_offset) {
+  for (const auto& file : fs::recursive_directory_iterator(directory)) {
+    const std::string filename = file.path().filename().string();
+    if (boost::algorithm::ends_with(filename, "world.txt") && filename.find(sensor_name) != std::string::npos) {
+      timestamps.emplace_back(LoadTimestamp(filename, timestamp_offset));
+      Eigen::Affine3d affine_pose;
+      if (!dm::readAffine(affine_pose, file.path().string())) {
+        LOG(FATAL) << "Failed to read pose for filename " << filename;
+      }
+      const Eigen::Isometry3d world_T_poses_sensor(affine_pose.matrix());
+      poses.push_back(world_T_poses_sensor * poses_sensor_T_sensor);
+    }
+  }
+}
+
+// Assumes each timestamp is on a newline of the file
+std::vector<lc::Time> LoadTimestamps(const std::string& timestamps_filename) {
+  std::vector<lc::Time> timestamps;
+  std::ifstream timestamps_file(timestamps_filename);
+  std::string file_line;
+  lc::Time timestamp;
+  while (std::getline(timestamps_file, file_line)) {
+    std::istringstream line_ss(file_line);
+    line_ss >> timestamp;
+    timestamps.emplace_back(timestamp);
+  }
+  return timestamps;
+}
+
+lc::PoseInterpolater MakePoseInterpolater(const std::string& directory, const Eigen::Isometry3d& body_T_sensor,
+                                          const std::vector<std::string>& groundtruth_sensor_names,
+                                          const std::vector<Eigen::Isometry3d>& body_T_groundtruth_sensor_vec,
+                                          const std::vector<double>& timestamp_offsets) {
+  std::vector<lc::Time> timestamps;
+  std::vector<Eigen::Isometry3d> poses;
+  for (int i = 0; i < groundtruth_sensor_names.size(); ++i) {
+    LoadTimestampsAndPoses(directory, groundtruth_sensor_names[i], timestamps, poses,
+                           (body_T_groundtruth_sensor_vec[i]).inverse() * body_T_sensor, timestamp_offsets[i]);
+  }
+  return lc::PoseInterpolater(timestamps, poses);
+}
+
+std::vector<Eigen::Vector3d> LoadSensorRays(const std::string& sensor_rays_filename, int& rows) {
+  std::vector<Eigen::Vector3d> sensor_t_rays;
+  std::ifstream sensor_rays_file(sensor_rays_filename);
+  std::string file_line;
+  double y, z;
+  int line_index = 0;
+  while (std::getline(sensor_rays_file, file_line)) {
+    std::istringstream line_ss(file_line);
+    if (line_index == 0) {
+      line_ss >> rows;
+    } else {
+      line_ss >> y;
+      line_ss >> z;
+      const Eigen::Vector3d sensor_t_ray(1.0, y, z);
+      sensor_t_rays.emplace_back(sensor_t_ray.normalized());
+    }
+    ++line_index;
+  }
+  return sensor_t_rays;
+}
+
+std::shared_ptr<BVHTree> LoadMeshTree(const std::string& mesh_file) {
+  mve::TriangleMesh::Ptr mesh;
+  std::shared_ptr<mve::MeshInfo> mesh_info;
+  std::shared_ptr<tex::Graph> graph;
+  std::shared_ptr<BVHTree> bvh_tree;
+  dm::loadMeshBuildTree(mesh_file, mesh, mesh_info, graph, bvh_tree);
+  return bvh_tree;
+}
+
+std::vector<boost::optional<double>> GetDepthData(const std::vector<lc::Time>& timestamps,
+                                                  const std::vector<Eigen::Vector3d>& sensor_t_rays,
+                                                  const lc::PoseInterpolater& groundtruth_pose_interpolater,
+                                                  const BVHTree& bvh_tree) {
+  std::vector<boost::optional<double>> depths;
+  for (const auto& timestamp : timestamps) {
+    const auto world_T_sensor = groundtruth_pose_interpolater.Interpolate(timestamp);
+    if (!world_T_sensor) {
+      LOG(ERROR) << "Failed to get groundtruth pose at timstamp " << std::setprecision(20) << timestamp;
+      depths.emplace_back(boost::none);
+      continue;
+    }
+    for (const auto& sensor_t_ray : sensor_t_rays) {
+      depths.emplace_back(Depth(sensor_t_ray, *world_T_sensor, bvh_tree));
+    }
+  }
+  return depths;
+}
+
+void SaveDepthData(const std::vector<lc::Time>& timestamps, const std::vector<Eigen::Vector3d>& sensor_t_rays,
+                   const std::vector<boost::optional<double>>& depths, const std::string& output_filename,
+                   const int rows) {
+  std::ofstream output_file;
+  output_file.open(output_filename);
+  for (const auto timestamp : timestamps) {
+    output_file << std::setprecision(20) << timestamp << " ";
+  }
+  output_file << std::endl;
+  const int cols = depths.size() / (timestamps.size() * rows);
+  output_file << cols << std::endl;
+  const int depths_per_row = depths.size() / timestamps.size();
+  int index = 0;
+  for (const auto& depth : depths) {
+    if (!depth) {
+      output_file << -1 << " ";
+    } else {
+      output_file << *depth << " ";
+    }
+    ++index;
+    if (index >= depths_per_row) {
+      output_file << std::endl;
+      index = 0;
+    }
+  }
+  output_file.close();
+}
+}  // namespace dense_map

dense_map/geometry_mapper/test/data/1234.22_nav_cam_world.txt

@@ -0,0 +1,4 @@
+1 0 0 0 
+0 1 0 0 
+0 0 1 0
+0 0 0 1

dense_map/geometry_mapper/test/data/22222.1119_nav_cam_world.txt

@@ -0,0 +1,4 @@
+1 0 0 1 
+0 1 0 2.22 
+0 0 1 -3.33
+0 0 0 1

dense_map/geometry_mapper/test/data/313131.99_nav_cam_world.txt

@@ -0,0 +1,4 @@
+0.5 -0.5 0.70710678118654746 -1.1
+0.85355339059327373 0.14644660940672627 -0.5 -0.22
+0.14644660940672627 0.85355339059327373 0.5 300.3
+0 0 0 1

dense_map/geometry_mapper/test/data/sensor_rays.csv

@@ -0,0 +1,3 @@
+2
+1.23 4.56
+7.888999 99999.111222

dense_map/geometry_mapper/test/data/timestamps.csv

@@ -0,0 +1,3 @@
+1.11
+2.222
+333.3333