diff --git a/dense_map/geometry_mapper/CMakeLists.txt b/dense_map/geometry_mapper/CMakeLists.txt index 18c12e73..efa8ab43 100644 --- a/dense_map/geometry_mapper/CMakeLists.txt +++ b/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 ## ############# diff --git a/dense_map/geometry_mapper/include/depth_from_mesh_utils.h b/dense_map/geometry_mapper/include/depth_from_mesh_utils.h new file mode 100644 index 00000000..7bbd318d --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace dense_map { +typedef acc::BVHTree BVHTree; + +boost::optional 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& timestamps, std::vector& 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 LoadTimestamps(const std::string& timestamps_filename); + +localization_common::PoseInterpolater MakePoseInterpolater( + const std::string& directory, const Eigen::Isometry3d& body_T_sensor, + const std::vector& groundtruth_sensor_names, + const std::vector& body_T_groundtruth_sensor_vec, const std::vector& 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 LoadSensorRays(const std::string& sensor_rays_filename, int& rows); + +std::shared_ptr LoadMeshTree(const std::string& mesh_file); + +std::vector> GetDepthData( + const std::vector& timestamps, const std::vector& sensor_t_rays, + const localization_common::PoseInterpolater& groundtruth_pose_interpolater, const BVHTree& bvh_tree); + +void SaveDepthData(const std::vector& timestamps, + const std::vector& sensor_t_rays, + const std::vector>& depths, const std::string& output_filename, + const int rows); +} // namespace dense_map + +#endif // DEPTH_FROM_MESH_UTILS_H_ diff --git a/dense_map/geometry_mapper/include/texture_processing.h b/dense_map/geometry_mapper/include/texture_processing.h index 4bf09af0..4bd0fa62 100644 --- a/dense_map/geometry_mapper/include/texture_processing.h +++ b/dense_map/geometry_mapper/include/texture_processing.h @@ -159,7 +159,7 @@ inline IsaacTexturePatch::IsaacTexturePatch(int64_t label, std::vector(texture_patch.faces); texcoords = std::vector(texture_patch.texcoords); diff --git a/dense_map/geometry_mapper/package.xml b/dense_map/geometry_mapper/package.xml index 6708fb43..0c0d97ed 100644 --- a/dense_map/geometry_mapper/package.xml +++ b/dense_map/geometry_mapper/package.xml @@ -29,6 +29,7 @@ cv_bridge image_transport config_reader + localization_common roscpp std_msgs sensor_msgs @@ -43,7 +44,7 @@ rosbag cv_bridge image_transport - config_reader + localization_common roscpp std_msgs sensor_msgs @@ -59,6 +60,7 @@ cv_bridge image_transport config_reader + localization_common + + + + + + + + + + + + + + + + + + + + + diff --git a/dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc b/dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc new file mode 100644 index 00000000..d7b5f2d8 --- /dev/null +++ b/dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc @@ -0,0 +1,136 @@ +/* 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 +#include +#include +#include + +#include +#include + +#include + +#include + +namespace dm = dense_map; +namespace fs = boost::filesystem; +namespace po = boost::program_options; +namespace lc = localization_common; +namespace mc = msg_conversions; + +int main(int argc, char** argv) { + std::string robot_config_file; + std::string world; + std::string sensor_frame; + std::string output_file; + po::options_description desc( + "Adds depth data to desired points at provided timestamps. Uses a provided mesh and sequence of groundtruth poses " + "to obtain the depth data."); + desc.add_options()("help,h", "produce help message")( + "timestamps-file", po::value()->required(), + "File containing a set of timestamps to generate depth data for. Each timestamp should be on a newline.")( + "sensor-rays-file", po::value()->required(), + "File containing a set of rays from the sensor frame to generate depth data for. Each point has an assumed x " + "offset of 1 meter from the sensor, so only y z pairs are required. Each point pair should be on a newline.")( + "groundtruth-directory", po::value()->required(), + "Directory containing groundtruth poses with timestamps as filenames.")( + "mesh", po::value()->required(), "Mesh used to provide depth data.")( + "config-path,c", po::value()->required(), "Path to astrobee config directory.")( + "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( + "output-file,o", po::value(&output_file)->default_value("depths.csv"), + "Output file containing timestamp, depth, x, y values on each line. Timestamps for which depth data was not " + "successfully loaded are not included in the output.") + // TODO(rsoussan): Add soundsee sensor trafo to astrobee configs! + ("sensor-frame,s", po::value(&sensor_frame)->default_value("soundsee"), + "Sensor frame to generate depth data in."); + + po::positional_options_description p; + p.add("timestamps-file", 1); + p.add("sensor-rays-file", 1); + p.add("groundtruth-directory", 1); + p.add("mesh", 1); + p.add("config-path", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + if (vm.count("help") || (argc <= 1)) { + std::cout << desc << "\n"; + return 1; + } + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + const std::string timestamps_file = vm["timestamps-file"].as(); + const std::string sensor_rays_file = vm["sensor-rays-file"].as(); + const std::string groundtruth_directory = vm["groundtruth-directory"].as(); + const std::string mesh_file = vm["mesh"].as(); + const std::string config_path = vm["config-path"].as(); + + // Only pass program name to free flyer so that boost command line options + // are ignored when parsing gflags. + int ff_argc = 1; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv); + + if (!fs::exists(timestamps_file)) { + LOG(FATAL) << "Timestamps file " << timestamps_file << " not found."; + } + if (!fs::exists(sensor_rays_file)) { + LOG(FATAL) << "Sensor rays file " << sensor_rays_file << " not found."; + } + if (!fs::exists(groundtruth_directory)) { + LOG(FATAL) << "Groundtruth directory " << groundtruth_directory << " not found."; + } + if (!fs::exists(mesh_file)) { + LOG(FATAL) << "Mesh " << mesh_file << " not found."; + } + + lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + config_reader::ConfigReader config; + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LOG(FATAL) << "Failed to read config files."; + } + + // Needed later to write results in correct format + int mesh_rows; + const auto sensor_t_rays = dm::LoadSensorRays(sensor_rays_file, mesh_rows); + const auto query_timestamps = dm::LoadTimestamps(timestamps_file); + const auto body_T_sensor = mc::LoadEigenTransform(config, sensor_frame + "_transform"); + std::vector groundtruth_sensor_frames{"nav_cam", "sci_cam", "haz_cam"}; + // TODO(rsoussan): Add option to pass these as args? + std::vector timestamp_offsets{0, 0, 0}; + std::vector body_T_groundtruth_sensor_vec; + for (const auto& sensor_frame : groundtruth_sensor_frames) { + body_T_groundtruth_sensor_vec.emplace_back(mc::LoadEigenTransform(config, sensor_frame + "_transform")); + } + const auto groundtruth_pose_interpolater = dm::MakePoseInterpolater( + groundtruth_directory, body_T_sensor, groundtruth_sensor_frames, body_T_groundtruth_sensor_vec, timestamp_offsets); + const auto mesh_tree = dm::LoadMeshTree(mesh_file); + const auto depths = dm::GetDepthData(query_timestamps, sensor_t_rays, groundtruth_pose_interpolater, *mesh_tree); + int depth_count = 0; + for (const auto& depth : depths) { + if (depth) ++depth_count; + } + LOG(INFO) << "Got " << depth_count << " of " << depths.size() << " depths successfully."; + dm::SaveDepthData(query_timestamps, sensor_t_rays, depths, output_file, mesh_rows); +}