├── voxgraph_msgs ├── msg │ ├── LoopClosure.msg │ ├── LoopClosureEdgeList.msg │ ├── LoopClosureEdge.msg │ └── MapSurface.msg ├── CMakeLists.txt └── package.xml ├── arche_ssh.rosinstall ├── arche_https.rosinstall ├── voxgraph_ssh.rosinstall ├── voxgraph_https.rosinstall ├── .gitignore ├── voxgraph ├── launch │ ├── registration_test_bench.launch │ ├── arche_demo.launch │ └── voxgraph_mapper.launch ├── include │ └── voxgraph │ │ ├── common.h │ │ ├── tools │ │ ├── rosbag_helper.h │ │ ├── tf_helper.h │ │ ├── visualization │ │ │ ├── pose_graph_visuals.h │ │ │ ├── cost_function_visuals.h │ │ │ ├── loop_closure_visuals.h │ │ │ └── submap_visuals.h │ │ ├── io.h │ │ ├── threading_helper.h │ │ ├── ros_params.h │ │ ├── submap_registration_helper.h │ │ ├── data_servers │ │ │ ├── projected_map_server.h │ │ │ ├── loop_closure_edge_server.h │ │ │ └── submap_server.h │ │ ├── odometry_simulator │ │ │ ├── normal_distribution.h │ │ │ └── odometry_simulator.h │ │ └── evaluation │ │ │ └── map_evaluation.h │ │ ├── frontend │ │ ├── submap_collection │ │ │ ├── registration_point.h │ │ │ ├── bounding_box.h │ │ │ ├── submap_timeline.h │ │ │ ├── weighted_sampler.h │ │ │ ├── weighted_sampler_inl.h │ │ │ ├── voxgraph_submap_collection.h │ │ │ └── voxgraph_submap.h │ │ ├── measurement_processors │ │ │ ├── gps_processor.h │ │ │ └── pointcloud_integrator.h │ │ ├── pose_graph_interface │ │ │ ├── node_templates.h │ │ │ ├── measurement_templates.h │ │ │ └── pose_graph_interface.h │ │ ├── map_tracker │ │ │ ├── transformers │ │ │ │ ├── tf_transformer.h │ │ │ │ └── odometry_transformer.h │ │ │ └── map_tracker.h │ │ ├── frame_names.h │ │ └── voxgraph_mapper.h │ │ └── backend │ │ ├── local_parameterization │ │ ├── normalize_angle.h │ │ └── angle_local_parameterization.h │ │ ├── node │ │ ├── pose │ │ │ ├── pose.h │ │ │ ├── pose_6d.h │ │ │ └── pose_4d.h │ │ ├── submap_node.h │ │ ├── reference_frame_node.h │ │ ├── node.h │ │ └── node_collection.h │ │ ├── constraint │ │ ├── absolute_pose_constraint.h │ │ ├── relative_pose_constraint.h │ │ ├── constraint.h │ │ ├── constraint_collection.h │ │ ├── registration_constraint.h │ │ └── cost_functions │ │ │ ├── relative_pose_cost_function.h │ │ │ ├── registration_cost_function.h │ │ │ └── relative_pose_cost_function_inl.h │ │ └── pose_graph.h ├── src │ ├── tools │ │ ├── rosbag_helper.cpp │ │ ├── tf_helper.cpp │ │ ├── visualization │ │ │ ├── pose_graph_visuals.cpp │ │ │ ├── loop_closure_visuals.cpp │ │ │ ├── cost_function_visuals.cpp │ │ │ └── submap_visuals.cpp │ │ ├── data_servers │ │ │ ├── projected_map_server.cpp │ │ │ └── loop_closure_edge_server.cpp │ │ ├── submap_registration_helper.cpp │ │ └── evaluation │ │ │ └── map_evaluation.cpp │ ├── voxgraph_mapping_node.cpp │ ├── odometry_simulator_node.cpp │ ├── backend │ │ ├── node │ │ │ ├── pose │ │ │ │ ├── pose_6d.cpp │ │ │ │ └── pose_4d.cpp │ │ │ ├── node.cpp │ │ │ └── node_collection.cpp │ │ └── constraint │ │ │ ├── constraint_collection.cpp │ │ │ ├── absolute_pose_constraint.cpp │ │ │ ├── relative_pose_constraint.cpp │ │ │ ├── registration_constraint.cpp │ │ │ └── constraint.cpp │ └── frontend │ │ ├── pose_graph_interface │ │ ├── node_templates.cpp │ │ └── measurement_templates.cpp │ │ ├── map_tracker │ │ ├── transformers │ │ │ ├── tf_transformer.cpp │ │ │ └── odometry_transformer.cpp │ │ └── map_tracker.cpp │ │ ├── submap_collection │ │ ├── submap_timeline.cpp │ │ ├── bounding_box.cpp │ │ └── voxgraph_submap_collection.cpp │ │ └── measurement_processors │ │ └── pointcloud_integrator.cpp ├── package.xml ├── config │ ├── odometry_simulator.yaml │ ├── registration_test_bench.yaml │ ├── voxgraph_mapper.yaml │ └── voxgraph_mapper.rviz ├── CMakeLists.txt └── scripts │ └── jacobians_xyz_yaw.py ├── LICENSE └── .clang-format /voxgraph_msgs/msg/LoopClosure.msg: -------------------------------------------------------------------------------- 1 | time from_timestamp 2 | time to_timestamp 3 | geometry_msgs/Transform transform 4 | float64[36] information -------------------------------------------------------------------------------- /voxgraph_msgs/msg/LoopClosureEdgeList.msg: -------------------------------------------------------------------------------- 1 | # Message header 2 | std_msgs/Header header 3 | 4 | # List of loop closure edges 5 | voxgraph_msgs/LoopClosureEdge[] loop_closure_edges -------------------------------------------------------------------------------- /arche_ssh.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ouster_lidar 3 | uri: git@github.com:ethz-asl/ouster_lidar.git 4 | - git: 5 | local-name: lidar_undistortion 6 | uri: git@github.com:ethz-asl/lidar_undistortion.git 7 | -------------------------------------------------------------------------------- /arche_https.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ouster_lidar 3 | uri: https://github.com/ethz-asl/ouster_lidar.git 4 | - git: 5 | local-name: lidar_undistortion 6 | uri: https://github.com/ethz-asl/lidar_undistortion.git 7 | -------------------------------------------------------------------------------- /voxgraph_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxgraph_msgs) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ########## 8 | # EXPORT # 9 | ########## 10 | cs_install() 11 | cs_export() 12 | -------------------------------------------------------------------------------- /voxgraph_msgs/msg/LoopClosureEdge.msg: -------------------------------------------------------------------------------- 1 | # Timestamps of both vertices 2 | time timestamp_A 3 | time timestamp_B 4 | 5 | # Relative pose and covariance of the edge 6 | # NOTE: These are expressed as the pose of vertex B in the frame of vertex A 7 | geometry_msgs/PoseWithCovariance T_A_B -------------------------------------------------------------------------------- /voxgraph_ssh.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ceres_catkin 3 | uri: git@github.com:ethz-asl/ceres_catkin.git 4 | - git: 5 | local-name: suitesparse 6 | uri: git@github.com:ethz-asl/suitesparse.git 7 | - git: 8 | local-name: cblox 9 | uri: git@github.com:ethz-asl/cblox.git 10 | -------------------------------------------------------------------------------- /voxgraph_https.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: ceres_catkin 3 | uri: https://github.com/ethz-asl/ceres_catkin.git 4 | - git: 5 | local-name: suitesparse 6 | uri: https://github.com/ethz-asl/suitesparse.git 7 | - git: 8 | local-name: cblox 9 | uri: https://github.com/ethz-asl/cblox.git 10 | -------------------------------------------------------------------------------- /voxgraph_msgs/msg/MapSurface.msg: -------------------------------------------------------------------------------- 1 | # Message header 2 | Header header 3 | 4 | # Map header encapsulating the map's main properties 5 | cblox_msgs/MapHeader map_header 6 | 7 | # Pointcloud of surface vertices 8 | # NOTE: It is expressed in the frame specified by header.frame_id 9 | sensor_msgs/PointCloud2 pointcloud 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /voxgraph/launch/registration_test_bench.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/common.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_COMMON_H_ 2 | #define VOXGRAPH_COMMON_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace voxgraph { 9 | using Transformation = voxblox::Transformation; 10 | using SubmapID = cblox::SubmapID; 11 | using SubmapIdPair = std::pair; 12 | using TransformationD = kindr::minimal::QuatTransformationTemplate; 13 | using BiasVectorType = TransformationD::Vector3; 14 | } // namespace voxgraph 15 | 16 | #endif // VOXGRAPH_COMMON_H_ 17 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/rosbag_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_ROSBAG_HELPER_H_ 2 | #define VOXGRAPH_TOOLS_ROSBAG_HELPER_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace voxgraph { 8 | class RosbagHelper { 9 | public: 10 | explicit RosbagHelper(ros::NodeHandle node_handle); 11 | 12 | void pauseRosbag(); 13 | void playRosbag(); 14 | 15 | private: 16 | ros::ServiceClient rosbag_pause_srv_; 17 | std_srvs::SetBool srv_msg_; 18 | }; 19 | } // namespace voxgraph 20 | 21 | #endif // VOXGRAPH_TOOLS_ROSBAG_HELPER_H_ 22 | -------------------------------------------------------------------------------- /voxgraph/src/tools/rosbag_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/rosbag_helper.h" 2 | 3 | namespace voxgraph { 4 | RosbagHelper::RosbagHelper(ros::NodeHandle node_handle) { 5 | rosbag_pause_srv_ = 6 | node_handle.serviceClient("/player/pause_playback"); 7 | } 8 | 9 | void RosbagHelper::pauseRosbag() { 10 | srv_msg_.request.data = true; 11 | rosbag_pause_srv_.call(srv_msg_); 12 | } 13 | 14 | void RosbagHelper::playRosbag() { 15 | srv_msg_.request.data = false; 16 | rosbag_pause_srv_.call(srv_msg_); 17 | } 18 | } // namespace voxgraph 19 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/registration_point.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_REGISTRATION_POINT_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_REGISTRATION_POINT_H_ 3 | 4 | namespace voxgraph { 5 | // Small struct that can be used to store voxels or points on the isosurface 6 | struct RegistrationPoint { 7 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 8 | 9 | voxblox::Point position; 10 | float distance; 11 | float weight; 12 | }; 13 | } // namespace voxgraph 14 | 15 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_REGISTRATION_POINT_H_ 16 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/measurement_processors/gps_processor.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_MEASUREMENT_PROCESSORS_GPS_PROCESSOR_H_ 2 | #define VOXGRAPH_FRONTEND_MEASUREMENT_PROCESSORS_GPS_PROCESSOR_H_ 3 | 4 | namespace voxgraph { 5 | // TODO(victorr): Implement this class, which would be responsible for 6 | // converting NavSatFix msgs to absolute pose measurements 7 | // that can be added to the pose graph. 8 | class GpsProcessor { 9 | public: 10 | private: 11 | }; 12 | } // namespace voxgraph 13 | 14 | #endif // VOXGRAPH_FRONTEND_MEASUREMENT_PROCESSORS_GPS_PROCESSOR_H_ 15 | -------------------------------------------------------------------------------- /voxgraph/src/voxgraph_mapping_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "voxgraph/frontend/voxgraph_mapper.h" 5 | 6 | int main(int argc, char** argv) { 7 | // Start logging 8 | google::InitGoogleLogging(argv[0]); 9 | google::InstallFailureSignalHandler(); 10 | 11 | // Register with ROS master 12 | ros::init(argc, argv, "voxgraph_mapping"); 13 | 14 | // Create node handles 15 | ros::NodeHandle nh; 16 | ros::NodeHandle nh_private("~"); 17 | 18 | // Create the mapper 19 | voxgraph::VoxgraphMapper voxgraph_mapper(nh, nh_private); 20 | 21 | // Spin 22 | ros::spin(); 23 | 24 | // Exit normally 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /voxgraph/launch/arche_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /voxgraph/src/odometry_simulator_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "voxgraph/tools/odometry_simulator/odometry_simulator.h" 5 | 6 | int main(int argc, char** argv) { 7 | // Start logging 8 | google::InitGoogleLogging(argv[0]); 9 | google::InstallFailureSignalHandler(); 10 | 11 | // Register with ROS master 12 | ros::init(argc, argv, "voxgraph_odometry_simulator"); 13 | 14 | // Create node handles 15 | ros::NodeHandle nh; 16 | ros::NodeHandle nh_private("~"); 17 | 18 | // Create the mapper 19 | voxgraph::OdometrySimulator odometry_simulator(nh, nh_private); 20 | 21 | // Spin 22 | ros::spin(); 23 | 24 | // Exit normally 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/tf_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_TF_HELPER_H_ 2 | #define VOXGRAPH_TOOLS_TF_HELPER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace voxgraph { 10 | class TfHelper { 11 | public: 12 | static void publishTransform(const voxblox::Transformation& transform, 13 | const std::string& base_frame, 14 | const std::string& target_frame, 15 | bool tf_is_static = false, 16 | const ros::Time& timestamp = ros::Time::now()); 17 | }; 18 | } // namespace voxgraph 19 | 20 | #endif // VOXGRAPH_TOOLS_TF_HELPER_H_ 21 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/visualization/pose_graph_visuals.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_VISUALIZATION_POSE_GRAPH_VISUALS_H_ 2 | #define VOXGRAPH_TOOLS_VISUALIZATION_POSE_GRAPH_VISUALS_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/backend/pose_graph.h" 7 | 8 | namespace voxgraph { 9 | class PoseGraphVisuals { 10 | public: 11 | PoseGraphVisuals() = default; 12 | 13 | void publishPoseGraph(const PoseGraph& pose_graph, 14 | const std::string& frame_id, 15 | const std::string& name_space, 16 | const ros::Publisher& publisher) const; 17 | }; 18 | } // namespace voxgraph 19 | 20 | #endif // VOXGRAPH_TOOLS_VISUALIZATION_POSE_GRAPH_VISUALS_H_ 21 | -------------------------------------------------------------------------------- /voxgraph_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxgraph_msgs 4 | 1.0.0 5 | Message definitions for voxgraph ROS interface 6 | 7 | Victor Reijgwart 8 | 9 | Victor Reijgwart 10 | 11 | ASL 2.0 12 | 13 | catkin 14 | catkin_simple 15 | 16 | message_generation 17 | message_runtime 18 | 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | voxblox_msgs 23 | cblox_msgs 24 | 25 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/local_parameterization/normalize_angle.h: -------------------------------------------------------------------------------- 1 | // NOTE: This function comes directly from the ceres::examples 2 | 3 | #ifndef VOXGRAPH_BACKEND_LOCAL_PARAMETERIZATION_NORMALIZE_ANGLE_H_ 4 | #define VOXGRAPH_BACKEND_LOCAL_PARAMETERIZATION_NORMALIZE_ANGLE_H_ 5 | 6 | #include 7 | 8 | namespace voxgraph { 9 | // Normalize the angle to stay within [-pi and pi) radians 10 | template 11 | inline T NormalizeAngle(const T& angle_radians) { 12 | // Use ceres::floor because it is specialized for double and Jet types. 13 | T two_pi(2.0 * M_PI); 14 | return angle_radians - 15 | two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi); 16 | } 17 | } // namespace voxgraph 18 | 19 | #endif // VOXGRAPH_BACKEND_LOCAL_PARAMETERIZATION_NORMALIZE_ANGLE_H_ 20 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/pose/pose.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_POSE_POSE_H_ 2 | #define VOXGRAPH_BACKEND_NODE_POSE_POSE_H_ 3 | 4 | #include "voxgraph/common.h" 5 | 6 | namespace voxgraph { 7 | class Pose { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | typedef double ValueType; 12 | 13 | explicit Pose(const Transformation& initial_pose) { 14 | initial_pose_vec_ = initial_pose.log(); 15 | } 16 | 17 | ~Pose() = default; 18 | 19 | virtual operator Transformation() const = 0; // NOLINT 20 | 21 | virtual ValueType* optimizationVectorData() = 0; 22 | virtual size_t optimizationVectorSize() const = 0; 23 | 24 | protected: 25 | Transformation::Vector6 initial_pose_vec_; 26 | }; 27 | } // namespace voxgraph 28 | 29 | #endif // VOXGRAPH_BACKEND_NODE_POSE_POSE_H_ 30 | -------------------------------------------------------------------------------- /voxgraph/src/backend/node/pose/pose_6d.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/node/pose/pose_6d.h" 2 | 3 | namespace voxgraph { 4 | Pose6D::Pose6D(const Transformation& initial_pose) : Pose(initial_pose) { 5 | // Initialize the optimization variables 6 | // TODO(victorr): Replace this with the appropriate std method 7 | for (int i = 0; i < 6; i++) { 8 | r3_so3_vector_[i] = initial_pose_vec_[i]; 9 | } 10 | } 11 | 12 | Pose6D::operator Transformation() const { 13 | // Convert the optimization array to a minkindr compatible type 14 | Transformation::Vector6 T_vec_6d; 15 | // TODO(victorr): Replace this with the appropriate std method 16 | for (int i = 0; i < 6; i++) { 17 | T_vec_6d[i] = r3_so3_vector_[i]; 18 | } 19 | 20 | return Transformation::exp(T_vec_6d); 21 | } 22 | } // namespace voxgraph 23 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/submap_node.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_SUBMAP_NODE_H_ 2 | #define VOXGRAPH_BACKEND_NODE_SUBMAP_NODE_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "voxgraph/backend/node/node.h" 9 | 10 | namespace voxgraph { 11 | class SubmapNode : public Node { 12 | public: 13 | typedef std::shared_ptr Ptr; 14 | typedef cblox::SubmapID SubmapId; 15 | 16 | struct Config : Node::Config { 17 | SubmapId submap_id; 18 | }; 19 | 20 | SubmapNode(const NodeId& node_id, const Config& config) 21 | : Node(node_id, config), config_(config) {} 22 | 23 | cblox::SubmapID getSubmapId() const { return config_.submap_id; } 24 | 25 | private: 26 | Config config_; 27 | }; 28 | } // namespace voxgraph 29 | 30 | #endif // VOXGRAPH_BACKEND_NODE_SUBMAP_NODE_H_ 31 | -------------------------------------------------------------------------------- /voxgraph/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | voxgraph 4 | 1.0.0 5 | Pose graph optimization of voxblox submaps 6 | 7 | Victor Reijgwart 8 | 9 | Victor Reijgwart 10 | 11 | ASL 2.0 12 | 13 | catkin 14 | catkin_simple 15 | 16 | roscpp 17 | voxblox 18 | voxblox_ros 19 | cblox 20 | cblox_ros 21 | ceres_catkin 22 | eigen_catkin 23 | 24 | voxblox_msgs 25 | cblox_msgs 26 | voxgraph_msgs 27 | 28 | 29 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/reference_frame_node.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_REFERENCE_FRAME_NODE_H_ 2 | #define VOXGRAPH_BACKEND_NODE_REFERENCE_FRAME_NODE_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/backend/node/node.h" 7 | 8 | namespace voxgraph { 9 | class ReferenceFrameNode : public Node { 10 | public: 11 | typedef std::shared_ptr Ptr; 12 | typedef unsigned int FrameId; 13 | 14 | struct Config : Node::Config { 15 | FrameId reference_frame_id; 16 | }; 17 | 18 | ReferenceFrameNode(const NodeId& node_id, const Config& config) 19 | : Node(node_id, config), config_(config) {} 20 | 21 | FrameId getReferenceFrameId() const { return config_.reference_frame_id; } 22 | 23 | private: 24 | Config config_; 25 | }; 26 | } // namespace voxgraph 27 | 28 | #endif // VOXGRAPH_BACKEND_NODE_REFERENCE_FRAME_NODE_H_ 29 | -------------------------------------------------------------------------------- /voxgraph/config/odometry_simulator.yaml: -------------------------------------------------------------------------------- 1 | debug: true 2 | subscriber_queue_length: 100 3 | subscribe_to_odom_topic: "/firefly/ground_truth/odometry" 4 | published_mission_frame: "odom" 5 | published_simulated_base_frame: "base" 6 | published_original_base_frame: "base_ground_truth" 7 | 8 | odometry_noise: 9 | velocity: 10 | x: 11 | mean: 0.05 12 | stddev: 0.01 13 | y: 14 | mean: 0.05 15 | stddev: 0.01 16 | z: 17 | mean: 0.0001 18 | stddev: 0.0001 19 | yaw: 20 | mean: 0.005 21 | stddev: 0.00001 22 | position: 23 | x: 24 | mean: 0 25 | stddev: 0 26 | y: 27 | mean: 0 28 | stddev: 0 29 | z: 30 | mean: 0 31 | stddev: 0 32 | yaw: 33 | mean: 0 34 | stddev: 0 35 | pitch: 36 | mean: 0 37 | stddev: 0 38 | roll: 39 | mean: 0 40 | stddev: 0 41 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/bounding_box.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_BOUNDING_BOX_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_BOUNDING_BOX_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace voxgraph { 8 | typedef Eigen::Matrix BoxCornerMatrix; 9 | class BoundingBox { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | voxblox::Point min = {INFINITY, INFINITY, INFINITY}; 14 | voxblox::Point max = {-INFINITY, -INFINITY, -INFINITY}; 15 | 16 | void reset(); 17 | 18 | const BoxCornerMatrix getCornerCoordinates() const; 19 | 20 | static const BoundingBox getAabbFromObbAndPose( 21 | const BoundingBox& obb, const voxblox::Transformation& pose); 22 | }; 23 | } // namespace voxgraph 24 | 25 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_BOUNDING_BOX_H_ 26 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/pose/pose_6d.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_POSE_POSE_6D_H_ 2 | #define VOXGRAPH_BACKEND_NODE_POSE_POSE_6D_H_ 3 | 4 | #include "voxgraph/backend/node/pose/pose.h" 5 | 6 | namespace voxgraph { 7 | class Pose6D : public Pose { 8 | public: 9 | typedef std::array R3So3Vector; 10 | 11 | // Converting constructor from minkindr 12 | explicit Pose6D(const Transformation& initial_pose); 13 | 14 | // User defined conversion to minkindr 15 | operator Transformation() const final; // NOLINT 16 | 17 | R3So3Vector::value_type* optimizationVectorData() final { 18 | return r3_so3_vector_.data(); 19 | } 20 | 21 | size_t optimizationVectorSize() const final { return r3_so3_vector_.size(); } 22 | 23 | private: 24 | R3So3Vector r3_so3_vector_; 25 | }; 26 | } // namespace voxgraph 27 | 28 | #endif // VOXGRAPH_BACKEND_NODE_POSE_POSE_6D_H_ 29 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/pose/pose_4d.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_POSE_POSE_4D_H_ 2 | #define VOXGRAPH_BACKEND_NODE_POSE_POSE_4D_H_ 3 | 4 | #include "voxgraph/backend/node/pose/pose.h" 5 | 6 | namespace voxgraph { 7 | class Pose4D : public Pose { 8 | public: 9 | typedef std::array XyzYawVector; 10 | 11 | // Converting constructor from minkindr 12 | explicit Pose4D(const Transformation& initial_pose); 13 | 14 | // User defined conversion to minkindr 15 | operator Transformation() const final; // NOLINT 16 | 17 | XyzYawVector::value_type* optimizationVectorData() final { 18 | return xyz_yaw_vector_.data(); 19 | } 20 | 21 | size_t optimizationVectorSize() const final { return xyz_yaw_vector_.size(); } 22 | 23 | private: 24 | XyzYawVector xyz_yaw_vector_; 25 | }; 26 | } // namespace voxgraph 27 | 28 | #endif // VOXGRAPH_BACKEND_NODE_POSE_POSE_4D_H_ 29 | -------------------------------------------------------------------------------- /voxgraph/src/backend/node/node.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/node/node.h" 2 | 3 | namespace voxgraph { 4 | void Node::addToProblem(ceres::Problem* problem, 5 | ceres::LocalParameterization* local_parameterization) { 6 | CHECK_NOTNULL(problem); 7 | CHECK_NOTNULL(local_parameterization); 8 | 9 | // Add the node to the solver and set it to be held constant if appropriate 10 | problem->AddParameterBlock(optimized_pose_.optimizationVectorData(), 11 | optimized_pose_.optimizationVectorSize()); 12 | if (config_.set_constant) { 13 | problem->SetParameterBlockConstant( 14 | optimized_pose_.optimizationVectorData()); 15 | } 16 | 17 | // Set the local parameterization s.t. yaw stays normalized 18 | problem->SetParameterization(getPosePtr()->optimizationVectorData(), 19 | local_parameterization); 20 | } 21 | } // namespace voxgraph 22 | -------------------------------------------------------------------------------- /voxgraph/src/backend/node/pose/pose_4d.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/node/pose/pose_4d.h" 2 | 3 | namespace voxgraph { 4 | Pose4D::Pose4D(const Transformation& initial_pose) : Pose(initial_pose) { 5 | // Initialize the optimization variables 6 | xyz_yaw_vector_[0] = initial_pose_vec_[0]; 7 | xyz_yaw_vector_[1] = initial_pose_vec_[1]; 8 | xyz_yaw_vector_[2] = initial_pose_vec_[2]; 9 | xyz_yaw_vector_[3] = initial_pose_vec_[5]; 10 | } 11 | 12 | Pose4D::operator Transformation() const { 13 | Transformation::Vector6 T_vec_6d; 14 | // Set x, y, z and yaw from the optimization variables 15 | T_vec_6d[0] = xyz_yaw_vector_[0]; 16 | T_vec_6d[1] = xyz_yaw_vector_[1]; 17 | T_vec_6d[2] = xyz_yaw_vector_[2]; 18 | T_vec_6d[5] = xyz_yaw_vector_[3]; 19 | // Set pitch and roll to zero from the initial pose 20 | T_vec_6d[3] = initial_pose_vec_[3]; 21 | T_vec_6d[4] = initial_pose_vec_[4]; 22 | return Transformation::exp(T_vec_6d); 23 | } 24 | } // namespace voxgraph 25 | -------------------------------------------------------------------------------- /voxgraph/src/backend/constraint/constraint_collection.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/constraint/constraint_collection.h" 2 | 3 | namespace voxgraph { 4 | void ConstraintCollection::addConstraintsToProblem( 5 | const NodeCollection& node_collection, ceres::Problem* problem_ptr, 6 | bool exclude_registration_constraints) { 7 | CHECK_NOTNULL(problem_ptr); 8 | 9 | // Add all constraints of the appropriate types to the problem 10 | for (Constraint& absolute_pose_constraint : absolute_pose_constraints_) { 11 | absolute_pose_constraint.addToProblem(node_collection, problem_ptr); 12 | } 13 | for (Constraint& relative_pose_constraint : relative_pose_constraints_) { 14 | relative_pose_constraint.addToProblem(node_collection, problem_ptr); 15 | } 16 | if (!exclude_registration_constraints) { 17 | for (Constraint& registration_constraint : registration_constraints_) { 18 | registration_constraint.addToProblem(node_collection, problem_ptr); 19 | } 20 | } 21 | } 22 | } // namespace voxgraph 23 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/pose_graph_interface/node_templates.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_NODE_TEMPLATES_H_ 2 | #define VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_NODE_TEMPLATES_H_ 3 | 4 | #include "voxgraph/backend/node/reference_frame_node.h" 5 | #include "voxgraph/backend/node/submap_node.h" 6 | 7 | namespace voxgraph { 8 | class NodeTemplates { 9 | public: 10 | enum ReferenceFrames : ReferenceFrameNode::FrameId { 11 | kMissionFrame, 12 | kGpsFrame 13 | }; 14 | 15 | NodeTemplates(); 16 | 17 | ReferenceFrameNode::Config getReferenceFrameConfigById( 18 | ReferenceFrameNode::FrameId frame_id); 19 | 20 | const ReferenceFrameNode::Config& mission_frame; 21 | const ReferenceFrameNode::Config& gps_frame; 22 | const SubmapNode::Config& submap; 23 | 24 | private: 25 | ReferenceFrameNode::Config mission_frame_; 26 | ReferenceFrameNode::Config gps_frame_; 27 | SubmapNode::Config submap_; 28 | }; 29 | } // namespace voxgraph 30 | 31 | #endif // VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_NODE_TEMPLATES_H_ 32 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/absolute_pose_constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_ABSOLUTE_POSE_CONSTRAINT_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_ABSOLUTE_POSE_CONSTRAINT_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/backend/constraint/constraint.h" 7 | 8 | namespace voxgraph { 9 | class AbsolutePoseConstraint : public Constraint { 10 | public: 11 | typedef std::shared_ptr Ptr; 12 | struct Config : Constraint::Config { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | ReferenceFrameNode::FrameId reference_frame_id; 15 | cblox::SubmapID submap_id; 16 | voxblox::Transformation T_ref_submap; 17 | }; 18 | 19 | AbsolutePoseConstraint(ConstraintId constraint_id, const Config& config) 20 | : Constraint(constraint_id, config), config_(config) {} 21 | 22 | void addToProblem(const NodeCollection& node_collection, 23 | ceres::Problem* problem) final; 24 | 25 | private: 26 | const Config config_; 27 | }; 28 | } // namespace voxgraph 29 | 30 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_ABSOLUTE_POSE_CONSTRAINT_H_ 31 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/relative_pose_constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/backend/constraint/constraint.h" 7 | 8 | namespace voxgraph { 9 | class RelativePoseConstraint : public Constraint { 10 | public: 11 | typedef std::shared_ptr Ptr; 12 | struct Config : Constraint::Config { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | cblox::SubmapID origin_submap_id; 15 | cblox::SubmapID destination_submap_id; 16 | voxblox::Transformation T_origin_destination; 17 | }; 18 | 19 | RelativePoseConstraint(ConstraintId constraint_id, const Config& config) 20 | : Constraint(constraint_id, config), config_(config) {} 21 | 22 | void addToProblem(const NodeCollection& node_collection, 23 | ceres::Problem* problem) final; 24 | 25 | private: 26 | const Config config_; 27 | }; 28 | } // namespace voxgraph 29 | 30 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_H_ 31 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/submap_timeline.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_SUBMAP_TIMELINE_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_SUBMAP_TIMELINE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace voxgraph { 12 | class SubmapTimeline { 13 | public: 14 | SubmapTimeline() = default; 15 | 16 | void addNextSubmap(const ros::Time& submap_creation_timestamp, 17 | const cblox::SubmapID& submap_id); 18 | 19 | bool lookupActiveSubmapByTime(const ros::Time& timestamp, 20 | cblox::SubmapID* submap_id); 21 | 22 | cblox::SubmapID getPreviousSubmapId() const; 23 | cblox::SubmapID getFirstSubmapId() const; 24 | cblox::SubmapID getLastSubmapId() const; 25 | 26 | private: 27 | // Map from each time interval's start time to the corresponding active submap 28 | std::map submap_timeline_; 29 | }; 30 | } // namespace voxgraph 31 | 32 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_SUBMAP_TIMELINE_H_ 33 | -------------------------------------------------------------------------------- /voxgraph/config/registration_test_bench.yaml: -------------------------------------------------------------------------------- 1 | submap_collection_file_path: "/home/victor/data/voxgraph/submap_collections/burning_house_rubble_gt_truncdist06_z-aligned" 2 | log_folder_path: "/home/victor/data/voxgraph/registration_test_bench_stats" 3 | 4 | reference_submap_id: 3 5 | reading_submap_id: 3 6 | 7 | solver_report_style: "brief" # "brief", "full" or "none" 8 | 9 | test_range: 10 | x: [-0.6, 0, 0.3] 11 | y: [-0.6, 0, 0.3] 12 | z: [-0.6, 0, 0.3] 13 | yaw: [-0.2, 0, 0.1] 14 | # pitch: [-0.17, 0, 0.17] 15 | # roll: [-0.17, 0, 0.17] 16 | 17 | submap_registration: 18 | solver: 19 | max_num_iterations: 40 20 | parameter_tolerance: 3e-9 21 | cost: 22 | use_esdf_distance: true 23 | jacobian_evaluation_method: "analytic" # "analytic" or "numeric" 24 | registration_method: "explicit_to_implicit" # "implicit_to_implicit" or "explicit_to_implicit" 25 | min_voxel_weight: 1 26 | max_voxel_distance: 0.4 27 | sampling_ratio: 0.2 # Set to -1 or leave out to disable 28 | no_correspondence_cost: 0.0 29 | visualize_residuals: false 30 | visualize_gradients: false 31 | visualize_transforms: true 32 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/local_parameterization/angle_local_parameterization.h: -------------------------------------------------------------------------------- 1 | // NOTE: This function comes directly from the ceres::examples 2 | 3 | #ifndef VOXGRAPH_BACKEND_LOCAL_PARAMETERIZATION_ANGLE_LOCAL_PARAMETERIZATION_H_ 4 | #define VOXGRAPH_BACKEND_LOCAL_PARAMETERIZATION_ANGLE_LOCAL_PARAMETERIZATION_H_ 5 | 6 | #include 7 | 8 | #include "voxgraph/backend/local_parameterization/normalize_angle.h" 9 | 10 | namespace voxgraph { 11 | // Define a local parameterization that keeps the angle within [-pi to pi) rad. 12 | class AngleLocalParameterization { 13 | public: 14 | template 15 | bool operator()(const T* theta_radians, const T* delta_theta_radians, 16 | T* theta_radians_plus_delta) const { 17 | *theta_radians_plus_delta = 18 | NormalizeAngle(*theta_radians + *delta_theta_radians); 19 | 20 | return true; 21 | } 22 | 23 | static ceres::LocalParameterization* Create() { 24 | return (new ceres::AutoDiffLocalParameterization); 26 | } 27 | }; 28 | } // namespace voxgraph 29 | 30 | #endif // VOXGRAPH_BACKEND_LOCAL_PARAMETERIZATION_ANGLE_LOCAL_PARAMETERIZATION_H_ 31 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/io.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_IO_H_ 2 | #define VOXGRAPH_TOOLS_IO_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 12 | 13 | namespace voxgraph { 14 | namespace io { 15 | bool savePoseHistoryToFile( 16 | const std::string& filepath, 17 | const VoxgraphSubmapCollection::PoseStampedVector& pose_history) { 18 | // Write the pose history to a rosbag 19 | rosbag::Bag bag; 20 | bag.open(filepath, rosbag::bagmode::Write); 21 | for (const geometry_msgs::PoseStamped& pose_stamped : pose_history) { 22 | bag.write("pose_history", pose_stamped.header.stamp, pose_stamped); 23 | } 24 | bag.close(); 25 | return true; // Zero error checking here! 26 | } 27 | 28 | template 29 | bool saveVectorToFile(const std::string& filepath, const std::vector vec) { 30 | std::ofstream os; 31 | os.open(filepath, std::ios::out | std::ios::trunc); 32 | for (const auto& num : vec) { 33 | os << num << std::endl; 34 | } 35 | os.close(); 36 | return true; 37 | } 38 | } // namespace io 39 | } // namespace voxgraph 40 | 41 | #endif // VOXGRAPH_TOOLS_IO_H_ 42 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/visualization/cost_function_visuals.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_VISUALIZATION_COST_FUNCTION_VISUALS_H_ 2 | #define VOXGRAPH_TOOLS_VISUALIZATION_COST_FUNCTION_VISUALS_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace voxgraph { 8 | class CostFunctionVisuals { 9 | public: 10 | CostFunctionVisuals(); 11 | 12 | void addResidual(const voxblox::Point& coordinate, const double& residual); 13 | void addJacobian(const voxblox::Point& coordinate, 14 | const voxblox::Point& jacobian); 15 | 16 | void scaleAndPublish(const double factor); 17 | 18 | void reset(); 19 | 20 | private: 21 | // ROS publishers 22 | ros::Publisher residual_ptcloud_pub_; 23 | ros::Publisher jacobians_pub_; 24 | 25 | // Pointcloud used to visualize the residuals 26 | pcl::PointCloud residual_ptcloud_; 27 | 28 | // Variables used to visualize the Jacobians, 29 | // see more details in the constructor 30 | visualization_msgs::MarkerArray jacobian_marker_array_; 31 | visualization_msgs::Marker jacobian_arrows_; 32 | visualization_msgs::Marker jacobian_origins_; 33 | }; 34 | } // namespace voxgraph 35 | 36 | #endif // VOXGRAPH_TOOLS_VISUALIZATION_COST_FUNCTION_VISUALS_H_ 37 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/node.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_NODE_H_ 2 | #define VOXGRAPH_BACKEND_NODE_NODE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace voxgraph { 11 | class Node { 12 | public: 13 | typedef std::shared_ptr Ptr; 14 | typedef unsigned int NodeId; 15 | 16 | struct Config { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | bool set_constant; 19 | voxblox::Transformation T_mission_node_initial; 20 | }; 21 | 22 | Node(const Node::NodeId& node_id, const Config& config) 23 | : node_id_(node_id), 24 | config_(config), 25 | optimized_pose_(config.T_mission_node_initial) {} 26 | virtual ~Node() = default; 27 | 28 | const Pose& getPose() const { return optimized_pose_; } 29 | Pose* getPosePtr() { return &optimized_pose_; } 30 | 31 | void setConstant(bool constant) { config_.set_constant = constant; } 32 | bool isConstant() const { return config_.set_constant; } 33 | 34 | void addToProblem(ceres::Problem* problem, 35 | ceres::LocalParameterization* local_parameterization); 36 | 37 | protected: 38 | const NodeId node_id_; 39 | Config config_; 40 | 41 | Pose4D optimized_pose_; 42 | }; 43 | } // namespace voxgraph 44 | 45 | #endif // VOXGRAPH_BACKEND_NODE_NODE_H_ 46 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/measurement_processors/pointcloud_integrator.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_MEASUREMENT_PROCESSORS_POINTCLOUD_INTEGRATOR_H_ 2 | #define VOXGRAPH_FRONTEND_MEASUREMENT_PROCESSORS_POINTCLOUD_INTEGRATOR_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "voxgraph/common.h" 11 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 12 | 13 | namespace voxgraph { 14 | class PointcloudIntegrator { 15 | public: 16 | explicit PointcloudIntegrator(bool verbose = false); 17 | 18 | void setTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& node_handle); 19 | 20 | void integratePointcloud(const sensor_msgs::PointCloud2::Ptr& pointcloud_msg, 21 | const voxblox::Transformation& T_submap_sensor, 22 | VoxgraphSubmap* submap_ptr); 23 | 24 | private: 25 | bool verbose_; 26 | 27 | // Tools used when integrating the pointclouds into submaps 28 | std::shared_ptr color_map_; 29 | voxblox::TsdfIntegratorBase::Config tsdf_integrator_config_; 30 | std::unique_ptr tsdf_integrator_; 31 | }; 32 | } // namespace voxgraph 33 | 34 | #endif // VOXGRAPH_FRONTEND_MEASUREMENT_PROCESSORS_POINTCLOUD_INTEGRATOR_H_ 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, ETHZ ASL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/threading_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_THREADING_HELPER_H_ 2 | #define VOXGRAPH_TOOLS_THREADING_HELPER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace voxgraph { 9 | class ThreadingHelper { 10 | public: 11 | template 12 | static void launchBackgroundThread(Function&& function, Args&&... args) { 13 | // Bind the function to avoid having to distinguish between special cases 14 | // when calling it later (member functions,...) 15 | // NOTE: Unfortunately std::invoke is not yet available in C++11 16 | auto bound_function = std::bind(function, args...); 17 | 18 | // Run the task in the background, using a low-priority detached thread 19 | std::thread background_thread([bound_function] { 20 | // NOTE: A higher 'nice' value corresponds to a lower priority, see 21 | // https://linux.die.net/man/3/nice 22 | constexpr int LOWEST_THREAD_PRIORITY = 19; 23 | if (nice(LOWEST_THREAD_PRIORITY) == -1) { 24 | ROS_WARN_STREAM( 25 | "Could not deprioritize thread to " 26 | "run in the background: " 27 | << strerror(errno)); 28 | } 29 | bound_function(); 30 | }); 31 | background_thread.detach(); 32 | } 33 | }; 34 | } // namespace voxgraph 35 | 36 | #endif // VOXGRAPH_TOOLS_THREADING_HELPER_H_ 37 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/ros_params.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_ROS_PARAMS_H_ 2 | #define VOXGRAPH_TOOLS_ROS_PARAMS_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxgraph/frontend/submap_collection/voxgraph_submap.h" 8 | 9 | namespace voxgraph { 10 | inline VoxgraphSubmap::Config getVoxgraphSubmapConfigFromRosParams( 11 | const ros::NodeHandle& nh_private) { 12 | // Getting the tsdf map params from ROS. 13 | const voxblox::TsdfMap::Config tsdf_map_config = 14 | voxblox::getTsdfMapConfigFromRosParam(nh_private); 15 | const voxblox::EsdfMap::Config esdf_map_config = 16 | voxblox::getEsdfMapConfigFromRosParam(nh_private); 17 | 18 | // Copying over the members 19 | // Note(alexmillane): This is very prone to failure down the line. 20 | // The basic problem is that inheritance is the wrong thing to use for these 21 | // config structs. 22 | VoxgraphSubmap::Config config; 23 | config.tsdf_voxel_size = tsdf_map_config.tsdf_voxel_size; 24 | config.tsdf_voxels_per_side = tsdf_map_config.tsdf_voxels_per_side; 25 | config.esdf_voxel_size = esdf_map_config.esdf_voxel_size; 26 | config.esdf_voxels_per_side = esdf_map_config.esdf_voxels_per_side; 27 | 28 | // TODO(alexmillane): RegistrationFilter params are not grabbed yet here 29 | 30 | return config; 31 | } 32 | } // namespace voxgraph 33 | 34 | #endif // VOXGRAPH_TOOLS_ROS_PARAMS_H_ 35 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_CONSTRAINT_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_CONSTRAINT_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "voxgraph/backend/node/node_collection.h" 9 | 10 | namespace voxgraph { 11 | class Constraint { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | typedef std::shared_ptr Ptr; 16 | typedef unsigned int ConstraintId; 17 | typedef Eigen::Matrix InformationMatrix; 18 | 19 | struct Config { 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | InformationMatrix information_matrix; 22 | bool allow_semi_definite_information_matrix = false; 23 | }; 24 | 25 | explicit Constraint(ConstraintId constraint_id, const Config& config); 26 | virtual ~Constraint() = default; 27 | 28 | virtual void addToProblem(const NodeCollection& node_collection, 29 | ceres::Problem* problem) = 0; 30 | 31 | ceres::ResidualBlockId getResidualBlockId() { return residual_block_id_; } 32 | 33 | protected: 34 | static constexpr ceres::LossFunction* kNoRobustLossFunction = nullptr; 35 | 36 | const ConstraintId constraint_id_; 37 | ceres::ResidualBlockId residual_block_id_ = nullptr; 38 | 39 | InformationMatrix sqrt_information_matrix_; 40 | }; 41 | } // namespace voxgraph 42 | 43 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_CONSTRAINT_H_ 44 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/submap_registration_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_SUBMAP_REGISTRATION_HELPER_H_ 2 | #define VOXGRAPH_TOOLS_SUBMAP_REGISTRATION_HELPER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "voxgraph/backend/constraint/cost_functions/registration_cost_function.h" 11 | #include "voxgraph/frontend/submap_collection/voxgraph_submap.h" 12 | 13 | namespace voxgraph { 14 | class SubmapRegistrationHelper { 15 | public: 16 | struct Options { 17 | ceres::Solver::Options solver; 18 | RegistrationCostFunction::Config registration; 19 | }; 20 | 21 | SubmapRegistrationHelper( 22 | cblox::SubmapCollection::ConstPtr submap_collection_ptr, 23 | const Options& options); 24 | 25 | ~SubmapRegistrationHelper() = default; 26 | 27 | bool testRegistration(const cblox::SubmapID& reference_submap_id, 28 | const cblox::SubmapID& reading_submap_id, 29 | double* mission_pose_reading, 30 | ceres::Solver::Summary* summary); 31 | 32 | private: 33 | cblox::SubmapCollection::ConstPtr submap_collection_ptr_; 34 | Options options_; 35 | }; 36 | } // namespace voxgraph 37 | 38 | #endif // VOXGRAPH_TOOLS_SUBMAP_REGISTRATION_HELPER_H_ 39 | -------------------------------------------------------------------------------- /voxgraph/config/voxgraph_mapper.yaml: -------------------------------------------------------------------------------- 1 | verbose: false 2 | debug: true 3 | auto_pause_rosbag: false 4 | 5 | submap_creation_interval: 10 6 | subscriber_queue_length: 10 7 | 8 | input_odom_frame: "odom" 9 | input_base_link_frame: "imu" 10 | output_mission_frame: "mission" 11 | output_odom_frame: "odom" 12 | output_base_link_frame: "imu" 13 | output_sensor_frame: "sensor_voxgraph" 14 | 15 | # T_base_sensor: 16 | # - [-0.9908275, -0.0196803, 0.1336922, -0.024] 17 | # - [ 0.0202159, -0.9997921, 0.0026504, -0.011] 18 | # - [ 0.1336122, 0.0053288, 0.9910194, -0.131] 19 | # - [ 0.0, 0.0, 0.0, 1.0] 20 | 21 | tsdf_voxel_size: 0.20 22 | tsdf_integrator: 23 | truncation_distance: 0.60 24 | max_ray_length_m: 16.0 25 | use_const_weight: true 26 | use_weight_dropoff: true 27 | use_sparsity_compensation_factor: true 28 | sparsity_compensation_factor: 20.0 29 | # integration_order_mode: "sorted" 30 | 31 | measurements: 32 | submap_registration: 33 | enabled: true 34 | sampling_ratio: 0.05 35 | registration_method: "explicit_to_implicit" 36 | information_matrix: 37 | x_x: 1.0 38 | y_y: 1.0 39 | z_z: 1.0 40 | yaw_yaw: 1.0 41 | odometry: 42 | enabled: true 43 | information_matrix: 44 | x_x: 1.0 45 | y_y: 1.0 46 | z_z: 2500.0 47 | yaw_yaw: 2500.0 48 | height: 49 | enabled: false 50 | information_zz: 2500.0 51 | 52 | mesh_min_weight: 2.0 53 | mesh_use_color: false 54 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/weighted_sampler.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_WEIGHTED_SAMPLER_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_WEIGHTED_SAMPLER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace voxgraph { 9 | template 10 | class WeightedSampler { 11 | public: 12 | using ItemContainer = std::vector; 13 | 14 | WeightedSampler() = default; 15 | 16 | void addItem(const ItemType& new_item, const double weight); 17 | 18 | // Deterministically get the i-th item 19 | inline const ItemType& operator[](int i) const { return items_[i]; } 20 | 21 | // Randomly draw an item with probability proportional to its weight 22 | inline const ItemType& getRandomItem() const; 23 | 24 | size_t size() const { return items_.size(); } 25 | 26 | void clear(); 27 | 28 | private: 29 | // Items and vector storing their cumulative probabilities 30 | ItemContainer items_; 31 | std::vector cumulative_item_weights_; 32 | 33 | // Uniform random number generator 34 | mutable std::mt19937 random_number_generator_; 35 | mutable std::uniform_real_distribution uniform_distribution_{0.0, 36 | 1.0}; 37 | }; 38 | } // namespace voxgraph 39 | 40 | #include "voxgraph/frontend/submap_collection/weighted_sampler_inl.h" 41 | 42 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_WEIGHTED_SAMPLER_H_ 43 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/node/node_collection.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_NODE_NODE_COLLECTION_H_ 2 | #define VOXGRAPH_BACKEND_NODE_NODE_COLLECTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxgraph/backend/node/reference_frame_node.h" 8 | #include "voxgraph/backend/node/submap_node.h" 9 | 10 | namespace voxgraph { 11 | class NodeCollection { 12 | public: 13 | typedef std::map SubmapNodeMap; 14 | typedef std::map 15 | ReferenceFrameNodeMap; 16 | 17 | NodeCollection(); 18 | 19 | void addSubmapNode(const SubmapNode::Config& config); 20 | void addReferenceFrameNode(const ReferenceFrameNode::Config& config); 21 | 22 | ceres::LocalParameterization* getLocalParameterization() const { 23 | return local_parameterization_.get(); 24 | } 25 | 26 | SubmapNode::Ptr getSubmapNodePtrById(const cblox::SubmapID& submap_id) const; 27 | ReferenceFrameNode::Ptr getReferenceFrameNodePtrById( 28 | const ReferenceFrameNode::FrameId& frame_id) const; 29 | 30 | const SubmapNodeMap& getSubmapNodes() { return submap_nodes_; } 31 | 32 | private: 33 | Node::NodeId node_id_counter_ = 0; 34 | Node::NodeId newNodeId() { return node_id_counter_++; } 35 | 36 | SubmapNodeMap submap_nodes_; 37 | ReferenceFrameNodeMap reference_frame_nodes_; 38 | 39 | std::shared_ptr local_parameterization_; 40 | }; 41 | } // namespace voxgraph 42 | 43 | #endif // VOXGRAPH_BACKEND_NODE_NODE_COLLECTION_H_ 44 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/pose_graph_interface/measurement_templates.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_MEASUREMENT_TEMPLATES_H_ 2 | #define VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_MEASUREMENT_TEMPLATES_H_ 3 | 4 | #include "voxgraph/backend/constraint/absolute_pose_constraint.h" 5 | #include "voxgraph/backend/constraint/registration_constraint.h" 6 | #include "voxgraph/backend/constraint/relative_pose_constraint.h" 7 | #include "voxgraph/frontend/pose_graph_interface/node_templates.h" 8 | 9 | namespace voxgraph { 10 | class MeasurementTemplates { 11 | public: 12 | explicit MeasurementTemplates(bool verbose = false); 13 | 14 | void setFromRosParams(const ros::NodeHandle& node_handle); 15 | 16 | const RelativePoseConstraint::Config& odometry; 17 | const RelativePoseConstraint::Config& loop_closure; 18 | const AbsolutePoseConstraint::Config& gps; 19 | const AbsolutePoseConstraint::Config& height; 20 | const RegistrationConstraint::Config& registration; 21 | 22 | private: 23 | bool verbose_; 24 | 25 | void setInformationMatrixFromRosParams( 26 | const ros::NodeHandle& node_handle, 27 | Constraint::InformationMatrix* information_matrix); 28 | 29 | RelativePoseConstraint::Config odometry_; 30 | RelativePoseConstraint::Config loop_closure_; 31 | AbsolutePoseConstraint::Config gps_; 32 | AbsolutePoseConstraint::Config height_; 33 | RegistrationConstraint::Config registration_; 34 | }; 35 | } // namespace voxgraph 36 | 37 | #endif // VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_MEASUREMENT_TEMPLATES_H_ 38 | -------------------------------------------------------------------------------- /voxgraph/src/tools/tf_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/tf_helper.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace voxgraph { 9 | void TfHelper::publishTransform(const voxblox::Transformation& transform, 10 | const std::string& base_frame, 11 | const std::string& target_frame, 12 | bool tf_is_static, const ros::Time& timestamp) { 13 | static tf2_ros::TransformBroadcaster transform_broadcaster; 14 | static tf2_ros::StaticTransformBroadcaster static_transform_broadcaster; 15 | 16 | geometry_msgs::TransformStamped tf_stamped; 17 | tf_stamped.header.stamp = timestamp; 18 | tf_stamped.header.frame_id = base_frame; 19 | tf_stamped.child_frame_id = target_frame; 20 | tf_stamped.transform.translation.x = transform.getPosition().x(); 21 | tf_stamped.transform.translation.y = transform.getPosition().y(); 22 | tf_stamped.transform.translation.z = transform.getPosition().z(); 23 | tf_stamped.transform.rotation.x = transform.getRotation().x(); 24 | tf_stamped.transform.rotation.y = transform.getRotation().y(); 25 | tf_stamped.transform.rotation.z = transform.getRotation().z(); 26 | tf_stamped.transform.rotation.w = transform.getRotation().w(); 27 | 28 | if (tf_is_static) { 29 | static_transform_broadcaster.sendTransform(tf_stamped); 30 | } else { 31 | transform_broadcaster.sendTransform(tf_stamped); 32 | } 33 | } 34 | } // namespace voxgraph 35 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/data_servers/projected_map_server.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_DATA_SERVERS_PROJECTED_MAP_SERVER_H_ 2 | #define VOXGRAPH_TOOLS_DATA_SERVERS_PROJECTED_MAP_SERVER_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/common.h" 7 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 8 | 9 | namespace voxgraph { 10 | class ProjectedMapServer { 11 | public: 12 | explicit ProjectedMapServer(ros::NodeHandle nh_private); 13 | 14 | // Publish the map using this map server instance's ros publisher member 15 | void publishProjectedMap(const VoxgraphSubmapCollection& submap_collection, 16 | const ros::Time& timestamp); 17 | 18 | // "Bring your own publisher" method 19 | // NOTE: This method is provided s.t. it can be called using publishers to 20 | // custom topics and without requiring a ProjectedMapServer instance. 21 | // It is therefore static. 22 | static void publishProjectedMap( 23 | const VoxgraphSubmapCollection& submap_collection, 24 | const ros::Time& timestamp, 25 | const ros::Publisher& projected_map_publisher); 26 | 27 | private: 28 | ros::Publisher projected_tsdf_map_pub_; 29 | 30 | // Convenience methods to generate the message and submap headers 31 | static std_msgs::Header generateHeaderMsg(const ros::Time& timestamp); 32 | static cblox_msgs::MapHeader generateMapHeaderMsg( 33 | const VoxgraphSubmapCollection& submap_collection); 34 | }; 35 | } // namespace voxgraph 36 | 37 | #endif // VOXGRAPH_TOOLS_DATA_SERVERS_PROJECTED_MAP_SERVER_H_ 38 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/pose_graph_interface/node_templates.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/pose_graph_interface/node_templates.h" 2 | 3 | namespace voxgraph { 4 | NodeTemplates::NodeTemplates() 5 | : mission_frame(mission_frame_), gps_frame(gps_frame_), submap(submap_) { 6 | // Initialize the mission frame config 7 | // NOTE: The frame is fixed to (X, Y, Z, Yaw) = (0, 0, 0, 0) 8 | mission_frame_.reference_frame_id = kMissionFrame; 9 | mission_frame_.set_constant = true; 10 | mission_frame_.T_mission_node_initial.setIdentity(); 11 | 12 | // Initialize the GPS frame config 13 | // NOTE: We let the frame float freely w.r.t. the optimization's mission 14 | // frame. 15 | // This way the submap positions will not jump when the first GPS 16 | // measurement is received even if the robot started mapping before 17 | // getting an absolute position fix. The absolute pose of a submap can 18 | // still be retrieved by multiplying the GPS reference frame's absolute 19 | // pose with the transform between the GPS frame and the submap. 20 | gps_frame_.reference_frame_id = kGpsFrame; 21 | gps_frame_.set_constant = false; 22 | } 23 | 24 | ReferenceFrameNode::Config NodeTemplates::getReferenceFrameConfigById( 25 | ReferenceFrameNode::FrameId frame_id) { 26 | switch (frame_id) { 27 | case kMissionFrame: 28 | return mission_frame; 29 | case kGpsFrame: 30 | return gps_frame; 31 | default: 32 | LOG(FATAL) << "Requested config for unknown reference frame id: " 33 | << frame_id; 34 | break; 35 | } 36 | } 37 | } // namespace voxgraph 38 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/weighted_sampler_inl.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_WEIGHTED_SAMPLER_INL_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_WEIGHTED_SAMPLER_INL_H_ 3 | 4 | namespace voxgraph { 5 | template 6 | void WeightedSampler::addItem(const ItemType& new_item, 7 | const double weight) { 8 | items_.push_back(new_item); 9 | if (cumulative_item_weights_.empty()) { 10 | cumulative_item_weights_.push_back(weight); 11 | } else { 12 | const double new_item_cumulative_weight = 13 | cumulative_item_weights_.back() + weight; 14 | cumulative_item_weights_.push_back(new_item_cumulative_weight); 15 | } 16 | } 17 | 18 | template 19 | const ItemType& WeightedSampler::getRandomItem() const { 20 | const double random_number = uniform_distribution_(random_number_generator_); 21 | const double random_cumulative_weight = 22 | random_number * cumulative_item_weights_.back(); 23 | const auto it = std::upper_bound(cumulative_item_weights_.begin(), 24 | cumulative_item_weights_.end(), 25 | random_cumulative_weight); 26 | const unsigned int random_index = it - cumulative_item_weights_.begin(); 27 | return items_[random_index]; 28 | } 29 | 30 | template 31 | void WeightedSampler::clear() { 32 | items_.clear(); 33 | cumulative_item_weights_.clear(); 34 | } 35 | } // namespace voxgraph 36 | 37 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_WEIGHTED_SAMPLER_INL_H_ 38 | -------------------------------------------------------------------------------- /voxgraph/src/backend/constraint/absolute_pose_constraint.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/constraint/absolute_pose_constraint.h" 2 | 3 | #include "voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h" 4 | 5 | namespace voxgraph { 6 | void AbsolutePoseConstraint::addToProblem(const NodeCollection& node_collection, 7 | ceres::Problem* problem) { 8 | CHECK_NOTNULL(problem); 9 | 10 | ceres::LossFunction* loss_function = kNoRobustLossFunction; 11 | 12 | // Get pointers to both submap nodes 13 | ReferenceFrameNode::Ptr reference_frame_node_ptr = 14 | node_collection.getReferenceFrameNodePtrById(config_.reference_frame_id); 15 | SubmapNode::Ptr submap_node_ptr = 16 | node_collection.getSubmapNodePtrById(config_.submap_id); 17 | CHECK_NOTNULL(reference_frame_node_ptr); 18 | CHECK_NOTNULL(submap_node_ptr); 19 | 20 | // Add the submap parameters to the problem 21 | reference_frame_node_ptr->addToProblem( 22 | problem, node_collection.getLocalParameterization()); 23 | submap_node_ptr->addToProblem(problem, 24 | node_collection.getLocalParameterization()); 25 | 26 | // Add the constraint to the optimization and keep track of it 27 | ceres::CostFunction* cost_function = RelativePoseCostFunction::Create( 28 | config_.T_ref_submap, sqrt_information_matrix_); 29 | residual_block_id_ = problem->AddResidualBlock( 30 | cost_function, loss_function, 31 | reference_frame_node_ptr->getPosePtr()->optimizationVectorData(), 32 | submap_node_ptr->getPosePtr()->optimizationVectorData()); 33 | } 34 | } // namespace voxgraph 35 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/map_tracker/transformers/tf_transformer.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/map_tracker/transformers/tf_transformer.h" 2 | 3 | #include 4 | 5 | namespace voxgraph { 6 | bool TfTransformer::waitForTransform(const std::string& to_frame_id, 7 | const std::string& from_frame_id, 8 | const ros::Time& frame_timestamp) { 9 | // Total time spent waiting for the updated pose 10 | ros::WallDuration t_waited(0.0); 11 | while (t_waited < transform_lookup_max_time_) { 12 | if (tf_buffer_.canTransform(to_frame_id, from_frame_id, frame_timestamp)) { 13 | return true; 14 | } 15 | transform_lookup_retry_period_.sleep(); 16 | t_waited += transform_lookup_retry_period_; 17 | } 18 | ROS_WARN("Waited %.3fs, but still could not get the TF from %s to %s", 19 | t_waited.toSec(), from_frame_id.c_str(), to_frame_id.c_str()); 20 | return false; 21 | } 22 | 23 | bool TfTransformer::lookupTransform(const std::string& to_frame_id, 24 | const std::string& from_frame_id, 25 | const ros::Time& frame_timestamp, 26 | Transformation* transform) { 27 | CHECK_NOTNULL(transform); 28 | if (!waitForTransform(to_frame_id, from_frame_id, frame_timestamp)) { 29 | return false; 30 | } 31 | geometry_msgs::TransformStamped transform_msg = 32 | tf_buffer_.lookupTransform(to_frame_id, from_frame_id, frame_timestamp); 33 | tf::transformMsgToKindr(transform_msg.transform, transform); 34 | return true; 35 | } 36 | } // namespace voxgraph 37 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/submap_collection/submap_timeline.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/submap_collection/submap_timeline.h" 2 | 3 | namespace voxgraph { 4 | void voxgraph::SubmapTimeline::addNextSubmap( 5 | const ros::Time& submap_creation_timestamp, 6 | const cblox::SubmapID& submap_id) { 7 | submap_timeline_.emplace_hint(submap_timeline_.end(), 8 | submap_creation_timestamp, submap_id); 9 | } 10 | 11 | bool SubmapTimeline::lookupActiveSubmapByTime(const ros::Time& timestamp, 12 | cblox::SubmapID* submap_id) { 13 | CHECK_NOTNULL(submap_id); 14 | 15 | // Get an iterator to the end of the time interval in which timestamp falls 16 | auto iterator = submap_timeline_.upper_bound(timestamp); 17 | 18 | // Ensure that the timestamp is not from before we started logging 19 | if (iterator == submap_timeline_.begin()) { 20 | submap_id = nullptr; 21 | return false; 22 | } 23 | 24 | // The interval's active submap id is stored at its start point 25 | iterator--; 26 | *submap_id = iterator->second; 27 | return true; 28 | } 29 | 30 | cblox::SubmapID SubmapTimeline::getPreviousSubmapId() const { 31 | CHECK_GE(submap_timeline_.size(), 2); 32 | return (--(--submap_timeline_.end()))->second; 33 | } 34 | 35 | cblox::SubmapID SubmapTimeline::getFirstSubmapId() const { 36 | CHECK(!submap_timeline_.empty()); 37 | return submap_timeline_.begin()->second; 38 | } 39 | 40 | cblox::SubmapID SubmapTimeline::getLastSubmapId() const { 41 | CHECK(!submap_timeline_.empty()); 42 | return (--submap_timeline_.end())->second; 43 | } 44 | } // namespace voxgraph 45 | -------------------------------------------------------------------------------- /voxgraph/src/backend/constraint/relative_pose_constraint.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/constraint/relative_pose_constraint.h" 2 | 3 | #include "voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h" 4 | 5 | namespace voxgraph { 6 | void RelativePoseConstraint::addToProblem(const NodeCollection& node_collection, 7 | ceres::Problem* problem) { 8 | CHECK_NOTNULL(problem); 9 | 10 | ceres::LossFunction* loss_function = kNoRobustLossFunction; 11 | 12 | // Get pointers to both submap nodes 13 | SubmapNode::Ptr origin_submap_node_ptr = 14 | node_collection.getSubmapNodePtrById(config_.origin_submap_id); 15 | SubmapNode::Ptr destination_submap_node_ptr = 16 | node_collection.getSubmapNodePtrById(config_.destination_submap_id); 17 | CHECK_NOTNULL(origin_submap_node_ptr); 18 | CHECK_NOTNULL(destination_submap_node_ptr); 19 | 20 | // Add the submap parameters to the problem 21 | origin_submap_node_ptr->addToProblem( 22 | problem, node_collection.getLocalParameterization()); 23 | destination_submap_node_ptr->addToProblem( 24 | problem, node_collection.getLocalParameterization()); 25 | 26 | // Add the constraint to the optimization and keep track of it 27 | ceres::CostFunction* cost_function = RelativePoseCostFunction::Create( 28 | config_.T_origin_destination, sqrt_information_matrix_); 29 | residual_block_id_ = problem->AddResidualBlock( 30 | cost_function, loss_function, 31 | origin_submap_node_ptr->getPosePtr()->optimizationVectorData(), 32 | destination_submap_node_ptr->getPosePtr()->optimizationVectorData()); 33 | } 34 | } // namespace voxgraph 35 | -------------------------------------------------------------------------------- /voxgraph/src/backend/node/node_collection.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/node/node_collection.h" 2 | 3 | #include 4 | 5 | #include "voxgraph/backend/local_parameterization/angle_local_parameterization.h" 6 | 7 | namespace voxgraph { 8 | NodeCollection::NodeCollection() { 9 | local_parameterization_ = std::make_shared( 10 | new ceres::IdentityParameterization(3), 11 | AngleLocalParameterization::Create()); 12 | } 13 | 14 | void NodeCollection::addSubmapNode(const SubmapNode::Config& config) { 15 | auto submap_node_ptr = std::make_shared(newNodeId(), config); 16 | submap_nodes_.emplace(config.submap_id, submap_node_ptr); 17 | } 18 | 19 | void NodeCollection::addReferenceFrameNode( 20 | const ReferenceFrameNode::Config& config) { 21 | auto reference_frame_node_ptr = 22 | std::make_shared(newNodeId(), config); 23 | reference_frame_nodes_.emplace(config.reference_frame_id, 24 | reference_frame_node_ptr); 25 | } 26 | 27 | SubmapNode::Ptr NodeCollection::getSubmapNodePtrById( 28 | const cblox::SubmapID& submap_id) const { 29 | auto it = submap_nodes_.find(submap_id); 30 | if (it != submap_nodes_.end()) { 31 | return it->second; 32 | } else { 33 | return nullptr; 34 | } 35 | } 36 | 37 | ReferenceFrameNode::Ptr NodeCollection::getReferenceFrameNodePtrById( 38 | const ReferenceFrameNode::FrameId& frame_id) const { 39 | auto it = reference_frame_nodes_.find(frame_id); 40 | if (it != reference_frame_nodes_.end()) { 41 | return it->second; 42 | } else { 43 | return nullptr; 44 | } 45 | } 46 | } // namespace voxgraph 47 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/odometry_simulator/normal_distribution.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_ODOMETRY_SIMULATOR_NORMAL_DISTRIBUTION_H_ 2 | #define VOXGRAPH_TOOLS_ODOMETRY_SIMULATOR_NORMAL_DISTRIBUTION_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace voxgraph { 9 | class NormalDistribution { 10 | public: 11 | explicit NormalDistribution(const double& mean = 0, const double& stddev = 1) 12 | : mean_(mean), stddev_(stddev) {} 13 | 14 | double& mean() { return mean_; } 15 | double& stddev() { return stddev_; } 16 | 17 | // Return a sample from the normal distribution N(mean_, stddev_) 18 | double operator()() { 19 | CHECK_GE(stddev_, 0) << "The standard deviation must be non-negative."; 20 | // Random engine 21 | // TODO(victorr): Add proper random seed handling (and option to provide it) 22 | // NOTE: The noise generator is static to ensure that all instances draw 23 | // subsequent (different) numbers from the same pseudo random 24 | // sequence. If the generator is instance specific, there's a risk 25 | // that multiple instances use generators with the same seed and 26 | // output the same sequence. 27 | static std::mt19937 noise_generator_; 28 | 29 | // Draw a sample from the standard normal N(0,1) and 30 | // scale it using the change of variables formula 31 | return normal_distribution_(noise_generator_) * stddev_ + mean_; 32 | } 33 | 34 | private: 35 | double mean_, stddev_; 36 | 37 | // Standard normal distribution 38 | std::normal_distribution normal_distribution_; 39 | }; 40 | } // namespace voxgraph 41 | 42 | #endif // VOXGRAPH_TOOLS_ODOMETRY_SIMULATOR_NORMAL_DISTRIBUTION_H_ 43 | -------------------------------------------------------------------------------- /voxgraph/launch/voxgraph_mapper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 8 | 11 | 14 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/submap_collection/bounding_box.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/submap_collection/bounding_box.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace voxgraph { 7 | void BoundingBox::reset() { 8 | min = {INFINITY, INFINITY, INFINITY}; 9 | max = {-INFINITY, -INFINITY, -INFINITY}; 10 | } 11 | 12 | const BoxCornerMatrix BoundingBox::getCornerCoordinates() const { 13 | BoxCornerMatrix box_corners; 14 | // The set of box corners is composed by taking all combinations of 15 | // coefficients from the min and max point, i.e. 2^3=8 combinations. 16 | // Since these combinations correspond to the bit table of an int 17 | // going from 0 to 7, we can use: 18 | std::bitset<3> bit_table_row; 19 | for (unsigned int i = 0; i < 8; i++) { 20 | bit_table_row = std::bitset<3>(i); 21 | box_corners(0, i) = bit_table_row[0] ? min.x() : max.x(); 22 | box_corners(1, i) = bit_table_row[1] ? min.y() : max.y(); 23 | box_corners(2, i) = bit_table_row[2] ? min.z() : max.z(); 24 | } 25 | return box_corners; 26 | } 27 | 28 | const BoundingBox BoundingBox::getAabbFromObbAndPose( 29 | const BoundingBox& obb, const voxblox::Transformation& pose) { 30 | // Create AABB 31 | BoundingBox aabb; 32 | // Transform the OBB corners into mission frame and update the AABB 33 | const BoxCornerMatrix submap_t_submap__obb_corners = 34 | obb.getCornerCoordinates(); 35 | for (int i = 0; i < 8; i++) { 36 | const voxblox::Point mission_t_mission__obb_corner = 37 | pose * submap_t_submap__obb_corners.col(i); 38 | aabb.min = aabb.min.cwiseMin(mission_t_mission__obb_corner); 39 | aabb.max = aabb.max.cwiseMax(mission_t_mission__obb_corner); 40 | } 41 | return aabb; 42 | } 43 | } // namespace voxgraph 44 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/visualization/loop_closure_visuals.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_VISUALIZATION_LOOP_CLOSURE_VISUALS_H_ 2 | #define VOXGRAPH_TOOLS_VISUALIZATION_LOOP_CLOSURE_VISUALS_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "voxgraph/backend/pose_graph.h" 9 | 10 | namespace voxgraph { 11 | class LoopClosureVisuals { 12 | public: 13 | struct Color { 14 | Color() : r(0), g(0), b(0), a(0) {} 15 | Color(float _r, float _g, float _b) : Color(_r, _g, _b, 1.0) {} 16 | Color(float _r, float _g, float _b, float _a) 17 | : r(_r), g(_g), b(_b), a(_a) {} 18 | 19 | float r; 20 | float g; 21 | float b; 22 | float a; 23 | }; 24 | 25 | LoopClosureVisuals() = default; 26 | 27 | void publishLoopClosure(const Transformation& T_W_1, 28 | const Transformation& T_W_2, 29 | const Transformation& T_1_2_est, 30 | const std::string& frame_id, 31 | const ros::Publisher& publisher) const; 32 | 33 | visualization_msgs::Marker getLinkMarker(const Transformation& T_W_1, 34 | const Transformation& T_W_2, 35 | const std::string& frame_id, 36 | const Color& color, 37 | const std::string& ns) const; 38 | 39 | void publishAxes(const Transformation& T_W_1, const Transformation& T_W_2, 40 | const Transformation& T_1_2_est, const std::string& frame_id, 41 | const ros::Publisher& publisher) const; 42 | }; 43 | } // namespace voxgraph 44 | 45 | #endif // VOXGRAPH_TOOLS_VISUALIZATION_LOOP_CLOSURE_VISUALS_H_ 46 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/map_tracker/transformers/tf_transformer.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_MAP_TRACKER_TRANSFORMERS_TF_TRANSFORMER_H_ 2 | #define VOXGRAPH_FRONTEND_MAP_TRACKER_TRANSFORMERS_TF_TRANSFORMER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "voxgraph/common.h" 11 | 12 | namespace voxgraph { 13 | class TfTransformer { 14 | public: 15 | TfTransformer() 16 | : tf_buffer_(ros::Duration(10.0)), 17 | tf_listener_(tf_buffer_), 18 | transform_lookup_retry_period_(0.02), 19 | transform_lookup_max_time_(0.25) {} 20 | 21 | // Method that waits for a transform to become available, while doing less 22 | // agressive polling that ROS's standard tf2_ros::Buffer::canTransform(...) 23 | bool waitForTransform(const std::string& to_frame_id, 24 | const std::string& from_frame_id, 25 | const ros::Time& frame_timestamp); 26 | 27 | // Method to lookup transforms and convert them Kindr 28 | bool lookupTransform(const std::string& to_frame_id, 29 | const std::string& from_frame_id, 30 | const ros::Time& frame_timestamp, 31 | Transformation* transform); 32 | 33 | private: 34 | tf2_ros::Buffer tf_buffer_; 35 | tf2_ros::TransformListener tf_listener_; 36 | 37 | // Transform lookup timers 38 | // Timeout between each update attempt 39 | const ros::WallDuration transform_lookup_retry_period_; 40 | // Maximum time to wait before giving up 41 | const ros::WallDuration transform_lookup_max_time_; 42 | }; 43 | } // namespace voxgraph 44 | 45 | #endif // VOXGRAPH_FRONTEND_MAP_TRACKER_TRANSFORMERS_TF_TRANSFORMER_H_ 46 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/constraint_collection.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_CONSTRAINT_COLLECTION_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_CONSTRAINT_COLLECTION_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/backend/constraint/absolute_pose_constraint.h" 7 | #include "voxgraph/backend/constraint/registration_constraint.h" 8 | #include "voxgraph/backend/constraint/relative_pose_constraint.h" 9 | 10 | namespace voxgraph { 11 | class ConstraintCollection { 12 | public: 13 | void addAbsolutePoseConstraint(const AbsolutePoseConstraint::Config& config) { 14 | absolute_pose_constraints_.emplace_back(newConstraintId(), config); 15 | } 16 | void addRelativePoseConstraint(const RelativePoseConstraint::Config& config) { 17 | relative_pose_constraints_.emplace_back(newConstraintId(), config); 18 | } 19 | void addRegistrationConstraint(const RegistrationConstraint::Config& config) { 20 | registration_constraints_.emplace_back(newConstraintId(), config); 21 | } 22 | 23 | void resetRegistrationConstraints() { registration_constraints_.clear(); } 24 | 25 | void addConstraintsToProblem(const NodeCollection& node_collection, 26 | ceres::Problem* problem_ptr, 27 | bool exclude_registration_constraints = false); 28 | 29 | private: 30 | Constraint::ConstraintId constraint_id_counter_ = 0; 31 | Constraint::ConstraintId newConstraintId() { 32 | return constraint_id_counter_++; 33 | } 34 | 35 | std::list absolute_pose_constraints_; 36 | std::list relative_pose_constraints_; 37 | std::list registration_constraints_; 38 | }; 39 | } // namespace voxgraph 40 | 41 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_CONSTRAINT_COLLECTION_H_ 42 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/registration_constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_REGISTRATION_CONSTRAINT_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_REGISTRATION_CONSTRAINT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxgraph/backend/constraint/constraint.h" 8 | #include "voxgraph/backend/constraint/cost_functions/registration_cost_function.h" 9 | #include "voxgraph/frontend/submap_collection/voxgraph_submap.h" 10 | 11 | namespace voxgraph { 12 | class RegistrationConstraint : public Constraint { 13 | public: 14 | typedef std::shared_ptr Ptr; 15 | struct Config : Constraint::Config { 16 | cblox::SubmapID first_submap_id; 17 | cblox::SubmapID second_submap_id; 18 | VoxgraphSubmap::ConstPtr first_submap_ptr; 19 | VoxgraphSubmap::ConstPtr second_submap_ptr; 20 | RegistrationCostFunction::Config registration; 21 | }; 22 | 23 | explicit RegistrationConstraint(ConstraintId constraint_id, 24 | const Config& config) 25 | : Constraint(constraint_id, config), config_(config) { 26 | // Check whether both submap pointers have been provided 27 | CHECK_NOTNULL(config_.first_submap_ptr); 28 | CHECK_NOTNULL(config_.second_submap_ptr); 29 | 30 | // Registration constraints do not yet support non-identity information 31 | // matrices, therefore assert this 32 | CHECK(sqrt_information_matrix_.isIdentity()) 33 | << "Registration constraint information matrices that differ " 34 | "from the identity matrix are not yet supported."; 35 | } 36 | 37 | void addToProblem(const NodeCollection& node_collection, 38 | ceres::Problem* problem) final; 39 | 40 | private: 41 | const Config config_; 42 | }; 43 | } // namespace voxgraph 44 | 45 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_REGISTRATION_CONSTRAINT_H_ 46 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/data_servers/loop_closure_edge_server.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_DATA_SERVERS_LOOP_CLOSURE_EDGE_SERVER_H_ 2 | #define VOXGRAPH_TOOLS_DATA_SERVERS_LOOP_CLOSURE_EDGE_SERVER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 8 | 9 | namespace voxgraph { 10 | class LoopClosureEdgeServer { 11 | public: 12 | explicit LoopClosureEdgeServer(ros::NodeHandle nh_private, 13 | bool verbose = false); 14 | 15 | // Publish the map using this map server instance's ros publisher member 16 | void publishLoopClosureEdges( 17 | const PoseGraphInterface& pose_graph_interface, 18 | const VoxgraphSubmapCollection& submap_collection, 19 | const ros::Time& timestamp); 20 | 21 | // "Bring your own publisher" method 22 | // NOTE: This method is provided s.t. it can be called using publishers to 23 | // custom topics and without requiring a LoopClosureEdgeServer instance. 24 | // It is therefore static. 25 | static void publishLoopClosureEdges( 26 | const PoseGraphInterface& pose_graph_interface, 27 | const VoxgraphSubmapCollection& submap_collection, 28 | const ros::Time& timestamp, 29 | const ros::Publisher& loop_closure_edge_list_publisher, 30 | bool verbose = false); 31 | 32 | private: 33 | bool verbose_; 34 | static constexpr bool kFake6dofTransforms = true; 35 | static constexpr double kSetUnknownCovarianceEntriesTo = 1e4; 36 | 37 | ros::Publisher loop_closure_edge_list_pub_; 38 | 39 | // Convenience methods to generate the message header 40 | static std_msgs::Header generateHeaderMsg(const ros::Time& timestamp); 41 | }; 42 | } // namespace voxgraph 43 | 44 | #endif // VOXGRAPH_TOOLS_DATA_SERVERS_LOOP_CLOSURE_EDGE_SERVER_H_ 45 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/evaluation/map_evaluation.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_EVALUATION_MAP_EVALUATION_H_ 2 | #define VOXGRAPH_TOOLS_EVALUATION_MAP_EVALUATION_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 11 | #include "voxgraph/tools/visualization/submap_visuals.h" 12 | 13 | namespace voxgraph { 14 | class MapEvaluation { 15 | public: 16 | struct EvaluationDetails { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | voxblox::utils::VoxelEvaluationDetails reconstruction; 19 | voxblox::Transformation T_ground_truth__reading; 20 | }; 21 | 22 | MapEvaluation(const ros::NodeHandle& nh, 23 | const std::string& ground_truth_tsdf_file_path); 24 | 25 | MapEvaluation::EvaluationDetails evaluate( 26 | const VoxgraphSubmapCollection& submap_collection); 27 | 28 | // Find and apply the best rigid body alignment of layer A to B 29 | void alignSubmapAtoSubmapB(VoxgraphSubmap::Ptr submap_A, 30 | VoxgraphSubmap::ConstPtr submap_B); 31 | 32 | private: 33 | using TsdfVoxel = voxblox::TsdfVoxel; 34 | using TsdfLayer = voxblox::Layer; 35 | using EsdfLayer = voxblox::Layer; 36 | using VoxelEvaluationMode = voxblox::utils::VoxelEvaluationMode; 37 | using PointcloudMsg = pcl::PointCloud; 38 | 39 | ros::NodeHandle node_handle_; 40 | 41 | // Ground Truth map 42 | VoxgraphSubmap::Ptr ground_truth_map_ptr_; 43 | 44 | // ROS publishers for visualization purposes 45 | ros::Publisher ground_truth_layer_pub_; 46 | ros::Publisher ground_truth_mesh_pub_; 47 | ros::Publisher rmse_error_pub_; 48 | ros::Publisher rmse_error_slice_pub_; 49 | }; 50 | } // namespace voxgraph 51 | 52 | #endif // VOXGRAPH_TOOLS_EVALUATION_MAP_EVALUATION_H_ 53 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | ColumnLimit: 80 5 | DerivePointerAlignment: false 6 | PointerAlignment: Left 7 | IncludeBlocks: Regroup 8 | IncludeCategories: 9 | # The the main header for a source file is automatically assigned priority 0 10 | # C system headers 11 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 12 | Priority: 1 13 | # C++ system headers 14 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 15 | Priority: 2 16 | # Other libraries' .h files 17 | - Regex: '^<' 18 | Priority: 3 19 | # Your project's .h files 20 | - Regex: '^"' 21 | Priority: 4 22 | --- 23 | Language: Proto 24 | BasedOnStyle: Google 25 | ... 26 | -------------------------------------------------------------------------------- /voxgraph/src/backend/constraint/registration_constraint.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/constraint/registration_constraint.h" 2 | 3 | #include "voxgraph/backend/constraint/cost_functions/registration_cost_function.h" 4 | 5 | namespace voxgraph { 6 | void RegistrationConstraint::addToProblem(const NodeCollection& node_collection, 7 | ceres::Problem* problem) { 8 | CHECK_NOTNULL(problem); 9 | 10 | ceres::LossFunction* loss_function = kNoRobustLossFunction; 11 | 12 | // Get pointers to both submap nodes 13 | SubmapNode::Ptr first_submap_node_ptr = 14 | node_collection.getSubmapNodePtrById(config_.first_submap_id); 15 | SubmapNode::Ptr second_submap_node_ptr = 16 | node_collection.getSubmapNodePtrById(config_.second_submap_id); 17 | CHECK_NOTNULL(first_submap_node_ptr); 18 | CHECK_NOTNULL(second_submap_node_ptr); 19 | 20 | // Add the submap parameters to the problem 21 | first_submap_node_ptr->addToProblem( 22 | problem, node_collection.getLocalParameterization()); 23 | second_submap_node_ptr->addToProblem( 24 | problem, node_collection.getLocalParameterization()); 25 | 26 | // Create submap registration cost function 27 | ceres::CostFunction* cost_function; 28 | if (config_.registration.jacobian_evaluation_method == 29 | RegistrationCostFunction::JacobianEvaluationMethod::kNumeric) { 30 | cost_function = nullptr; 31 | LOG(FATAL) << "Numeric cost not yet implemented"; 32 | } else { 33 | cost_function = new RegistrationCostFunction(config_.first_submap_ptr, 34 | config_.second_submap_ptr, 35 | config_.registration); 36 | } 37 | 38 | // Add the constraint to the optimization and keep track of it 39 | residual_block_id_ = problem->AddResidualBlock( 40 | cost_function, loss_function, 41 | first_submap_node_ptr->getPosePtr()->optimizationVectorData(), 42 | second_submap_node_ptr->getPosePtr()->optimizationVectorData()); 43 | } 44 | } // namespace voxgraph 45 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/map_tracker/transformers/odometry_transformer.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_MAP_TRACKER_TRANSFORMERS_ODOMETRY_TRANSFORMER_H_ 2 | #define VOXGRAPH_FRONTEND_MAP_TRACKER_TRANSFORMERS_ODOMETRY_TRANSFORMER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "voxgraph/common.h" 12 | 13 | namespace voxgraph { 14 | class OdometryTransformer { 15 | public: 16 | OdometryTransformer() 17 | : odometry_queue_length_(200), 18 | transform_lookup_retry_period_(0.02), 19 | transform_lookup_max_time_(0.25) {} 20 | 21 | inline bool canTransform(const ros::Time& timestamp) { 22 | return (!odometry_queue_.empty() && 23 | timestamp < (--odometry_queue_.end())->second->header.stamp); 24 | } 25 | 26 | // Method that waits for a transform to become available 27 | bool waitForTransform(const ros::Time& frame_timestamp); 28 | 29 | // Method to lookup transforms and convert them Kindr 30 | bool lookupTransform(const ros::Time& frame_timestamp, 31 | Transformation* transform); 32 | 33 | // Method to lookup the closest full odometry msg 34 | bool lookupOdometryMsg(const ros::Time& frame_timestamp, 35 | nav_msgs::Odometry* odometry_msg); 36 | 37 | void subscribeToTopic(ros::NodeHandle nh, const std::string& odometry_topic); 38 | void odometryCallback(const nav_msgs::Odometry::ConstPtr& odometry_msg); 39 | 40 | private: 41 | ros::Subscriber odometry_subscriber_; 42 | 43 | size_t odometry_queue_length_; 44 | std::map odometry_queue_; 45 | 46 | // Transform lookup timers 47 | // Timeout between each update attempt 48 | const ros::WallDuration transform_lookup_retry_period_; 49 | // Maximum time to wait before giving up 50 | const ros::WallDuration transform_lookup_max_time_; 51 | }; 52 | } // namespace voxgraph 53 | 54 | #endif // VOXGRAPH_FRONTEND_MAP_TRACKER_TRANSFORMERS_ODOMETRY_TRANSFORMER_H_ 55 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_RELATIVE_POSE_COST_FUNCTION_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_RELATIVE_POSE_COST_FUNCTION_H_ 3 | 4 | #include 5 | 6 | #include "voxgraph/backend/constraint/constraint.h" 7 | 8 | namespace voxgraph { 9 | class RelativePoseCostFunction { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | RelativePoseCostFunction( 14 | const voxblox::Transformation& observed_relative_pose, 15 | const Constraint::InformationMatrix& sqrt_information_matrix, 16 | bool verbose = false) 17 | : verbose_(verbose), 18 | observed_relative_translation_( 19 | observed_relative_pose.getPosition().cast()), 20 | observed_relative_yaw_( 21 | static_cast(observed_relative_pose.log()[5])), 22 | sqrt_information_matrix_(sqrt_information_matrix) {} 23 | 24 | template 25 | bool operator()(const T* pose_A, const T* pose_B, T* residuals) const; 26 | 27 | static ceres::CostFunction* Create( 28 | const voxblox::Transformation& observed_relative_pose, 29 | const Constraint::InformationMatrix& sqrt_information_matrix, 30 | bool verbose = false) { 31 | return (new ceres::AutoDiffCostFunction( 32 | new RelativePoseCostFunction(observed_relative_pose, 33 | sqrt_information_matrix, verbose))); 34 | } 35 | 36 | private: 37 | bool verbose_; 38 | 39 | const Eigen::Matrix observed_relative_translation_; 40 | const double observed_relative_yaw_; 41 | const Constraint::InformationMatrix sqrt_information_matrix_; 42 | 43 | template 44 | Eigen::Matrix rotationMatrixFromYaw(T yaw_radians) const; 45 | }; 46 | } // namespace voxgraph 47 | 48 | #include "voxgraph/backend/constraint/cost_functions/relative_pose_cost_function_inl.h" 49 | 50 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_RELATIVE_POSE_COST_FUNCTION_H_ 51 | -------------------------------------------------------------------------------- /voxgraph/src/backend/constraint/constraint.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/backend/constraint/constraint.h" 2 | 3 | namespace voxgraph { 4 | Constraint::Constraint(Constraint::ConstraintId constraint_id, 5 | const Constraint::Config& config) 6 | : constraint_id_(constraint_id) { 7 | if (!config.allow_semi_definite_information_matrix) { 8 | // Compute the square root of the information matrix 9 | // using Eigen's Cholesky LL^T decomposition 10 | Eigen::LLT information_llt(config.information_matrix); 11 | CHECK(information_llt.info() != Eigen::NumericalIssue) 12 | << "The square root of the information matrix could not be computed, " 13 | << "make sure it is symmetric and positive definite: " 14 | << config.information_matrix; 15 | sqrt_information_matrix_ = information_llt.matrixL(); 16 | } else { 17 | // Compute the robust square root of the information matrix 18 | // using Eigen's LDL^T Cholesky decomposition 19 | Eigen::LDLT information_ldlt(config.information_matrix); 20 | CHECK(information_ldlt.info() != Eigen::NumericalIssue) 21 | << "The square root of the information matrix could not be computed," 22 | << "despite using the robust LDL^T Cholesky decomposition, check: " 23 | << config.information_matrix; 24 | CHECK(information_ldlt.isPositive()) 25 | << "The information matrix must be positive semi-definite:" 26 | << config.information_matrix; 27 | 28 | sqrt_information_matrix_ = 29 | information_ldlt.transpositionsP().transpose() * 30 | information_ldlt.matrixL().toDenseMatrix() * 31 | information_ldlt.vectorD().cwiseSqrt().asDiagonal() * 32 | information_ldlt.transpositionsP(); 33 | // NOTE: The first permutation term (transpositionsP.transpose) could 34 | // be left out, since it cancels once the residual is squared. 35 | // However, we leave it in such that the sqrt_information_matrix_ 36 | // looks familiar to users debugging intermediate steps. 37 | } 38 | } 39 | } // namespace voxgraph 40 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/odometry_simulator/odometry_simulator.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_ODOMETRY_SIMULATOR_ODOMETRY_SIMULATOR_H_ 2 | #define VOXGRAPH_TOOLS_ODOMETRY_SIMULATOR_ODOMETRY_SIMULATOR_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "voxgraph/tools/odometry_simulator/normal_distribution.h" 13 | 14 | namespace voxgraph { 15 | class OdometrySimulator { 16 | public: 17 | explicit OdometrySimulator(const ros::NodeHandle& nh, 18 | const ros::NodeHandle& nh_private); 19 | ~OdometrySimulator() = default; 20 | 21 | void odometryCallback(const nav_msgs::Odometry::ConstPtr& odometry_msg); 22 | 23 | private: 24 | typedef kindr::minimal::RotationQuaternionTemplate Rotation; 25 | typedef Eigen::Matrix Vector3; 26 | 27 | // Internal pose = integrate(odom + noise on X_vel, Y_vel, Z_vel and Yaw_rate) 28 | geometry_msgs::PoseStamped internal_pose_; 29 | // Published pose = internal_pose_ + noise on X, Y, Z, Yaw, Pitch and Roll 30 | geometry_msgs::TransformStamped published_pose_; 31 | 32 | // Debug mode (determines whether to publish the true pose) 33 | bool debug_; 34 | 35 | // ROS Node handles 36 | ros::NodeHandle nh_; 37 | ros::NodeHandle nh_private_; 38 | 39 | // Odometry subscriber and related settings 40 | ros::Subscriber odometry_subscriber_; 41 | int subscriber_queue_length_; 42 | std::string subscribe_to_odom_topic_; 43 | std::string published_mission_frame_; 44 | std::string published_simulated_base_frame_; 45 | std::string published_original_base_frame_; 46 | 47 | // Noise distributions 48 | struct NoiseDistributions { 49 | NormalDistribution x, y, z; 50 | NormalDistribution yaw, pitch, roll; 51 | NormalDistribution x_vel, y_vel, z_vel; 52 | NormalDistribution yaw_rate; 53 | } noise_; 54 | 55 | // Transform publisher for the simulated noisy pose 56 | void publishSimulatedPoseTf(); 57 | // Transform publisher for the true, noise-free pose 58 | void publishOriginalPoseTf(const nav_msgs::Odometry::ConstPtr& odometry_msg); 59 | }; 60 | } // namespace voxgraph 61 | 62 | #endif // VOXGRAPH_TOOLS_ODOMETRY_SIMULATOR_ODOMETRY_SIMULATOR_H_ 63 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/pose_graph.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_POSE_GRAPH_H_ 2 | #define VOXGRAPH_BACKEND_POSE_GRAPH_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "voxgraph/backend/constraint/constraint_collection.h" 11 | #include "voxgraph/backend/node/node_collection.h" 12 | #include "voxgraph/common.h" 13 | 14 | namespace voxgraph { 15 | class PoseGraph { 16 | public: 17 | typedef std::shared_ptr ConstPtr; 18 | typedef std::list SolverSummaryList; 19 | typedef std::map PoseMap; 20 | 21 | PoseGraph() = default; 22 | 23 | void addSubmapNode(const SubmapNode::Config& config); 24 | bool hasSubmapNode(const SubmapNode::SubmapId& submap_id); 25 | void addReferenceFrameNode(const ReferenceFrameNode::Config& config); 26 | bool hasReferenceFrameNode(const ReferenceFrameNode::FrameId& frame_id); 27 | 28 | void addAbsolutePoseConstraint(const AbsolutePoseConstraint::Config& config); 29 | void addRelativePoseConstraint(const RelativePoseConstraint::Config& config); 30 | void addRegistrationConstraint(const RegistrationConstraint::Config& config); 31 | 32 | void resetRegistrationConstraints() { 33 | constraints_collection_.resetRegistrationConstraints(); 34 | } 35 | 36 | void initialize(bool exclude_registration_constraints = false); 37 | void optimize(bool exclude_registration_constraints = false); 38 | 39 | PoseMap getSubmapPoses(); 40 | 41 | typedef Eigen::Matrix EdgeCovarianceMatrix; 42 | typedef std::map EdgeCovarianceMap; 43 | bool getEdgeCovarianceMap(EdgeCovarianceMap* edge_covariance_map) const; 44 | 45 | struct VisualizationEdge { 46 | Transformation::Position first_node_position; 47 | Transformation::Position second_node_position; 48 | double residual; 49 | }; 50 | typedef std::vector VisualizationEdgeList; 51 | VisualizationEdgeList getVisualizationEdges() const; 52 | 53 | const SolverSummaryList& getSolverSummaries() const { 54 | return solver_summaries_; 55 | } 56 | 57 | private: 58 | ConstraintCollection constraints_collection_; 59 | NodeCollection node_collection_; 60 | 61 | // Ceres problem 62 | ceres::Problem::Options problem_options_; 63 | std::shared_ptr problem_ptr_; 64 | SolverSummaryList solver_summaries_; 65 | }; 66 | } // namespace voxgraph 67 | 68 | #endif // VOXGRAPH_BACKEND_POSE_GRAPH_H_ 69 | -------------------------------------------------------------------------------- /voxgraph/src/tools/visualization/pose_graph_visuals.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/visualization/pose_graph_visuals.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace voxgraph { 8 | void PoseGraphVisuals::publishPoseGraph(const PoseGraph& pose_graph, 9 | const std::string& frame_id, 10 | const std::string& name_space, 11 | const ros::Publisher& publisher) const { 12 | // Initialize the Rviz marker msg 13 | visualization_msgs::Marker marker; 14 | marker.header.frame_id = frame_id; 15 | marker.header.stamp = ros::Time(); 16 | marker.ns = name_space; 17 | marker.id = 0; 18 | marker.type = visualization_msgs::Marker::LINE_LIST; 19 | marker.action = visualization_msgs::Marker::ADD; 20 | marker.pose.orientation.w = 1.0; // Set to unit quaternion 21 | marker.scale.x = 0.1; 22 | marker.frame_locked = false; 23 | 24 | // Get the costs and endpoints for all pose graph edges 25 | std::vector edges = 26 | pose_graph.getVisualizationEdges(); 27 | 28 | // Assert that the 'infinity' numeric limit is available at compile time, 29 | // such that it can be used below 30 | static_assert(std::numeric_limits::has_infinity, 31 | "Support for " 32 | "std::numeric_limits::infinity() is required"); 33 | 34 | // Compute the maximum cost, for normalization of the edge colors 35 | double max_cost = -std::numeric_limits::infinity(); 36 | for (const PoseGraph::VisualizationEdge& edge : edges) { 37 | if (edge.residual > max_cost) { 38 | max_cost = edge.residual; 39 | } 40 | } 41 | 42 | // Add the edges to the marker 43 | for (const PoseGraph::VisualizationEdge& edge : edges) { 44 | // Add edge endpoints 45 | geometry_msgs::Point point_msg; 46 | tf::pointEigenToMsg(edge.first_node_position.cast(), point_msg); 47 | marker.points.push_back(point_msg); 48 | tf::pointEigenToMsg(edge.second_node_position.cast(), point_msg); 49 | marker.points.push_back(point_msg); 50 | 51 | // Color edge according to residual 52 | std_msgs::ColorRGBA color_msg; 53 | color_msg.a = 1; 54 | color_msg.r = static_cast(edge.residual / max_cost); 55 | // color_msg.r = float(std::log10(edge.residual + 1)); 56 | marker.colors.push_back(color_msg); 57 | marker.colors.push_back(color_msg); 58 | } 59 | 60 | // Publish the marker 61 | publisher.publish(marker); 62 | } 63 | } // namespace voxgraph 64 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/visualization/submap_visuals.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_VISUALIZATION_SUBMAP_VISUALS_H_ 2 | #define VOXGRAPH_TOOLS_VISUALIZATION_SUBMAP_VISUALS_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 16 | 17 | namespace voxgraph { 18 | class SubmapVisuals { 19 | public: 20 | explicit SubmapVisuals(VoxgraphSubmap::Config submap_config, 21 | voxblox::MeshIntegratorConfig mesh_config); 22 | 23 | void setMeshOpacity(float mesh_opacity) { mesh_opacity_ = mesh_opacity; } 24 | 25 | void publishMesh(const voxblox::MeshLayer::Ptr& mesh_layer_ptr, 26 | const std::string& submap_frame, 27 | const ros::Publisher& publisher, 28 | const voxblox::ColorMode& color_mode = 29 | voxblox::ColorMode::kLambertColor) const; 30 | 31 | void publishMesh( 32 | const cblox::SubmapCollection& submap_collection, 33 | const cblox::SubmapID& submap_id, const voxblox::Color& submap_color, 34 | const std::string& submap_frame, const ros::Publisher& publisher) const; 35 | 36 | void publishSeparatedMesh( 37 | const cblox::SubmapCollection& submap_collection, 38 | const std::string& mission_frame, const ros::Publisher& publisher); 39 | 40 | void publishCombinedMesh( 41 | const cblox::SubmapCollection& submap_collection, 42 | const std::string& mission_frame, const ros::Publisher& publisher); 43 | 44 | void saveSeparatedMesh( 45 | const std::string& filepath, 46 | const cblox::SubmapCollection& submap_collection); 47 | 48 | void saveCombinedMesh( 49 | const std::string& filepath, 50 | const cblox::SubmapCollection& submap_collection); 51 | 52 | void publishBox(const BoxCornerMatrix& box_corner_matrix, 53 | const voxblox::Color& box_color, const std::string& frame_id, 54 | const std::string& name_space, 55 | const ros::Publisher& publisher) const; 56 | 57 | void publishPoseHistory(const VoxgraphSubmapCollection& submap_collection, 58 | const std::string& mission_frame, 59 | const ros::Publisher& publisher) const; 60 | 61 | private: 62 | voxblox::MeshIntegratorConfig mesh_config_; 63 | std::unique_ptr separated_submap_mesher_; 64 | std::unique_ptr combined_submap_mesher_; 65 | 66 | float mesh_opacity_; 67 | }; 68 | } // namespace voxgraph 69 | 70 | #endif // VOXGRAPH_TOOLS_VISUALIZATION_SUBMAP_VISUALS_H_ 71 | -------------------------------------------------------------------------------- /voxgraph/src/tools/data_servers/projected_map_server.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/data_servers/projected_map_server.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace voxgraph { 7 | ProjectedMapServer::ProjectedMapServer(ros::NodeHandle nh_private) { 8 | projected_tsdf_map_pub_ = 9 | nh_private.advertise("projected_map_tsdf", 1, true); 10 | } 11 | 12 | void ProjectedMapServer::publishProjectedMap( 13 | const voxgraph::VoxgraphSubmapCollection& submap_collection, 14 | const ros::Time& timestamp) { 15 | // Only publish if there are subscribers 16 | if (projected_tsdf_map_pub_.getNumSubscribers() > 0) { 17 | publishProjectedMap(submap_collection, timestamp, projected_tsdf_map_pub_); 18 | } 19 | } 20 | 21 | void ProjectedMapServer::publishProjectedMap( 22 | const VoxgraphSubmapCollection& submap_collection, 23 | const ros::Time& timestamp, const ros::Publisher& projected_map_publisher) { 24 | // Create the message and set its headers 25 | cblox_msgs::MapLayer projected_map_tsdf_msg; 26 | projected_map_tsdf_msg.header = generateHeaderMsg(timestamp); 27 | projected_map_tsdf_msg.map_header = generateMapHeaderMsg(submap_collection); 28 | 29 | // Set the message's TSDF 30 | voxblox::serializeLayerAsMsg( 31 | submap_collection.getProjectedMap()->getTsdfLayer(), false, 32 | &projected_map_tsdf_msg.tsdf_layer); 33 | projected_map_tsdf_msg.tsdf_layer.action = 34 | static_cast(voxblox::MapDerializationAction::kReset); 35 | 36 | // Publish 37 | projected_map_publisher.publish(projected_map_tsdf_msg); 38 | } 39 | 40 | std_msgs::Header ProjectedMapServer::generateHeaderMsg( 41 | const ros::Time& timestamp) { 42 | std_msgs::Header msg_header; 43 | // TODO(victorr): Get the world frame name from FrameNames once implemented 44 | msg_header.frame_id = "mission"; 45 | msg_header.stamp = timestamp; 46 | return msg_header; 47 | } 48 | 49 | cblox_msgs::MapHeader ProjectedMapServer::generateMapHeaderMsg( 50 | const VoxgraphSubmapCollection& submap_collection) { 51 | // Set the map ID and type 52 | cblox_msgs::MapHeader map_header; 53 | map_header.id = 0; 54 | map_header.is_submap = false; 55 | 56 | // Set the map's start and end time 57 | if (!submap_collection.empty()) { 58 | map_header.start_time = 59 | submap_collection.getSubmap(submap_collection.getFirstSubmapId()) 60 | .getStartTime(); 61 | map_header.end_time = 62 | submap_collection.getSubmap(submap_collection.getLastSubmapId()) 63 | .getEndTime(); 64 | } else { 65 | map_header.start_time = ros::Time(0.0); 66 | map_header.end_time = ros::Time(0.0); 67 | } 68 | 69 | // Set the pose estimate to zero 70 | // TODO(victorr): Get the world frame name from FrameNames once implemented 71 | map_header.pose_estimate.frame_id = "mission"; 72 | tf::poseKindrToMsg(Transformation().cast(), 73 | &map_header.pose_estimate.map_pose); 74 | 75 | return map_header; 76 | } 77 | } // namespace voxgraph 78 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/frame_names.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_FRAME_NAMES_H_ 2 | #define VOXGRAPH_FRONTEND_FRAME_NAMES_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace voxgraph { 9 | struct FrameNames { 10 | // Coordinate frame naming convention 11 | // - M: Mission (for a single run of a single robot this corresponds to the 12 | // World frame) 13 | // - O: Odometry input frame 14 | // - B: Base link (often corresponds to the robot's IMU frame) 15 | // - C: Sensor frame of the pointcloud sensor 16 | // - S: Active submap (used by the PointcloudIntegrator and MapTracker) 17 | // The full transform input and output chains are O -> B and M -> S -> B -> C 18 | // respectively, where: 19 | // - T_O_B is provided by the odometry input 20 | // - T_M_S corresponds the the optimized pose of the active submap 21 | // - T_S_B corresponds to the pose of the robot in the current submap 22 | // - T_B_C stores the transform from the pointcloud sensor 23 | // to the base_link frame 24 | 25 | // Input frame names 26 | std::string input_odom_frame = "odom"; 27 | std::string input_base_link_frame = "base_link"; 28 | // NOTE: When 'use_tf_transforms' is set to True, the sensor_frame must also 29 | // be defined. But we do not do this in this class since it is already 30 | // given in the header.frame_id field of the PointCloud2 msgs. 31 | 32 | // Output frame names 33 | std::string output_mission_frame = "voxgraph_mission"; 34 | std::string output_odom_frame = "voxgraph_odom"; 35 | std::string output_active_submap_frame = "voxgraph_active_submap"; 36 | std::string output_base_link_frame = "voxgraph_base_link"; 37 | std::string output_sensor_frame = "voxgraph_sensor"; 38 | 39 | // Static method to parse ROS params 40 | static FrameNames fromRosParams(const ros::NodeHandle& node_handle) { 41 | FrameNames frame_names; 42 | // Inputs 43 | node_handle.param("input_odom_frame", frame_names.input_odom_frame, 44 | frame_names.input_odom_frame); 45 | node_handle.param("input_base_link_frame", 46 | frame_names.input_base_link_frame, 47 | frame_names.input_base_link_frame); 48 | 49 | // Outputs 50 | node_handle.param("output_mission_frame", frame_names.output_mission_frame, 51 | frame_names.output_mission_frame); 52 | node_handle.param("output_odom_frame", frame_names.output_odom_frame, 53 | frame_names.output_odom_frame); 54 | node_handle.param("output_active_submap_frame", 55 | frame_names.output_active_submap_frame, 56 | frame_names.output_active_submap_frame); 57 | node_handle.param("output_base_link_frame", 58 | frame_names.output_base_link_frame, 59 | frame_names.output_base_link_frame); 60 | node_handle.param("output_sensor_frame", frame_names.output_sensor_frame, 61 | frame_names.output_sensor_frame); 62 | return frame_names; 63 | } 64 | }; 65 | } // namespace voxgraph 66 | 67 | #endif // VOXGRAPH_FRONTEND_FRAME_NAMES_H_ 68 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap_collection.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_VOXGRAPH_SUBMAP_COLLECTION_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_VOXGRAPH_SUBMAP_COLLECTION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "voxgraph/common.h" 16 | #include "voxgraph/frontend/submap_collection/submap_timeline.h" 17 | #include "voxgraph/frontend/submap_collection/voxgraph_submap.h" 18 | 19 | namespace voxgraph { 20 | class VoxgraphSubmapCollection 21 | : public cblox::SubmapCollection { 22 | public: 23 | typedef std::shared_ptr Ptr; 24 | typedef std::shared_ptr ConstPtr; 25 | typedef std::vector PoseStampedVector; 26 | 27 | explicit VoxgraphSubmapCollection(VoxgraphSubmap::Config submap_config, 28 | bool verbose = false) 29 | : SubmapCollection(submap_config), 30 | verbose_(verbose), 31 | submap_creation_interval_(20) {} 32 | 33 | void setSubmapCreationInterval(ros::Duration submap_creation_interval) { 34 | submap_creation_interval_ = submap_creation_interval; 35 | } 36 | 37 | bool shouldCreateNewSubmap(const ros::Time& current_time); 38 | 39 | // Overriden method that guarantees that the submap gets added to the timeline 40 | void createNewSubmap(const Transformation& T_mission_base, 41 | const ros::Time& timestamp); 42 | 43 | // Delete the inherited methods to avoid accidental calls 44 | void createNewSubmap(const Transformation& T_M_S, 45 | const SubmapID submap_id) = delete; 46 | SubmapID createNewSubmap(const Transformation& T_M_S) = delete; 47 | 48 | SubmapID getPreviousSubmapId() const { 49 | return submap_timeline_.getPreviousSubmapId(); 50 | } 51 | SubmapID getFirstSubmapId() const { 52 | return submap_timeline_.getFirstSubmapId(); 53 | } 54 | SubmapID getLastSubmapId() const { 55 | return submap_timeline_.getLastSubmapId(); 56 | } 57 | 58 | bool lookupActiveSubmapByTime(const ros::Time& timestamp, 59 | SubmapID* submap_id) { 60 | return submap_timeline_.lookupActiveSubmapByTime(timestamp, submap_id); 61 | } 62 | 63 | PoseStampedVector getPoseHistory() const; 64 | 65 | // Create a gravity aligned poses, as used for the submap origins 66 | // NOTE: The submap origin poses must have zero pitch and roll since 67 | // the pose graph optimization only operates in 4D (x, y, z and yaw). 68 | static Transformation gravityAlignPose(const Transformation& input_pose); 69 | 70 | private: 71 | bool verbose_; 72 | 73 | // Length of the time interval between the creation of subsequent submaps 74 | ros::Duration submap_creation_interval_; // In seconds 75 | 76 | // Timeline to enable lookups of submaps by time 77 | SubmapTimeline submap_timeline_; 78 | }; 79 | } // namespace voxgraph 80 | 81 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_VOXGRAPH_SUBMAP_COLLECTION_H_ 82 | -------------------------------------------------------------------------------- /voxgraph/src/tools/submap_registration_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/submap_registration_helper.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "voxgraph/backend/constraint/cost_functions/registration_cost_function.h" 8 | 9 | namespace voxgraph { 10 | SubmapRegistrationHelper::SubmapRegistrationHelper( 11 | cblox::SubmapCollection::ConstPtr submap_collection_ptr, 12 | const Options& options) 13 | : submap_collection_ptr_(std::move(submap_collection_ptr)), 14 | options_(options) {} 15 | 16 | bool SubmapRegistrationHelper::testRegistration( 17 | const cblox::SubmapID& reference_submap_id, 18 | const cblox::SubmapID& reading_submap_id, double* mission_pose_reading, 19 | ceres::Solver::Summary* summary) { 20 | // Get shared pointers to the reference and reading submaps 21 | VoxgraphSubmap::ConstPtr reference_submap_ptr = 22 | submap_collection_ptr_->getSubmapConstPtr(reference_submap_id); 23 | VoxgraphSubmap::ConstPtr reading_submap_ptr = 24 | submap_collection_ptr_->getSubmapConstPtr(reading_submap_id); 25 | CHECK_NOTNULL(reference_submap_ptr); 26 | CHECK_NOTNULL(reading_submap_ptr); 27 | 28 | // Create problem and initial conditions 29 | ceres::Problem problem; 30 | ceres::LossFunction* loss_function = nullptr; // No robust loss function 31 | 32 | // Get initial pose of reference submap (not touched by the optimization) 33 | voxblox::Transformation::Vector6 T_vec_ref = 34 | reference_submap_ptr->getPose().log(); 35 | double mission_pose_ref[4] = {T_vec_ref[0], T_vec_ref[1], T_vec_ref[2], 36 | T_vec_ref[5]}; 37 | 38 | // Add the parameter blocks to the optimization 39 | problem.AddParameterBlock(mission_pose_ref, 4); 40 | problem.SetParameterBlockConstant(mission_pose_ref); 41 | problem.AddParameterBlock(mission_pose_reading, 4); 42 | 43 | // Create the submap registration cost function 44 | RegistrationCostFunction* registration_cost_function = 45 | new RegistrationCostFunction(reference_submap_ptr, reading_submap_ptr, 46 | options_.registration); 47 | 48 | // Toggle between analytic and numeric Jacobians 49 | ceres::CostFunction* ceres_cost_function; 50 | if (options_.registration.jacobian_evaluation_method == 51 | RegistrationCostFunction::JacobianEvaluationMethod::kNumeric) { 52 | // Wrap the registration cost function in a numeric diff cost function, 53 | // which only requests residuals and calculates the Jacobians numerically 54 | ceres_cost_function = new ceres::NumericDiffCostFunction< 55 | RegistrationCostFunction, ceres::CENTRAL, ceres::DYNAMIC, 4, 4>( 56 | registration_cost_function, ceres::TAKE_OWNERSHIP, 57 | registration_cost_function->num_residuals()); 58 | } else { 59 | // Let Ceres use the registration cost function's analytic Jacobians 60 | ceres_cost_function = registration_cost_function; 61 | } 62 | 63 | // Add the cost function to the problem 64 | problem.AddResidualBlock(ceres_cost_function, loss_function, mission_pose_ref, 65 | mission_pose_reading); 66 | 67 | // Run the solver 68 | ceres::Solve(options_.solver, &problem, summary); 69 | 70 | return summary->IsSolutionUsable(); 71 | } 72 | } // namespace voxgraph 73 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/map_tracker/transformers/odometry_transformer.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/map_tracker/transformers/odometry_transformer.h" 2 | 3 | #include 4 | 5 | namespace voxgraph { 6 | bool OdometryTransformer::waitForTransform(const ros::Time& frame_timestamp) { 7 | // Total time spent waiting for the updated pose 8 | ros::WallDuration t_waited(0.0); 9 | while (t_waited < transform_lookup_max_time_) { 10 | if (canTransform(frame_timestamp)) { 11 | return true; 12 | } 13 | transform_lookup_retry_period_.sleep(); 14 | t_waited += transform_lookup_retry_period_; 15 | ros::spinOnce(); 16 | } 17 | ROS_WARN( 18 | "Waited %.3fs, but still could not get an odometry transform for " 19 | "timestamp %.3fs", 20 | t_waited.toSec(), frame_timestamp.toSec()); 21 | return false; 22 | } 23 | 24 | bool OdometryTransformer::lookupTransform(const ros::Time& frame_timestamp, 25 | Transformation* transform) { 26 | CHECK_NOTNULL(transform); 27 | if (!waitForTransform(frame_timestamp)) { 28 | return false; 29 | } 30 | auto iterator = odometry_queue_.upper_bound(frame_timestamp); 31 | 32 | // Ensure that the timestamp is not from before the buffer 33 | if (iterator == odometry_queue_.begin()) { 34 | ROS_WARN( 35 | "Requested transform from odom topic whose timstamp falls before " 36 | "any msg in the buffer."); 37 | return false; 38 | } 39 | 40 | // TODO(victorr): Implement interpolation using the exponential map 41 | // The interval's active submap id is stored at its start point 42 | iterator--; 43 | 44 | TransformationD transform_double; 45 | tf::poseMsgToKindr(iterator->second->pose.pose, &transform_double); 46 | *transform = transform_double.cast(); 47 | return true; 48 | } 49 | 50 | bool OdometryTransformer::lookupOdometryMsg(const ros::Time& frame_timestamp, 51 | nav_msgs::Odometry* odometry_msg) { 52 | CHECK_NOTNULL(odometry_msg); 53 | 54 | if (!waitForTransform(frame_timestamp)) { 55 | return false; 56 | } 57 | auto iterator = odometry_queue_.upper_bound(frame_timestamp); 58 | 59 | // Ensure that the timestamp is not from before the buffer 60 | if (iterator == odometry_queue_.begin()) { 61 | ROS_WARN( 62 | "Requested transform from odom topic whose timstamp falls before " 63 | "any msg in the buffer."); 64 | return false; 65 | } 66 | 67 | // TODO(victorr): Implement interpolation using the exponential map 68 | // The interval's active submap id is stored at its start point 69 | iterator--; 70 | 71 | *odometry_msg = *iterator->second; 72 | 73 | return true; 74 | } 75 | 76 | void OdometryTransformer::subscribeToTopic(ros::NodeHandle nh, 77 | const std::string& odometry_topic) { 78 | odometry_subscriber_ = nh.subscribe( 79 | odometry_topic, 1, &OdometryTransformer::odometryCallback, this); 80 | } 81 | 82 | void OdometryTransformer::odometryCallback( 83 | const nav_msgs::Odometry::ConstPtr& odometry_msg) { 84 | if (odometry_queue_.size() >= odometry_queue_length_ && 85 | !odometry_queue_.empty()) { 86 | odometry_queue_.erase(odometry_queue_.begin()); 87 | } 88 | odometry_queue_.emplace(odometry_msg->header.stamp, odometry_msg); 89 | } 90 | } // namespace voxgraph 91 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/tools/data_servers/submap_server.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_TOOLS_DATA_SERVERS_SUBMAP_SERVER_H_ 2 | #define VOXGRAPH_TOOLS_DATA_SERVERS_SUBMAP_SERVER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "voxgraph/common.h" 10 | #include "voxgraph/frontend/frame_names.h" 11 | #include "voxgraph/frontend/submap_collection/voxgraph_submap.h" 12 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 13 | 14 | namespace voxgraph { 15 | class SubmapServer { 16 | public: 17 | explicit SubmapServer(ros::NodeHandle nh_private); 18 | 19 | // Publish maps using the publishers that are members of this server instance 20 | void publishSubmap(const VoxgraphSubmap& submap, const ros::Time& timestamp); 21 | void publishSubmapTsdf(const VoxgraphSubmap& submap, 22 | const ros::Time& timestamp); 23 | void publishSubmapTsdfAndEsdf(const VoxgraphSubmap& submap, 24 | const ros::Time& timestamp); 25 | void publishSubmapSurfacePointcloud(const VoxgraphSubmap& submap, 26 | const ros::Time& timestamp); 27 | void publishSubmapPoses( 28 | const VoxgraphSubmapCollection::Ptr& submap_collection_ptr, 29 | const ros::Time& timestamp); 30 | void publishActiveSubmap( 31 | const VoxgraphSubmapCollection::Ptr& submap_collection_ptr, 32 | const ros::Time& current_timestamp); 33 | 34 | // "Bring your own publisher" methods 35 | // NOTE: These methods are provided s.t. they can be called using publishers 36 | // to custom topics and without requiring a SubmapServer instance. 37 | // They are therefore static. 38 | static void publishSubmapTsdf(const VoxgraphSubmap& submap, 39 | const std::string& frame_id, 40 | const ros::Time& timestamp, 41 | const ros::Publisher& submap_tsdf_publisher); 42 | static void publishSubmapTsdfAndEsdf( 43 | const VoxgraphSubmap& submap, const std::string& frame_id, 44 | const ros::Time& timestamp, const ros::Publisher& submap_esdf_publisher); 45 | static void publishSubmapSurfacePointcloud( 46 | const VoxgraphSubmap& submap, const std::string& frame_id, 47 | const ros::Time& timestamp, 48 | const ros::Publisher& submap_surface_pointcloud_publisher); 49 | static void publishSubmapPoses( 50 | const VoxgraphSubmapCollection::Ptr& submap_collection_ptr, 51 | const std::string& frame_id, const ros::Time& timestamp, 52 | const ros::Publisher& submap_poses_publisher); 53 | 54 | private: 55 | ros::Publisher submap_tsdf_pub_; 56 | ros::Publisher submap_esdf_pub_; 57 | ros::Publisher submap_surface_pointcloud_pub_; 58 | ros::Publisher submap_poses_pub_; 59 | 60 | static constexpr bool fake_6dof_transforms_ = true; 61 | 62 | FrameNames frame_names_; 63 | 64 | // Conversion method from Kindr transforms to Eigen Affine3f transforms 65 | static void transformKindrToEigen(const Transformation& kindr, 66 | Eigen::Affine3f* eigen) { 67 | CHECK_NOTNULL(eigen); 68 | *eigen = 69 | Eigen::Translation3f(kindr.getPosition()) * kindr.getEigenQuaternion(); 70 | } 71 | }; 72 | } // namespace voxgraph 73 | 74 | #endif // VOXGRAPH_TOOLS_DATA_SERVERS_SUBMAP_SERVER_H_ 75 | -------------------------------------------------------------------------------- /voxgraph/src/tools/visualization/loop_closure_visuals.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/visualization/loop_closure_visuals.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace voxgraph { 11 | void LoopClosureVisuals::publishLoopClosure( 12 | const Transformation& T_M_1, const Transformation& T_M_2, 13 | const Transformation& T_1_2_est, const std::string& frame_id, 14 | const ros::Publisher& publisher) const { 15 | visualization_msgs::MarkerArray marker_array; 16 | // Getting links 17 | static const Color kColorOld = Color(0.0f, 0.0f, 1.0f, 1.0f); 18 | static const Color kColorEst = Color(1.0f, 0.0f, 0.0f, 1.0f); 19 | visualization_msgs::Marker old_link = 20 | getLinkMarker(T_M_1, T_M_2, frame_id, kColorOld, "old_link"); 21 | const Transformation T_M_2_est = T_M_1 * T_1_2_est; 22 | visualization_msgs::Marker new_link = 23 | getLinkMarker(T_M_1, T_M_2_est, frame_id, kColorEst, "new_link"); 24 | // Publishing 25 | marker_array.markers.push_back(old_link); 26 | marker_array.markers.push_back(new_link); 27 | publisher.publish(marker_array); 28 | } 29 | 30 | visualization_msgs::Marker LoopClosureVisuals::getLinkMarker( 31 | const Transformation& T_M_1, const Transformation& T_M_2, 32 | const std::string& frame_id, const Color& color, 33 | const std::string& ns) const { 34 | // Creating the marker 35 | visualization_msgs::Marker marker; 36 | marker.header.frame_id = frame_id; 37 | marker.type = visualization_msgs::Marker::LINE_LIST; 38 | marker.action = visualization_msgs::Marker::ADD; 39 | marker.scale.x = 0.05; 40 | marker.scale.y = 0.0; 41 | marker.scale.z = 0.0; 42 | marker.color.a = 1.0; 43 | marker.color.r = color.r; 44 | marker.color.g = color.g; 45 | marker.color.b = color.b; 46 | marker.ns = ns; 47 | // Extracting the matching frame end points 48 | // Adding the marker msg 49 | geometry_msgs::Point T_M_1_point_msg; 50 | geometry_msgs::Point T_M_2_point_msg; 51 | tf::pointEigenToMsg(T_M_1.getPosition().cast(), T_M_1_point_msg); 52 | tf::pointEigenToMsg(T_M_2.getPosition().cast(), T_M_2_point_msg); 53 | marker.points.push_back(T_M_1_point_msg); 54 | marker.points.push_back(T_M_2_point_msg); 55 | return marker; 56 | } 57 | 58 | void LoopClosureVisuals::publishAxes(const Transformation& T_M_1, 59 | const Transformation& T_M_2, 60 | const Transformation& T_1_2_est, 61 | const std::string& frame_id, 62 | const ros::Publisher& publisher) const { 63 | // Calculating the estimate transform 64 | const Transformation T_M_2_est = T_M_1 * T_1_2_est; 65 | // Publish 66 | geometry_msgs::PoseArray pose_array_msg; 67 | pose_array_msg.header.frame_id = frame_id; 68 | geometry_msgs::Pose T_M_1_msg; 69 | geometry_msgs::Pose T_M_2_msg; 70 | geometry_msgs::Pose T_M_2_est_msg; 71 | tf::poseKindrToMsg(T_M_1.cast(), &T_M_1_msg); 72 | tf::poseKindrToMsg(T_M_2.cast(), &T_M_2_msg); 73 | tf::poseKindrToMsg(T_M_2_est.cast(), &T_M_2_est_msg); 74 | pose_array_msg.poses.push_back(T_M_1_msg); 75 | pose_array_msg.poses.push_back(T_M_2_msg); 76 | pose_array_msg.poses.push_back(T_M_2_est_msg); 77 | publisher.publish(pose_array_msg); 78 | } 79 | } // namespace voxgraph 80 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/map_tracker/map_tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_MAP_TRACKER_MAP_TRACKER_H_ 2 | #define VOXGRAPH_FRONTEND_MAP_TRACKER_MAP_TRACKER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "voxgraph/common.h" 12 | #include "voxgraph/frontend/frame_names.h" 13 | #include "voxgraph/frontend/map_tracker/transformers/odometry_transformer.h" 14 | #include "voxgraph/frontend/map_tracker/transformers/tf_transformer.h" 15 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 16 | 17 | namespace voxgraph { 18 | class MapTracker { 19 | public: 20 | explicit MapTracker(VoxgraphSubmapCollection::ConstPtr submap_collection_ptr, 21 | FrameNames frame_names, bool verbose = false); 22 | 23 | void subscribeToTopics(ros::NodeHandle nh, 24 | const std::string& odometry_input_topic); 25 | void advertiseTopics(ros::NodeHandle nh_private, 26 | const std::string& odometry_output_topic); 27 | 28 | bool updateToTime(const ros::Time& timestamp, 29 | const std::string& sensor_frame_id); 30 | void switchToNewSubmap(const Transformation& T_M_S_new); 31 | 32 | void publishTFs(); 33 | 34 | // Transform getter methods 35 | // NOTE: For more info on the frame naming conventions, 36 | // see the FrameNames class 37 | Transformation get_T_M_B(); 38 | Transformation get_T_O_B() { return T_O_B_; } 39 | Transformation get_T_S_B() { return T_S_B_; } 40 | Transformation get_T_S_C() { return T_S_B_ * T_B_C_; } 41 | 42 | void set_T_B_C(const Transformation& T_B_C); 43 | 44 | const FrameNames& getFrameNames() const { return frame_names_; } 45 | 46 | // Config get/setters 47 | void setVerbosity(bool verbose) { verbose_ = verbose; } 48 | 49 | private: 50 | bool verbose_; 51 | 52 | // Pointer to the submap collection containing the one that is being tracked 53 | VoxgraphSubmapCollection::ConstPtr submap_collection_ptr_; 54 | 55 | // Timestamp at which the MapTracker was last updated 56 | ros::Time current_timestamp_; 57 | 58 | // Coordinate frame names 59 | // NOTE: This class is used to translate frame names between voxgraph 60 | // and the other ROS nodes 61 | FrameNames frame_names_; 62 | 63 | // Transform that stores the current raw odometry input 64 | Transformation T_O_B_; 65 | 66 | // Transform from the odom origin to the pose at which 67 | // the current submap was created 68 | // NOTE: This transform is used to convert the pose from the odometry input 69 | // into the coordinate frame of the current submap 70 | Transformation initial_T_S_O_; 71 | Transformation initial_T_M_S_; // for visualization purposes only 72 | 73 | // Transform that tracks the odometry in submap frame 74 | Transformation T_S_B_; 75 | 76 | // Transform from the pointcloud sensor frame to the robot's base_link 77 | Transformation T_B_C_; 78 | 79 | // Transformer class to lookup transforms from the TF tree or an odom topic 80 | bool use_odom_from_tfs_ = true; 81 | // NOTE: use_odom_tfs_ is automatically set to true if subscribeToTopics() 82 | // is called with a non-empty odometry_input_topic argument 83 | bool use_sensor_calibration_from_tfs_ = true; 84 | // NOTE: use_sensor_calibration_from_tfs_ is automatically set to 85 | // false if a calibration is provided through method set_T_B_C() 86 | TfTransformer tf_transformer_; 87 | OdometryTransformer odom_transformer_; 88 | }; 89 | } // namespace voxgraph 90 | 91 | #endif // VOXGRAPH_FRONTEND_MAP_TRACKER_MAP_TRACKER_H_ 92 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/cost_functions/registration_cost_function.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_REGISTRATION_COST_FUNCTION_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_REGISTRATION_COST_FUNCTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxgraph/frontend/submap_collection/voxgraph_submap.h" 8 | #include "voxgraph/tools/visualization/cost_function_visuals.h" 9 | 10 | namespace voxgraph { 11 | class RegistrationCostFunction : public ceres::CostFunction { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | enum class JacobianEvaluationMethod { kAnalytic = 0, kNumeric }; 16 | 17 | struct Config { 18 | // What type of reference submap points are used 19 | // to register it to the reading submap 20 | VoxgraphSubmap::RegistrationPointType registration_point_type = 21 | VoxgraphSubmap::RegistrationPointType::kIsosurfacePoints; 22 | 23 | // What method to use to calculate the Jacobians 24 | JacobianEvaluationMethod jacobian_evaluation_method = 25 | JacobianEvaluationMethod::kAnalytic; 26 | 27 | // Sampling ratio, set to -1 to disable down sampling 28 | float sampling_ratio = -1; 29 | 30 | // Cost to assign for voxels that can't be interpolated 31 | // i.e. that don't exist in the other submap 32 | double no_correspondence_cost = 0; 33 | 34 | // Whether to use the TSDF or ESDF distance 35 | bool use_esdf_distance = true; 36 | 37 | // Visuals for debugging purposes 38 | bool visualize_residuals = false; 39 | bool visualize_gradients = false; 40 | bool visualize_transforms_ = false; 41 | }; 42 | 43 | RegistrationCostFunction(VoxgraphSubmap::ConstPtr reference_submap_ptr, 44 | VoxgraphSubmap::ConstPtr reading_submap_ptr, 45 | const Config& config); 46 | 47 | bool Evaluate(double const* const* parameters, double* residuals, 48 | double** jacobians) const override; 49 | 50 | protected: 51 | Config config_; 52 | 53 | // Pointers and const refs to the reading submap that will be aligned 54 | VoxgraphSubmap::ConstPtr reading_submap_ptr_; 55 | const voxblox::Layer& reading_tsdf_layer_; 56 | const voxblox::Layer& reading_esdf_layer_; 57 | 58 | // Reference to registration point sampler 59 | // NOTE: These points are voxels or isosurface vertices 60 | // of the reference submap 61 | const WeightedSampler& registration_points_; 62 | 63 | // Interpolators 64 | voxblox::Interpolator tsdf_interpolator_; 65 | voxblox::Interpolator esdf_interpolator_; 66 | 67 | // Used for residual and Jacobian visualization 68 | mutable CostFunctionVisuals cost_function_visuals_; 69 | 70 | // This matrix is used to interpolate voxels 71 | // It corresponds to matrix B_1 from paper: http://spie.org/samples/PM159.pdf 72 | // clang-format off 73 | const voxblox::InterpTable interp_table_ = 74 | (voxblox::InterpTable() << 1, 0, 0, 0, 0, 0, 0, 0, 75 | -1, 0, 0, 0, 1, 0, 0, 0, 76 | -1, 0, 1, 0, 0, 0, 0, 0, 77 | -1, 1, 0, 0, 0, 0, 0, 0, 78 | 1, 0, -1, 0, -1, 0, 1, 0, 79 | 1, -1, -1, 1, 0, 0, 0, 0, 80 | 1, -1, 0, 0, -1, 1, 0, 0, 81 | -1, 1, 1, -1, 1, -1, -1, 1).finished(); 82 | // clang-format on 83 | }; 84 | } // namespace voxgraph 85 | 86 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_REGISTRATION_COST_FUNCTION_H_ 87 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/pose_graph_interface/pose_graph_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_POSE_GRAPH_INTERFACE_H_ 2 | #define VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_POSE_GRAPH_INTERFACE_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "voxgraph/backend/pose_graph.h" 8 | #include "voxgraph/common.h" 9 | #include "voxgraph/frontend/pose_graph_interface/measurement_templates.h" 10 | #include "voxgraph/frontend/pose_graph_interface/node_templates.h" 11 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 12 | #include "voxgraph/tools/visualization/pose_graph_visuals.h" 13 | #include "voxgraph/tools/visualization/submap_visuals.h" 14 | 15 | namespace voxgraph { 16 | class PoseGraphInterface { 17 | public: 18 | typedef std::vector OverlappingSubmapList; 19 | 20 | explicit PoseGraphInterface( 21 | ros::NodeHandle node_handle, 22 | VoxgraphSubmapCollection::Ptr submap_collection_ptr, 23 | voxblox::MeshIntegratorConfig mesh_config, 24 | const std::string& visualizations_mission_frame, bool verbose = false); 25 | 26 | void setVerbosity(bool verbose) { verbose_ = verbose; } 27 | void setMeasurementConfigFromRosParams(const ros::NodeHandle& node_handle) { 28 | measurement_templates_.setFromRosParams(node_handle); 29 | } 30 | 31 | void addSubmap(SubmapID submap_id); 32 | 33 | // Method to recalculate which submaps overlap and update their 34 | // registration constraints accordingly 35 | void updateRegistrationConstraints(); 36 | 37 | // NOTE: The pose graph optimization works in 4D. Therefore the 38 | // pitch and roll components of T_S1_S2 are simply ignored 39 | // by the RelativePoseCostFunction. 40 | void addOdometryMeasurement(const SubmapID& first_submap_id, 41 | const SubmapID& second_submap_id, 42 | const Transformation& T_S1_S2); 43 | void addLoopClosureMeasurement(const SubmapID& from_submap, 44 | const SubmapID& to_submap, 45 | const Transformation& transform); 46 | void addGpsMeasurement() {} 47 | void addHeightMeasurement(const SubmapID& submap_id, const double& height); 48 | 49 | void optimize(); 50 | 51 | void updateSubmapCollectionPoses(); 52 | 53 | const OverlappingSubmapList& getOverlappingSubmapList() const { 54 | return overlapping_submap_list_; 55 | } 56 | 57 | bool getEdgeCovarianceMap( 58 | PoseGraph::EdgeCovarianceMap* edge_covariance_map_ptr) const; 59 | 60 | const PoseGraph::SolverSummaryList& getSolverSummaries() const { 61 | return pose_graph_.getSolverSummaries(); 62 | } 63 | 64 | private: 65 | bool verbose_; 66 | 67 | VoxgraphSubmapCollection::Ptr submap_collection_ptr_; 68 | 69 | // Pose graph and visualization tools 70 | const std::string visualization_mission_frame_; 71 | PoseGraph pose_graph_; 72 | SubmapVisuals submap_vis_; 73 | PoseGraphVisuals pose_graph_vis_; 74 | ros::Publisher pose_graph_pub_; 75 | ros::Publisher submap_pub_; 76 | 77 | // Node and measurement config templates 78 | NodeTemplates node_templates_; 79 | MeasurementTemplates measurement_templates_; 80 | 81 | // Keep track of which submaps overlap 82 | OverlappingSubmapList overlapping_submap_list_; 83 | void updateOverlappingSubmapList(); 84 | 85 | // Keep track of whether new loop closures have been added 86 | // since the pose graph was last optimized 87 | bool new_loop_closures_added_since_last_optimization_; 88 | 89 | // Helper to add reference frames to the pose graph 90 | void addReferenceFrameIfMissing(ReferenceFrameNode::FrameId frame_id); 91 | }; 92 | } // namespace voxgraph 93 | 94 | #endif // VOXGRAPH_FRONTEND_POSE_GRAPH_INTERFACE_POSE_GRAPH_INTERFACE_H_ 95 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/measurement_processors/pointcloud_integrator.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/measurement_processors/pointcloud_integrator.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace voxgraph { 11 | PointcloudIntegrator::PointcloudIntegrator(bool verbose) 12 | : verbose_(verbose), color_map_(new voxblox::GrayscaleColorMap()) { 13 | // Configure the color map 14 | color_map_->setMaxValue(10000.0); 15 | } 16 | 17 | void PointcloudIntegrator::setTsdfIntegratorConfigFromRosParam( 18 | const ros::NodeHandle& node_handle) { 19 | tsdf_integrator_config_ = 20 | voxblox::getTsdfIntegratorConfigFromRosParam(node_handle); 21 | } 22 | 23 | void PointcloudIntegrator::integratePointcloud( 24 | const sensor_msgs::PointCloud2::Ptr& pointcloud_msg, 25 | const voxblox::Transformation& T_submap_sensor, 26 | VoxgraphSubmap* submap_ptr) { 27 | CHECK_NOTNULL(submap_ptr); 28 | 29 | // Convert the pointcloud msg into a voxblox::Pointcloud 30 | voxblox::Pointcloud pointcloud; 31 | voxblox::Colors colors; 32 | // Detect the PCL pointcloud type 33 | bool color_pointcloud = false; 34 | bool has_intensity = false; 35 | for (size_t d = 0; d < pointcloud_msg->fields.size(); ++d) { 36 | if (pointcloud_msg->fields[d].name == std::string("rgb")) { 37 | color_pointcloud = true; 38 | // Quick hack to fix PCL color parsing 39 | pointcloud_msg->fields[d].datatype = sensor_msgs::PointField::FLOAT32; 40 | } else if (pointcloud_msg->fields[d].name == std::string("intensity")) { 41 | has_intensity = true; 42 | } 43 | } 44 | // Convert differently depending on RGB or I type 45 | if (color_pointcloud) { 46 | pcl::PointCloud pointcloud_pcl; 47 | // pointcloud_pcl is modified below: 48 | pcl::fromROSMsg(*pointcloud_msg, pointcloud_pcl); 49 | voxblox::convertPointcloud(pointcloud_pcl, color_map_, &pointcloud, 50 | &colors); 51 | } else if (has_intensity) { 52 | pcl::PointCloud pointcloud_pcl; 53 | // pointcloud_pcl is modified below: 54 | pcl::fromROSMsg(*pointcloud_msg, pointcloud_pcl); 55 | voxblox::convertPointcloud(pointcloud_pcl, color_map_, &pointcloud, 56 | &colors); 57 | } else { 58 | pcl::PointCloud pointcloud_pcl; 59 | // pointcloud_pcl is modified below: 60 | pcl::fromROSMsg(*pointcloud_msg, pointcloud_pcl); 61 | voxblox::convertPointcloud(pointcloud_pcl, color_map_, &pointcloud, 62 | &colors); 63 | } 64 | 65 | // Initialize the TSDF integrator if this has not yet been done 66 | if (!tsdf_integrator_) { 67 | tsdf_integrator_.reset(new voxblox::FastTsdfIntegrator( 68 | tsdf_integrator_config_, 69 | submap_ptr->getTsdfMapPtr()->getTsdfLayerPtr())); 70 | ROS_INFO("Initialized TSDF Integrator"); 71 | } 72 | 73 | // TODO(victorr): Implement optional Cartographer style simultaneous 74 | // integration into multiple submaps for guaranteed overlap 75 | 76 | // Point the integrator to the current submap 77 | tsdf_integrator_->setLayer(submap_ptr->getTsdfMapPtr()->getTsdfLayerPtr()); 78 | 79 | // Integrate the pointcloud (and report timings if requested) 80 | ROS_INFO_COND(verbose_, "Integrating a pointcloud with %lu points.", 81 | pointcloud.size()); 82 | ros::WallTime start = ros::WallTime::now(); 83 | tsdf_integrator_->integratePointCloud(T_submap_sensor, pointcloud, colors); 84 | ros::WallTime end = ros::WallTime::now(); 85 | ROS_INFO_COND( 86 | verbose_, 87 | "Finished integrating in %f seconds, submap %u now has %lu blocks.", 88 | (end - start).toSec(), submap_ptr->getID(), 89 | submap_ptr->getTsdfMap().getTsdfLayer().getNumberOfAllocatedBlocks()); 90 | } 91 | } // namespace voxgraph 92 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function_inl.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_RELATIVE_POSE_COST_FUNCTION_INL_H_ 2 | #define VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_RELATIVE_POSE_COST_FUNCTION_INL_H_ 3 | 4 | #include "voxgraph/backend/local_parameterization/normalize_angle.h" 5 | 6 | namespace voxgraph { 7 | template 8 | bool RelativePoseCostFunction::operator()(const T* const pose_A, 9 | const T* const pose_B, 10 | T* residuals) const { 11 | const Eigen::Matrix t_mission_A(pose_A[0], pose_A[1], pose_A[2]); 12 | const Eigen::Matrix t_mission_B(pose_B[0], pose_B[1], pose_B[2]); 13 | const T yaw_mission_A(pose_A[3]); 14 | const T yaw_mission_B(pose_B[3]); 15 | CHECK_NEAR(yaw_mission_A, T(0), T(M_PI)); 16 | CHECK_NEAR(yaw_mission_B, T(0), T(M_PI)); 17 | 18 | Eigen::Map> residuals_map(residuals); 19 | 20 | // Compute translation error 21 | residuals_map.template head<3>() = 22 | rotationMatrixFromYaw(yaw_mission_A).transpose() * 23 | (t_mission_B - t_mission_A) - 24 | observed_relative_translation_.cast(); 25 | 26 | // Compute yaw error and normalize the angle 27 | residuals_map(3) = NormalizeAngle((yaw_mission_B - yaw_mission_A) - 28 | static_cast(observed_relative_yaw_)); 29 | 30 | if (verbose_) { 31 | std::cout << "t_mission_A: " << t_mission_A(0) << ", " << t_mission_A(1) 32 | << ", " << t_mission_A(2) << "\n" 33 | << "t_mission_B: " << t_mission_B(0) << ", " << t_mission_B(1) 34 | << ", " << t_mission_B(2) << "\n" 35 | << "Rotated(t_mission_B - t_mission_A):\n" 36 | << (rotationMatrixFromYaw(yaw_mission_A).transpose() * 37 | (t_mission_B - t_mission_A))(0) 38 | << "\n" 39 | << (rotationMatrixFromYaw(yaw_mission_A).transpose() * 40 | (t_mission_B - t_mission_A))(1) 41 | << "\n" 42 | << (rotationMatrixFromYaw(yaw_mission_A).transpose() * 43 | (t_mission_B - t_mission_A))(2) 44 | << "\n" 45 | << "observed_relative_translation: " 46 | << observed_relative_translation_.cast()(0) << ", " 47 | << observed_relative_translation_.cast()(1) << ", " 48 | << observed_relative_translation_.cast()(2) << "\n" 49 | << "yaw_mission_A: " << yaw_mission_A << "\n" 50 | << "yaw_mission_B: " << yaw_mission_B << "\n" 51 | << "observed_relative_yaw: " 52 | << static_cast(observed_relative_yaw_) << "\n" 53 | << "Error:" << residuals_map(0) << ", " << residuals_map(1) 54 | << ", " << residuals_map(2) << ", " << residuals_map(3) << "\n" 55 | << std::endl; 56 | } 57 | 58 | // Scale the residuals by the square root information matrix to account for 59 | // the measurement uncertainty 60 | residuals_map = sqrt_information_matrix_.template cast() * residuals_map; 61 | 62 | if (verbose_) { 63 | std::cout << "Error scaled by information: " << residuals_map(0) << ", " 64 | << residuals_map(1) << ", " << residuals_map(2) << ", " 65 | << residuals_map(3) << "\n" 66 | << "------------------------------------------" << std::endl; 67 | } 68 | 69 | return true; 70 | } 71 | 72 | template 73 | Eigen::Matrix RelativePoseCostFunction::rotationMatrixFromYaw( 74 | T yaw_radians) const { 75 | const T cos_yaw = ceres::cos(yaw_radians); 76 | const T sin_yaw = ceres::sin(yaw_radians); 77 | const T zero = static_cast(0); 78 | const T one = static_cast(1); 79 | 80 | Eigen::Matrix rotation_matrix; 81 | rotation_matrix << cos_yaw, -sin_yaw, zero, sin_yaw, cos_yaw, zero, zero, 82 | zero, one; 83 | return rotation_matrix; 84 | } 85 | } // namespace voxgraph 86 | 87 | #endif // VOXGRAPH_BACKEND_CONSTRAINT_COST_FUNCTIONS_RELATIVE_POSE_COST_FUNCTION_INL_H_ 88 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/submap_collection/voxgraph_submap_collection.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "voxgraph/tools/tf_helper.h" 8 | 9 | namespace voxgraph { 10 | bool VoxgraphSubmapCollection::shouldCreateNewSubmap( 11 | const ros::Time& current_time) { 12 | if (empty()) { 13 | ROS_INFO_COND(verbose_, 14 | "Submap collection is empty. " 15 | "Should create first submap."); 16 | return true; 17 | } else { 18 | // TODO(victorr): Add options to also consider distance traveled etc 19 | ros::Time new_submap_creation_deadline = 20 | getActiveSubmap().getStartTime() + submap_creation_interval_; 21 | ROS_INFO_STREAM_COND(verbose_, 22 | "Current time: " << current_time << "\n" 23 | << "New creation submap deadline: " 24 | << new_submap_creation_deadline); 25 | return current_time > new_submap_creation_deadline; 26 | } 27 | } 28 | 29 | // Creates a gravity aligned new submap 30 | void VoxgraphSubmapCollection::createNewSubmap( 31 | const Transformation& T_mission_base, const ros::Time& timestamp) { 32 | // Define the new submap frame to be at the current robot pose 33 | // and have its Z-axis aligned with gravity 34 | Transformation T_mission__new_submap = gravityAlignPose(T_mission_base); 35 | 36 | // Create the new submap 37 | SubmapID new_submap_id = 38 | cblox::SubmapCollection::createNewSubmap( 39 | T_mission__new_submap); 40 | 41 | ROS_INFO_STREAM("Created submap: " << new_submap_id << " with pose\n" 42 | << T_mission__new_submap); 43 | 44 | // Add the new submap to the timeline 45 | submap_timeline_.addNextSubmap(timestamp, new_submap_id); 46 | } 47 | 48 | VoxgraphSubmapCollection::PoseStampedVector 49 | VoxgraphSubmapCollection::getPoseHistory() const { 50 | PoseStampedVector poses; 51 | // Iterate over all submaps 52 | for (const VoxgraphSubmap::ConstPtr& submap_ptr : getSubmapConstPtrs()) { 53 | // Iterate over all poses in the submap 54 | for (const std::pair& time_pose_pair : 55 | submap_ptr->getPoseHistory()) { 56 | geometry_msgs::PoseStamped pose_stamped_msg; 57 | pose_stamped_msg.header.stamp = time_pose_pair.first; 58 | // Transform the pose from submap frame into mission frame 59 | const Transformation pose = submap_ptr->getPose() * time_pose_pair.second; 60 | tf::poseKindrToMsg(pose.cast(), &pose_stamped_msg.pose); 61 | poses.emplace_back(pose_stamped_msg); 62 | } 63 | } 64 | return poses; 65 | } 66 | 67 | Transformation VoxgraphSubmapCollection::gravityAlignPose( 68 | const Transformation& input_pose) { 69 | // Use the logarithmic map to get the pose's [x, y, z, r, p, y] components 70 | Transformation::Vector6 T_vec = input_pose.log(); 71 | 72 | // Print a warning if the original pitch & roll components were large 73 | constexpr float angle_threshold_rad = 30.f /* deg */ / 180.f * M_PI; 74 | if (std::abs(T_vec[3]) > angle_threshold_rad) { 75 | ROS_WARN_STREAM_THROTTLE( 76 | 1, "New submap creation called with proposed roll: " 77 | << T_vec[3] 78 | << "[rad]. This most likely isn't problematic, but keep in mind " 79 | "that voxgraph only optimizes over XYZ and Yaw. So please " 80 | "make sure that voxgraph's odometry input is in a coordinate " 81 | "system whose Z-axis is roughly gravity aligned."); 82 | } 83 | if (std::abs(T_vec[4]) > angle_threshold_rad) { 84 | ROS_WARN_STREAM_THROTTLE( 85 | 1, "New submap creation called with proposed pitch: " 86 | << T_vec[4] 87 | << "[rad]. This most likely isn't problematic, but keep in mind " 88 | "that voxgraph only optimizes over XYZ and Yaw. So please " 89 | "make sure that voxgraph's odometry input is in a coordinate " 90 | "system whose Z-axis is roughly gravity aligned."); 91 | } 92 | 93 | // Set the roll and pitch to zero 94 | T_vec[3] = 0; 95 | T_vec[4] = 0; 96 | 97 | // Return the gravity aligned pose as a translation + quaternion, 98 | // using the exponential map 99 | return Transformation::exp(T_vec); 100 | } 101 | } // namespace voxgraph 102 | -------------------------------------------------------------------------------- /voxgraph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(voxgraph) 3 | 4 | add_definitions(-std=c++14 -Wall -Wextra -Wno-unused-parameter -Wno-sign-compare -Wno-deprecated-copy -fPIC -DEIGEN_INITIALIZE_MATRICES_BY_NAN -fdiagnostics-color=auto) 5 | 6 | find_package(catkin_simple REQUIRED) 7 | catkin_simple(ALL_DEPS_REQUIRED) 8 | 9 | ############ 10 | # PROTOBUF # 11 | ############ 12 | # General idea: first check if we have protobuf catkin, then use that. 13 | # Otherwise use system protobuf. 14 | set(PROTO_LIBRARIES "") 15 | find_package(protobuf_catkin QUIET) 16 | if (protobuf_catkin_FOUND) 17 | message(STATUS "Using protobuf_catkin") 18 | set(PROTO_LIBRARIES ${protobuf_catkin_LIBRARIES}) 19 | else() 20 | message(STATUS "Using system protobuf") 21 | find_package(Protobuf REQUIRED) 22 | set(PROTO_LIBRARIES ${PROTOBUF_LIBRARIES}) 23 | endif() 24 | 25 | ############# 26 | # LIBRARIES # 27 | ############# 28 | # NOTE: The library is static to avoid segfaults that 29 | # otherwise occur in voxblox multi threading 30 | cs_add_library(${PROJECT_NAME} STATIC 31 | src/frontend/voxgraph_mapper.cpp 32 | src/frontend/pose_graph_interface/pose_graph_interface.cpp 33 | src/frontend/pose_graph_interface/node_templates.cpp 34 | src/frontend/pose_graph_interface/measurement_templates.cpp 35 | src/frontend/measurement_processors/pointcloud_integrator.cpp 36 | src/frontend/submap_collection/bounding_box.cpp 37 | src/frontend/submap_collection/submap_timeline.cpp 38 | src/frontend/submap_collection/voxgraph_submap.cpp 39 | src/frontend/submap_collection/voxgraph_submap_collection.cpp 40 | src/frontend/map_tracker/map_tracker.cpp 41 | src/frontend/map_tracker/transformers/tf_transformer.cpp 42 | src/frontend/map_tracker/transformers/odometry_transformer.cpp 43 | src/backend/pose_graph.cpp 44 | src/backend/node/node.cpp 45 | src/backend/node/node_collection.cpp 46 | src/backend/node/pose/pose_4d.cpp 47 | src/backend/node/pose/pose_6d.cpp 48 | src/backend/constraint/constraint.cpp 49 | src/backend/constraint/constraint_collection.cpp 50 | src/backend/constraint/relative_pose_constraint.cpp 51 | src/backend/constraint/absolute_pose_constraint.cpp 52 | src/backend/constraint/registration_constraint.cpp 53 | src/backend/constraint/cost_functions/registration_cost_function.cpp 54 | src/tools/rosbag_helper.cpp 55 | src/tools/tf_helper.cpp 56 | src/tools/submap_registration_helper.cpp 57 | src/tools/evaluation/map_evaluation.cpp 58 | src/tools/data_servers/projected_map_server.cpp 59 | src/tools/data_servers/submap_server.cpp 60 | src/tools/data_servers/loop_closure_edge_server.cpp 61 | src/tools/odometry_simulator/odometry_simulator.cpp 62 | src/tools/visualization/cost_function_visuals.cpp 63 | src/tools/visualization/pose_graph_visuals.cpp 64 | src/tools/visualization/submap_visuals.cpp 65 | src/tools/visualization/loop_closure_visuals.cpp 66 | ) 67 | 68 | ############ 69 | # BINARIES # 70 | ############ 71 | cs_add_executable(voxgraph_mapping_node 72 | src/voxgraph_mapping_node.cpp) 73 | target_link_libraries(voxgraph_mapping_node ${PROJECT_NAME} ${PROTO_LIBRARIES}) 74 | 75 | cs_add_executable(odometry_simulator 76 | src/odometry_simulator_node.cpp) 77 | target_link_libraries(odometry_simulator ${PROJECT_NAME} ${PROTO_LIBRARIES}) 78 | 79 | cs_add_executable(registration_test_bench 80 | src/tools/test_benches/registration_test_bench.cpp) 81 | target_link_libraries(registration_test_bench ${PROJECT_NAME} ${PROTO_LIBRARIES}) 82 | 83 | ########## 84 | # EXPORT # 85 | ########## 86 | cs_export() 87 | 88 | #################### 89 | # GIT VERSION INFO # 90 | #################### 91 | # Make the git branch name and git commit hash available in the sources as 92 | # a #define, such that it can easily be appended to logs 93 | IF(EXISTS ${voxgraph_SOURCE_DIR}/../.git) 94 | find_package(Git) 95 | IF(GIT_FOUND) 96 | execute_process( 97 | COMMAND ${GIT_EXECUTABLE} rev-parse --abbrev-ref HEAD 98 | WORKING_DIRECTORY "${voxgraph_SOURCE_DIR}" 99 | OUTPUT_VARIABLE "voxgraph_GIT_BRANCH" 100 | ERROR_QUIET 101 | OUTPUT_STRIP_TRAILING_WHITESPACE) 102 | execute_process( 103 | COMMAND ${GIT_EXECUTABLE} log -1 --format=%h 104 | WORKING_DIRECTORY "${voxgraph_SOURCE_DIR}" 105 | OUTPUT_VARIABLE "voxgraph_GIT_COMMIT_HASH" 106 | ERROR_QUIET 107 | OUTPUT_STRIP_TRAILING_WHITESPACE) 108 | message(STATUS "Defining GIT_BRANCH: ${voxgraph_GIT_BRANCH}") 109 | message(STATUS "Defining GIT_COMMIT_HASH: ${voxgraph_GIT_COMMIT_HASH}") 110 | ELSE(GIT_FOUND) 111 | set(voxgraph_GIT_BRANCH 0) 112 | set(voxgraph_GIT_COMMIT_HASH 0) 113 | ENDIF(GIT_FOUND) 114 | ENDIF(EXISTS ${voxgraph_SOURCE_DIR}/../.git) 115 | add_definitions("-DGIT_BRANCH=\"${voxgraph_GIT_BRANCH}\"") 116 | add_definitions("-DGIT_COMMIT_HASH=\"${voxgraph_GIT_COMMIT_HASH}\"") 117 | -------------------------------------------------------------------------------- /voxgraph/src/tools/visualization/cost_function_visuals.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/visualization/cost_function_visuals.h" 2 | 3 | namespace voxgraph { 4 | CostFunctionVisuals::CostFunctionVisuals() { 5 | // Advertise the residual visualization pointcloud and Jacobian vector field 6 | ros::NodeHandle nh_private("~"); 7 | residual_ptcloud_pub_ = nh_private.advertise>( 8 | "cost_residuals", 1, true); 9 | jacobians_pub_ = nh_private.advertise( 10 | "cost_jacobians", 1, true); 11 | 12 | // Configure the residual visualization pointcloud 13 | residual_ptcloud_.header.frame_id = "mission"; 14 | 15 | // The magnitudes and orientations of the Jacobians 16 | // are visualized as a list of lines 17 | jacobian_arrows_.header.stamp = ros::Time::now(); 18 | jacobian_arrows_.header.frame_id = "mission"; 19 | jacobian_arrows_.ns = "jacobian_vectors"; 20 | jacobian_arrows_.id = 1; 21 | jacobian_arrows_.type = visualization_msgs::Marker::LINE_LIST; 22 | jacobian_arrows_.action = visualization_msgs::Marker::ADD; 23 | jacobian_arrows_.pose.orientation.w = 1.0; // Set to unit quaternion 24 | jacobian_arrows_.scale.x = 0.02; 25 | voxblox::colorVoxbloxToMsg(voxblox::Color::Red(), &jacobian_arrows_.color); 26 | jacobian_arrows_.frame_locked = false; 27 | 28 | // To show the direction of the Jacobians, their origins 29 | // are displayed as a list of sphere 30 | jacobian_origins_.header = jacobian_arrows_.header; 31 | jacobian_origins_.ns = "jacobian_origins"; 32 | jacobian_origins_.id = 2; 33 | jacobian_origins_.type = visualization_msgs::Marker::SPHERE_LIST; 34 | jacobian_origins_.action = visualization_msgs::Marker::ADD; 35 | jacobian_origins_.pose.orientation.w = 1.0; // Set to unit quaternion 36 | jacobian_origins_.scale.x = 0.05; 37 | jacobian_origins_.scale.y = 0.05; 38 | jacobian_origins_.scale.z = 0.05; 39 | voxblox::colorVoxbloxToMsg(voxblox::Color::Black(), &jacobian_origins_.color); 40 | jacobian_origins_.frame_locked = false; 41 | } 42 | 43 | void CostFunctionVisuals::addResidual(const voxblox::Point& coordinate, 44 | const double& residual) { 45 | pcl::PointXYZI point; 46 | point.x = coordinate.x(); 47 | point.y = coordinate.y(); 48 | point.z = coordinate.z(); 49 | point.intensity = static_cast(residual); 50 | residual_ptcloud_.push_back(point); 51 | } 52 | 53 | void CostFunctionVisuals::addJacobian(const voxblox::Point& coordinate, 54 | const voxblox::Point& jacobian) { 55 | // Add the current point to the Jacobian origin and arrow vectors 56 | geometry_msgs::Point coordinate_msg; 57 | tf::pointEigenToMsg(coordinate.cast(), coordinate_msg); 58 | jacobian_origins_.points.push_back(coordinate_msg); 59 | jacobian_arrows_.points.push_back(coordinate_msg); 60 | // NOTE: The Jacobian is first stored in jacobian_arrows as is. 61 | // Once all Jacobians are known, they will be normalized and 62 | // converted into representative arrow end points. This happens 63 | // in the scaleAndPublish() method. 64 | geometry_msgs::Point jacobian_vector_msg; 65 | tf::pointEigenToMsg(jacobian.cast(), jacobian_vector_msg); 66 | jacobian_arrows_.points.push_back(jacobian_vector_msg); 67 | } 68 | 69 | void CostFunctionVisuals::scaleAndPublish(const double factor) { 70 | size_t num_residuals = residual_ptcloud_.size(); 71 | size_t num_jacobians = jacobian_origins_.points.size(); 72 | // Scale the residuals 73 | if (num_residuals != 0) { 74 | for (size_t i = 0; i < num_residuals; i++) { 75 | residual_ptcloud_[i].intensity *= factor; 76 | } 77 | } 78 | // Scale the Jacobians 79 | if (num_jacobians != 0) { 80 | for (size_t i = 0; i < num_jacobians; i++) { 81 | // Scale the Jacobians for good visibility in Rviz 82 | jacobian_arrows_.points[1 + i * 2].x *= factor * 0.05; 83 | jacobian_arrows_.points[1 + i * 2].y *= factor * 0.05; 84 | jacobian_arrows_.points[1 + i * 2].z *= factor * 0.05; 85 | // Compute the arrow endpoint 86 | // by summing the arrow origin to the scaled Jacobian vector 87 | jacobian_arrows_.points[1 + i * 2].x += jacobian_origins_.points[i].x; 88 | jacobian_arrows_.points[1 + i * 2].y += jacobian_origins_.points[i].y; 89 | jacobian_arrows_.points[1 + i * 2].z += jacobian_origins_.points[i].z; 90 | } 91 | } 92 | // Publish the residuals and Jacobians, given that they are not empty 93 | if (num_residuals != 0) { 94 | residual_ptcloud_pub_.publish(residual_ptcloud_); 95 | } 96 | if (num_jacobians != 0) { 97 | jacobian_marker_array_.markers.emplace_back(jacobian_arrows_); 98 | jacobian_marker_array_.markers.emplace_back(jacobian_origins_); 99 | jacobians_pub_.publish(jacobian_marker_array_); 100 | } 101 | } 102 | 103 | void CostFunctionVisuals::reset() { 104 | // Reset the residual pointcloud 105 | residual_ptcloud_.clear(); 106 | residual_ptcloud_.header.frame_id = "mission"; 107 | // Reset variables used to visualize the Jacobians 108 | jacobian_marker_array_.markers.clear(); 109 | jacobian_origins_.points.clear(); 110 | jacobian_arrows_.points.clear(); 111 | } 112 | } // namespace voxgraph 113 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/map_tracker/map_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/map_tracker/map_tracker.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "voxgraph/tools/tf_helper.h" 8 | 9 | namespace voxgraph { 10 | MapTracker::MapTracker(VoxgraphSubmapCollection::ConstPtr submap_collection_ptr, 11 | FrameNames frame_names, bool verbose) 12 | : verbose_(verbose), 13 | submap_collection_ptr_(submap_collection_ptr), 14 | frame_names_(std::move(frame_names)), 15 | tf_transformer_(), 16 | odom_transformer_() {} 17 | 18 | void MapTracker::subscribeToTopics(ros::NodeHandle nh, 19 | const std::string& odometry_input_topic) { 20 | if (!odometry_input_topic.empty()) { 21 | ROS_INFO_STREAM("Using odometry from ROS topic: " << odometry_input_topic); 22 | use_odom_from_tfs_ = false; 23 | odom_transformer_.subscribeToTopic(nh, odometry_input_topic); 24 | } 25 | } 26 | 27 | void MapTracker::advertiseTopics(ros::NodeHandle nh_private, 28 | const std::string& odometry_output_topic) {} 29 | 30 | bool MapTracker::updateToTime(const ros::Time& timestamp, 31 | const std::string& sensor_frame_id) { 32 | // Keep track of the timestamp that the MapTracker is currently at 33 | current_timestamp_ = timestamp; 34 | 35 | // Update the odometry 36 | if (use_odom_from_tfs_) { 37 | // Update the odometry estimate 38 | if (!tf_transformer_.lookupTransform(frame_names_.input_odom_frame, 39 | frame_names_.input_base_link_frame, 40 | timestamp, &T_O_B_)) { 41 | return false; 42 | } 43 | } else { 44 | if (!odom_transformer_.lookupTransform(timestamp, &T_O_B_)) { 45 | return false; 46 | } 47 | } 48 | 49 | // Express the odometry pose in the frame of the current submap 50 | T_S_B_ = initial_T_S_O_ * T_O_B_; 51 | 52 | // Get the transformation from the pointcloud sensor to the robot's 53 | // base link from TFs, unless it was already provided through ROS params 54 | if (use_sensor_calibration_from_tfs_) { 55 | // TODO(victorr): Implement option to provide a sensor_frame_id instead of 56 | // taking the one from the message 57 | // Strip leading slashes if needed to avoid TF errors 58 | std::string tf_sensor_frame_id; 59 | if (sensor_frame_id[0] == '/') { 60 | tf_sensor_frame_id = sensor_frame_id.substr(1, sensor_frame_id.length()); 61 | } else { 62 | tf_sensor_frame_id = sensor_frame_id; 63 | } 64 | 65 | // Lookup the transform 66 | if (!tf_transformer_.lookupTransform(frame_names_.input_base_link_frame, 67 | tf_sensor_frame_id, timestamp, 68 | &T_B_C_)) { 69 | return false; 70 | } 71 | } 72 | 73 | // Signal that all transforms were successfully updated 74 | return true; 75 | } 76 | 77 | void MapTracker::switchToNewSubmap(const Transformation& T_M_S_new) { 78 | // Store the initial submap pose for visualization purposes 79 | initial_T_M_S_ = T_M_S_new; 80 | 81 | // Get the pose of the new submap in odom frame 82 | Transformation T_O_S = VoxgraphSubmapCollection::gravityAlignPose(T_O_B_); 83 | 84 | // Store the transform used to convert the odometry input into submap frame 85 | initial_T_S_O_ = T_O_S.inverse(); 86 | 87 | // Update the current robot pose 88 | // NOTE: This initial pose can differ from Identity, since the submap pose 89 | // has zero pitch and roll whereas the robot pose is in 6DoF 90 | T_S_B_ = initial_T_S_O_ * T_O_B_; 91 | } 92 | 93 | void MapTracker::publishTFs() { 94 | TfHelper::publishTransform(submap_collection_ptr_->getActiveSubmapPose(), 95 | frame_names_.output_mission_frame, 96 | frame_names_.output_active_submap_frame, false, 97 | current_timestamp_); 98 | TfHelper::publishTransform( 99 | initial_T_S_O_, frame_names_.output_active_submap_frame, 100 | frame_names_.output_odom_frame, false, current_timestamp_); 101 | 102 | if (frame_names_.input_odom_frame != frame_names_.output_odom_frame || 103 | frame_names_.input_base_link_frame != 104 | frame_names_.output_base_link_frame || 105 | !use_odom_from_tfs_) { 106 | // Republish the odometry if the output frame names are different, 107 | // or if the odom input is coming from a ROS topic 108 | // (in which case it might not yet be in the TF tree) 109 | TfHelper::publishTransform(T_O_B_, frame_names_.output_odom_frame, 110 | frame_names_.output_base_link_frame, false, 111 | current_timestamp_); 112 | } 113 | 114 | TfHelper::publishTransform(T_B_C_, frame_names_.output_base_link_frame, 115 | frame_names_.output_sensor_frame, true, 116 | current_timestamp_); 117 | } 118 | 119 | Transformation MapTracker::get_T_M_B() { 120 | if (submap_collection_ptr_->empty()) { 121 | // If no submap has been created yet, return the odometry pose 122 | return T_O_B_; 123 | } else { 124 | return submap_collection_ptr_->getActiveSubmapPose() * T_S_B_; 125 | } 126 | } 127 | 128 | void MapTracker::set_T_B_C(const Transformation& T_B_C) { 129 | T_B_C_ = T_B_C; 130 | use_sensor_calibration_from_tfs_ = false; 131 | } 132 | } // namespace voxgraph 133 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_VOXGRAPH_SUBMAP_H_ 2 | #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_VOXGRAPH_SUBMAP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "voxgraph/frontend/submap_collection/bounding_box.h" 12 | #include "voxgraph/frontend/submap_collection/registration_point.h" 13 | #include "voxgraph/frontend/submap_collection/weighted_sampler.h" 14 | 15 | namespace voxgraph { 16 | class VoxgraphSubmap : public cblox::TsdfEsdfSubmap { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | typedef std::shared_ptr Ptr; 21 | typedef std::shared_ptr ConstPtr; 22 | // TODO(victorr): Use Eigen aligned map 23 | typedef std::map PoseHistoryMap; 24 | 25 | struct Config : cblox::TsdfEsdfSubmap::Config { 26 | struct RegistrationFilter { 27 | double min_voxel_weight = 1; 28 | double max_voxel_distance = 0.3; 29 | bool use_esdf_distance = true; 30 | } registration_filter; 31 | }; 32 | 33 | VoxgraphSubmap(const voxblox::Transformation& T_M_S, 34 | const cblox::SubmapID& submap_id, const Config& config, 35 | const voxblox::EsdfIntegrator::Config& esdf_integrator_config = 36 | voxblox::EsdfIntegrator::Config()); 37 | 38 | // Create a VoxgraphSubmap based on a COPY of a TsdfLayer 39 | VoxgraphSubmap(const voxblox::Transformation& T_M_S, 40 | const cblox::SubmapID& submap_id, 41 | const voxblox::Layer& tsdf_layer, 42 | const voxblox::EsdfIntegrator::Config& esdf_integrator_config = 43 | voxblox::EsdfIntegrator::Config()); 44 | 45 | // Setter method for the registration filter config 46 | // NOTE: This method is mainly useful for copy or proto constructed submaps 47 | void setRegistrationFilterConfig( 48 | const Config::RegistrationFilter& registration_filter_config); 49 | 50 | const ros::Time getStartTime() const; 51 | const ros::Time getEndTime() const; 52 | 53 | void addPoseToHistory(const ros::Time& timestamp, 54 | const voxblox::Transformation& T_submap_base); 55 | const PoseHistoryMap& getPoseHistory() const { return pose_history_; } 56 | bool lookupPoseByTime(const ros::Time& timestamp, 57 | voxblox::Transformation* T_submap_robot) const; 58 | 59 | // Indicate that the submap is finished and generate all cached members 60 | // NOTE: These cached members are mainly used in the registration cost funcs 61 | void finishSubmap() override; 62 | 63 | void transformSubmap(const voxblox::Transformation& T_new_old); 64 | 65 | // The type of registration points supported by this submap 66 | enum class RegistrationPointType { kIsosurfacePoints = 0, kVoxels }; 67 | // Getter to get the registration points of a certain type 68 | const WeightedSampler& getRegistrationPoints( 69 | RegistrationPointType registration_point_type) const; 70 | 71 | // Overlap and Bounding Box related methods 72 | bool overlapsWith(const VoxgraphSubmap& other_submap) const; 73 | const BoundingBox getSubmapFrameSurfaceObb() const; 74 | const BoundingBox getSubmapFrameSubmapObb() const; 75 | const BoundingBox getMissionFrameSurfaceAabb() const; 76 | const BoundingBox getMissionFrameSubmapAabb() const; 77 | const BoxCornerMatrix getMissionFrameSurfaceObbCorners() const; 78 | const BoxCornerMatrix getMissionFrameSubmapObbCorners() const; 79 | const BoxCornerMatrix getMissionFrameSurfaceAabbCorners() const; 80 | const BoxCornerMatrix getMissionFrameSubmapAabbCorners() const; 81 | 82 | // Load a submap from stream. 83 | // Note: Returns a nullptr if load is unsuccessful. 84 | static VoxgraphSubmap::Ptr LoadFromStream(const Config& config, 85 | std::istream* proto_file_ptr, 86 | uint64_t* tmp_byte_offset_ptr); 87 | 88 | private: 89 | typedef Eigen::Matrix HomogBoxCornerMatrix; 90 | using TsdfVoxel = voxblox::TsdfVoxel; 91 | using EsdfVoxel = voxblox::EsdfVoxel; 92 | using TsdfLayer = voxblox::Layer; 93 | using EsdfLayer = voxblox::Layer; 94 | using TsdfBlock = voxblox::Block; 95 | using EsdfBlock = voxblox::Block; 96 | using Interpolator = voxblox::Interpolator; 97 | 98 | Config config_; 99 | 100 | // Track whether this submap has been declared finished 101 | // and all cached values have been generated 102 | bool finished_ = false; 103 | 104 | // Oriented Bounding Boxes in submap frame 105 | mutable BoundingBox surface_obb_; // around the isosurface 106 | mutable BoundingBox map_obb_; // around the entire submap 107 | 108 | // Object containing all voxels that fall within the truncation distance 109 | // and have a sufficiently high weight 110 | WeightedSampler relevant_voxels_; 111 | void findRelevantVoxelIndices(); 112 | 113 | // Object containing all isosurface vertices stored as [x, y, z, weight] 114 | WeightedSampler isosurface_vertices_; 115 | voxblox::IndexSet isosurface_blocks_; 116 | void findIsosurfaceVertices(); 117 | 118 | // History of how the robot moved through the submap 119 | PoseHistoryMap pose_history_; 120 | }; 121 | } // namespace voxgraph 122 | 123 | #endif // VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_VOXGRAPH_SUBMAP_H_ 124 | -------------------------------------------------------------------------------- /voxgraph/scripts/jacobians_xyz_yaw.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # yapf: disable 3 | 4 | from __future__ import print_function 5 | from sympy import simplify, diff, cse 6 | from sympy import Matrix, MatrixSymbol 7 | from sympy import zeros 8 | from sympy import Symbol, symbols 9 | from sympy import cos, sin 10 | from sympy import init_printing, pprint 11 | init_printing(use_unicode=True) 12 | 13 | def rotationMatrixFromQuaternion(q_0, q_1, q_2, q_3): 14 | return Matrix([[q_0**2 + q_1**2 - q_2**2 - q_3**2, 2*q_1*q_2 - 2*q_0*q_3, 2*q_0*q_2 + 2*q_1*q_3], # pylint: disable=line-too-long 15 | [2*q_0*q_3 + 2*q_1*q_2, q_0**2 - q_1**2 + q_2**2 - q_3**2, 2*q_2*q_3 - 2*q_0*q_1], # pylint: disable=line-too-long 16 | [2*q_1*q_3 - 2*q_0*q_2, 2*q_0*q_1 + 2*q_2*q_3, q_0**2 - q_1**2 - q_2**2 + q_3**2]]) # pylint: disable=line-too-long 17 | 18 | # Settings 19 | print_jacobians = False 20 | print_jacobians_times_reference_coordinate = True 21 | print_common_subexpression_elimination = False 22 | 23 | # Frame and body names 24 | # w: world frame 25 | # o: reference submap frame 26 | # e: reading submap frame 27 | # p: point at which we want to calculate the residual 28 | 29 | # Homogeneous coordinate of the point in the reference submap at which we want 30 | # to calculate the residual 31 | o_x_oi, o_y_oi, o_z_oi = symbols('x_i y_i z_i') 32 | o_r_oi = Matrix([o_x_oi, o_y_oi, o_z_oi, 1]) 33 | 34 | # Translation of the reference submap 35 | x_o, y_o, z_o = symbols('x_o y_o z_o') 36 | w_r_wo = Matrix([x_o, y_o, z_o]) 37 | 38 | # Translation of the reading submap 39 | x_e, y_e, z_e = symbols('x_e y_e z_e') 40 | w_r_we = Matrix([x_e, y_e, z_e]) 41 | 42 | # Rotation matrix of the reference submap 43 | theta_o = Symbol('theta_o') 44 | # C_wo = rotationMatrixFromQuaternion(cos(theta_o/2), 0, 0, sin(theta_o/2)) 45 | C_wo = Matrix([[cos(theta_o), -sin(theta_o), 0], 46 | [sin(theta_o), cos(theta_o), 0], 47 | [0, 0, 1]]) 48 | 49 | # Rotation matrix of the reading submap 50 | theta_e = Symbol('theta_e') 51 | # C_we = rotationMatrixFromQuaternion(cos(theta_e/2), 0, 0, sin(theta_e/2)) 52 | C_we = Matrix([[cos(theta_e), -sin(theta_e), 0], 53 | [sin(theta_e), cos(theta_e), 0], 54 | [0, 0, 1]]) 55 | 56 | # Transform matrix of the reference submap frame to the world frame 57 | T_wo = Matrix(MatrixSymbol('T_wa', 4, 4)) 58 | T_wo[:3, :3] = C_wo 59 | T_wo[:3, 3] = w_r_wo 60 | T_wo[3, :3] = zeros(1, 3) 61 | T_wo[3, 3] = 1 62 | # pprint(T_wo) 63 | 64 | # NOTE: It would have been nicer to use BlockMatrices instead of assigning 65 | # through index ranges, but this kept causing block_collapse errors when 66 | # evaluating the final result 67 | 68 | # Transform matrix from the world frame to the reading submap frame 69 | T_ew = Matrix(MatrixSymbol('T_wb_inv', 4, 4)) 70 | T_ew[:3, :3] = C_we.T 71 | T_ew[:3, 3] = -C_we.T * w_r_we 72 | T_ew[3, :3] = zeros(1, 3) 73 | T_ew[3, 3] = 1 74 | # pprint(T_ew) 75 | 76 | # Transform matrix from the reference submap frame to the reading submap frame 77 | T_eo = simplify(T_ew * T_wo) 78 | # pprint(T_eo) 79 | 80 | # Get the derivatives of interest 81 | if print_jacobians: 82 | print("\nDerivative of T_eo over x_o:") 83 | pprint(diff(T_eo, x_o)) 84 | print("\nDerivative of T_eo over y_o:") 85 | pprint(diff(T_eo, y_o)) 86 | print("\nDerivative of T_eo over z_o:") 87 | pprint(diff(T_eo, z_o)) 88 | print("\nDerivative of T_eo over theta_o:") 89 | pprint(simplify(diff(T_eo, theta_o))) 90 | 91 | print("\nDerivative of T_eo over x_e:") 92 | pprint(diff(T_eo, x_e)) 93 | print("\nDerivative of T_eo over y_e:") 94 | pprint(diff(T_eo, y_e)) 95 | print("\nDerivative of T_eo over z_e:") 96 | pprint(diff(T_eo, z_e)) 97 | print("\nDerivative of T_eo over theta_e:") 98 | pprint(simplify(diff(T_eo, theta_e))) 99 | 100 | # Get the derivatives over interest times the reference_coordinate 101 | if print_jacobians_times_reference_coordinate: 102 | print("\nDerivative of T_eo over x_o, times o_r_oi:") 103 | pprint(diff(T_eo, x_o) * o_r_oi) 104 | print("\nDerivative of T_eo over y_o, times o_r_oi:") 105 | pprint(diff(T_eo, y_o) * o_r_oi) 106 | print("\nDerivative of T_eo over z_o, times o_r_oi:") 107 | pprint(diff(T_eo, z_o) * o_r_oi) 108 | print("\nDerivative of T_eo over theta_o, times o_r_oi:") 109 | pprint(simplify(diff(T_eo, theta_o) * o_r_oi)) 110 | 111 | print("\nDerivative of T_eo over x_e, times o_r_oi:") 112 | pprint(diff(T_eo, x_e) * o_r_oi) 113 | print("\nDerivative of T_eo over y_e, times o_r_oi:") 114 | pprint(diff(T_eo, y_e) * o_r_oi) 115 | print("\nDerivative of T_eo over z_e, times o_r_oi:") 116 | pprint(diff(T_eo, z_e) * o_r_oi) 117 | print("\nDerivative of T_eo over theta_e, times o_r_oi:") 118 | pprint(simplify(diff(T_eo, theta_e) * o_r_oi)) 119 | 120 | # Get the full derivative matrix after Common Subexpression Elimination 121 | if print_common_subexpression_elimination: 122 | print("\nDerivative of T_eo over [x_o, y_o, z_o, theta_o]^T, times o_r_oi:") 123 | pT_eo__pParam_o__roi = Matrix(MatrixSymbol("pTeo_pParam_o__roi", 4, 4)) 124 | pT_eo__pParam_o__roi[:, 0] = diff(T_eo, x_o) * o_r_oi 125 | pT_eo__pParam_o__roi[:, 1] = diff(T_eo, y_o) * o_r_oi 126 | pT_eo__pParam_o__roi[:, 2] = diff(T_eo, z_o) * o_r_oi 127 | pT_eo__pParam_o__roi[:, 3] = simplify(diff(T_eo, theta_o) * o_r_oi) 128 | pprint(pT_eo__pParam_o__roi) 129 | print("--> after CSE:") 130 | pprint(cse(pT_eo__pParam_o__roi)) 131 | 132 | 133 | print("\nDerivative of T_eo over [x_e, y_e, z_e, theta_e]^T, times o_r_oi:") 134 | pT_eo__pParam_e__roi = Matrix(MatrixSymbol("pTeo_pParam_e__roi", 4, 4)) 135 | pT_eo__pParam_e__roi[:, 0] = diff(T_eo, x_e) * o_r_oi 136 | pT_eo__pParam_e__roi[:, 1] = diff(T_eo, y_e) * o_r_oi 137 | pT_eo__pParam_e__roi[:, 2] = diff(T_eo, z_e) * o_r_oi 138 | pT_eo__pParam_e__roi[:, 3] = simplify(diff(T_eo, theta_e) * o_r_oi) 139 | pprint(pT_eo__pParam_e__roi) 140 | print("--> after CSE:") 141 | pprint(cse(pT_eo__pParam_e__roi)) 142 | 143 | print("\nOr combining everything:") 144 | pT_eo__pParam_oe__roi = Matrix(MatrixSymbol("pTeo_pParam_oe__roi", 4, 8)) 145 | pT_eo__pParam_oe__roi[:, :4] = pT_eo__pParam_o__roi 146 | pT_eo__pParam_oe__roi[:, 4:] = pT_eo__pParam_e__roi 147 | cse_pT_eo__pParam_oe__roi = cse(pT_eo__pParam_oe__roi) 148 | pprint(cse_pT_eo__pParam_oe__roi[0]) 149 | pprint(cse_pT_eo__pParam_oe__roi[1]) 150 | -------------------------------------------------------------------------------- /voxgraph/src/tools/data_servers/loop_closure_edge_server.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/data_servers/loop_closure_edge_server.h" 2 | 3 | #include 4 | 5 | #include "voxgraph/common.h" 6 | 7 | namespace voxgraph { 8 | 9 | constexpr bool LoopClosureEdgeServer::kFake6dofTransforms; 10 | constexpr double LoopClosureEdgeServer::kSetUnknownCovarianceEntriesTo; 11 | 12 | LoopClosureEdgeServer::LoopClosureEdgeServer(ros::NodeHandle nh_private, 13 | bool verbose) 14 | : verbose_(verbose) { 15 | loop_closure_edge_list_pub_ = 16 | nh_private.advertise( 17 | "loop_closure_edges", 3, false); 18 | } 19 | 20 | void LoopClosureEdgeServer::publishLoopClosureEdges( 21 | const PoseGraphInterface& pose_graph_interface, 22 | const VoxgraphSubmapCollection& submap_collection, 23 | const ros::Time& timestamp) { 24 | // Only publish if there are subscribers 25 | if (loop_closure_edge_list_pub_.getNumSubscribers() > 0) { 26 | publishLoopClosureEdges(pose_graph_interface, submap_collection, timestamp, 27 | loop_closure_edge_list_pub_, verbose_); 28 | } 29 | } 30 | 31 | void LoopClosureEdgeServer::publishLoopClosureEdges( 32 | const PoseGraphInterface& pose_graph_interface, 33 | const VoxgraphSubmapCollection& submap_collection, 34 | const ros::Time& timestamp, 35 | const ros::Publisher& loop_closure_edge_list_publisher, bool verbose) { 36 | voxgraph_msgs::LoopClosureEdgeList loop_closure_edge_list_msg; 37 | loop_closure_edge_list_msg.header = generateHeaderMsg(timestamp); 38 | 39 | PoseGraphInterface::OverlappingSubmapList overlapping_submap_list = 40 | pose_graph_interface.getOverlappingSubmapList(); 41 | loop_closure_edge_list_msg.loop_closure_edges.reserve( 42 | overlapping_submap_list.size()); 43 | 44 | PoseGraph::EdgeCovarianceMap edge_covariance_map; 45 | bool edge_covariances_useable = 46 | pose_graph_interface.getEdgeCovarianceMap(&edge_covariance_map); 47 | 48 | for (const SubmapIdPair& overlapping_submap_pair : overlapping_submap_list) { 49 | // Get const refs to the submap at each endpoint of the edge 50 | const VoxgraphSubmap& first_submap = 51 | submap_collection.getSubmap(overlapping_submap_pair.first); 52 | const VoxgraphSubmap& second_submap = 53 | submap_collection.getSubmap(overlapping_submap_pair.second); 54 | 55 | // Create the loop closure edge msg 56 | voxgraph_msgs::LoopClosureEdge edge_msg; 57 | edge_msg.timestamp_A = first_submap.getStartTime(); 58 | edge_msg.timestamp_B = second_submap.getStartTime(); 59 | 60 | // Set the edge's observed transformation 61 | Transformation T_M_A = first_submap.getPose(); 62 | Transformation T_M_B = second_submap.getPose(); 63 | 64 | if (LoopClosureEdgeServer::kFake6dofTransforms) { 65 | // Go from 4DoF submap origin poses T_M_S to 6DoF robot poses T_M_R 66 | // by using T_M_B = T_M_S * T_S_R, where T_S_R is the 6DoF robot pose at 67 | // submap creation time 68 | // NOTE: The loop closure edge covariances are estimated for the 4DoF 69 | // submap poses. Using covariances in combination with 6DoF poses 70 | // therefore isn't correct, but might be acceptably close for small 71 | // pitch/roll. 72 | T_M_A = T_M_A * first_submap.getPoseHistory().begin()->second; 73 | T_M_B = T_M_B * second_submap.getPoseHistory().begin()->second; 74 | } 75 | 76 | Transformation T_A_B = T_M_A.inverse() * T_M_B; 77 | tf::poseKindrToMsg(T_A_B.cast(), &edge_msg.T_A_B.pose); 78 | 79 | // Set the edge's covariance 80 | if (edge_covariances_useable) { 81 | // Get the covariance from the pose graph 82 | PoseGraph::EdgeCovarianceMap::const_iterator covariance_iter = 83 | edge_covariance_map.find(overlapping_submap_pair); 84 | if (covariance_iter != edge_covariance_map.end()) { 85 | edge_msg.T_A_B.covariance.fill( 86 | LoopClosureEdgeServer::kSetUnknownCovarianceEntriesTo); 87 | for (int original_row = 0; original_row < 4; ++original_row) { 88 | for (int original_col = 0; original_col < 4; ++original_col) { 89 | int msg_row = original_row; 90 | int msg_col = original_col; 91 | if (original_row == 3) msg_row = 5; 92 | if (original_col == 3) msg_col = 5; 93 | edge_msg.T_A_B.covariance[msg_row * 6 + msg_col] = 94 | covariance_iter->second(original_row, original_col); 95 | } 96 | } 97 | 98 | // Print the msg's covariance matrix for debugging 99 | if (verbose) { 100 | ROS_INFO_STREAM("Edge covariance from submap " 101 | << overlapping_submap_pair.first << " to " 102 | << overlapping_submap_pair.second << " is:\n"); 103 | for (int row = 0; row < 6; ++row) { 104 | for (int col = 0; col < 6; ++col) { 105 | if (col == 0) { 106 | std::cout << std::endl; 107 | } 108 | std::cout << edge_msg.T_A_B.covariance[row * 6 + col] << " "; 109 | } 110 | } 111 | std::cout << std::endl; 112 | } 113 | } else { 114 | ROS_WARN( 115 | "Missing edge covariance for submap pair despite successful " 116 | "covariance extraction. This should not happen. The loop " 117 | "closure edge list will not be published."); 118 | return; 119 | } 120 | } else { 121 | // Set the covariance to identity 122 | for (int row = 0; row < 6; ++row) { 123 | for (int col = 0; col < 6; ++col) { 124 | if (row == col) { 125 | edge_msg.T_A_B.covariance[row * 6 + col] = 1.0; 126 | } else { 127 | edge_msg.T_A_B.covariance[row * 6 + col] = 0.0; 128 | } 129 | } 130 | } 131 | } 132 | 133 | // Add the edge msg to the list 134 | loop_closure_edge_list_msg.loop_closure_edges.push_back(edge_msg); 135 | } 136 | 137 | // Publish the loop closure edge list msg 138 | loop_closure_edge_list_publisher.publish(loop_closure_edge_list_msg); 139 | } 140 | 141 | std_msgs::Header LoopClosureEdgeServer::generateHeaderMsg( 142 | const ros::Time& timestamp) { 143 | std_msgs::Header msg_header; 144 | msg_header.frame_id = ""; 145 | msg_header.stamp = timestamp; 146 | return msg_header; 147 | } 148 | } // namespace voxgraph 149 | -------------------------------------------------------------------------------- /voxgraph/src/frontend/pose_graph_interface/measurement_templates.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/frontend/pose_graph_interface/measurement_templates.h" 2 | 3 | #include 4 | 5 | namespace voxgraph { 6 | MeasurementTemplates::MeasurementTemplates(bool verbose) 7 | : odometry(odometry_), 8 | loop_closure(loop_closure_), 9 | gps(gps_), 10 | height(height_), 11 | registration(registration_), 12 | verbose_(verbose) { 13 | // Initialize the default odometry measurement config 14 | odometry_.information_matrix.setZero(); 15 | 16 | // Initialize the default loop closure config 17 | loop_closure_.information_matrix.setZero(); 18 | 19 | // Initialize the default GPS measurement config 20 | gps_.information_matrix.setZero(); 21 | gps_.reference_frame_id = NodeTemplates::kGpsFrame; 22 | // NOTE: We allow semi definite information matrices s.t. an infinite 23 | // covariance on Yaw (e.g. a zero in the information matrix) can be used 24 | gps_.allow_semi_definite_information_matrix = true; 25 | 26 | // Initialize the default height measurement config 27 | height_.information_matrix.setZero(); 28 | height_.reference_frame_id = NodeTemplates::kMissionFrame; 29 | height_.allow_semi_definite_information_matrix = true; 30 | 31 | // Initialize the default registration config 32 | registration_.information_matrix.setZero(); 33 | } 34 | 35 | void MeasurementTemplates::setFromRosParams( 36 | const ros::NodeHandle& node_handle) { 37 | if (node_handle.hasParam("odometry")) { 38 | // Set the information matrix 39 | setInformationMatrixFromRosParams( 40 | ros::NodeHandle(node_handle, "odometry/information_matrix"), 41 | &odometry_.information_matrix); 42 | ROS_INFO_STREAM_COND(verbose_, "Setting odometry information matrix to:\n" 43 | << odometry.information_matrix); 44 | } 45 | 46 | if (node_handle.hasParam("loop_closure")) { 47 | // Set the information matrix 48 | setInformationMatrixFromRosParams( 49 | ros::NodeHandle(node_handle, "loop_closure/information_matrix"), 50 | &loop_closure_.information_matrix); 51 | ROS_INFO_STREAM_COND(verbose_, 52 | "Setting loop closure information matrix " 53 | "to:\n" 54 | << loop_closure.information_matrix); 55 | } 56 | 57 | if (node_handle.hasParam("gps")) { 58 | // Set the information matrix 59 | setInformationMatrixFromRosParams( 60 | ros::NodeHandle(node_handle, "gps/information_matrix"), 61 | &gps_.information_matrix); 62 | ROS_INFO_STREAM_COND(verbose_, 63 | "Setting gps measurement information " 64 | "matrix to:\n" 65 | << gps.information_matrix); 66 | } 67 | 68 | if (node_handle.hasParam("height")) { 69 | // Set the information matrix 70 | height_.information_matrix.setZero(); 71 | // NOTE: We model height constraints as absolute pose constraints with 72 | // infinite covariance on X, Y, Yaw. The information matrix therefore 73 | // consists entirely of zeros except for the Z (height) entry. 74 | node_handle.param("height/information_zz", height_.information_matrix(2, 2), 75 | 0.0); 76 | ROS_INFO_STREAM_COND(verbose_, 77 | "Setting height measurement information matrix to:\n" 78 | << height.information_matrix); 79 | } 80 | 81 | if (node_handle.hasParam("submap_registration")) { 82 | // Set the sampling ratio 83 | node_handle.param("submap_registration/sampling_ratio", 84 | registration_.registration.sampling_ratio, 85 | registration_.registration.sampling_ratio); 86 | ROS_INFO_STREAM_COND(verbose_, 87 | "Setting submap registration sampling ratio " 88 | "to: " 89 | << registration.registration.sampling_ratio); 90 | 91 | // Set the registration method (from string) 92 | { 93 | std::string registration_method_str; 94 | node_handle.param("submap_registration/registration_method", 95 | registration_method_str, 96 | "implicit_to_implicit"); 97 | if (registration_method_str == "implicit_to_implicit") { 98 | registration_.registration.registration_point_type = 99 | VoxgraphSubmap::RegistrationPointType::kVoxels; 100 | } else if (registration_method_str == "explicit_to_implicit") { 101 | registration_.registration.registration_point_type = 102 | VoxgraphSubmap::RegistrationPointType::kIsosurfacePoints; 103 | } else { 104 | ROS_WARN_STREAM( 105 | "Param \"submap_registration/registration_method\" must be " 106 | "\"implicit_to_implicit\" (default) or \"explicit_to_implicit\", " 107 | "but received \"" 108 | << registration_method_str 109 | << "\". " 110 | "Will use default instead."); 111 | } 112 | ROS_INFO_STREAM_COND(verbose_, "Setting submap registration method to: " 113 | << registration_method_str); 114 | } 115 | 116 | // Set the information matrix 117 | setInformationMatrixFromRosParams( 118 | ros::NodeHandle(node_handle, "submap_registration/information_matrix"), 119 | ®istration_.information_matrix); 120 | ROS_INFO_STREAM_COND(verbose_, 121 | "Setting submap registration information matrix to:\n" 122 | << registration.information_matrix); 123 | } 124 | } 125 | 126 | void MeasurementTemplates::setInformationMatrixFromRosParams( 127 | const ros::NodeHandle& node_handle, 128 | Constraint::InformationMatrix* information_matrix) { 129 | CHECK_NOTNULL(information_matrix); 130 | Constraint::InformationMatrix& information_matrix_ref = *information_matrix; 131 | 132 | // Set the upper triangular part of the information matrix from ROS params 133 | node_handle.param("x_x", information_matrix_ref(0, 0), 0.0); 134 | node_handle.param("x_y", information_matrix_ref(0, 1), 0.0); 135 | node_handle.param("x_z", information_matrix_ref(0, 2), 0.0); 136 | node_handle.param("x_yaw", information_matrix_ref(0, 3), 0.0); 137 | 138 | node_handle.param("y_y", information_matrix_ref(1, 1), 0.0); 139 | node_handle.param("y_z", information_matrix_ref(1, 2), 0.0); 140 | node_handle.param("y_yaw", information_matrix_ref(1, 3), 0.0); 141 | 142 | node_handle.param("z_z", information_matrix_ref(2, 2), 0.0); 143 | node_handle.param("z_yaw", information_matrix_ref(2, 3), 0.0); 144 | 145 | node_handle.param("yaw_yaw", information_matrix_ref(3, 3), 0.0); 146 | 147 | // Copy the upper to the lower triangular part, to get a symmetric info matrix 148 | information_matrix_ref = 149 | information_matrix_ref.selfadjointView(); 150 | } 151 | } // namespace voxgraph 152 | -------------------------------------------------------------------------------- /voxgraph/include/voxgraph/frontend/voxgraph_mapper.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXGRAPH_FRONTEND_VOXGRAPH_MAPPER_H_ 2 | #define VOXGRAPH_FRONTEND_VOXGRAPH_MAPPER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "voxgraph/common.h" 15 | #include "voxgraph/frontend/frame_names.h" 16 | #include "voxgraph/frontend/map_tracker/map_tracker.h" 17 | #include "voxgraph/frontend/measurement_processors/gps_processor.h" 18 | #include "voxgraph/frontend/measurement_processors/pointcloud_integrator.h" 19 | #include "voxgraph/frontend/pose_graph_interface/pose_graph_interface.h" 20 | #include "voxgraph/frontend/submap_collection/voxgraph_submap_collection.h" 21 | #include "voxgraph/tools/data_servers/loop_closure_edge_server.h" 22 | #include "voxgraph/tools/data_servers/projected_map_server.h" 23 | #include "voxgraph/tools/data_servers/submap_server.h" 24 | #include "voxgraph/tools/rosbag_helper.h" 25 | #include "voxgraph/tools/visualization/loop_closure_visuals.h" 26 | #include "voxgraph/tools/visualization/submap_visuals.h" 27 | 28 | namespace voxgraph { 29 | class VoxgraphMapper { 30 | public: 31 | // Constructor & Destructor 32 | VoxgraphMapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 33 | VoxgraphMapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, 34 | VoxgraphSubmap::Config submap_config, 35 | voxblox::MeshIntegratorConfig mesh_config); 36 | ~VoxgraphMapper() = default; 37 | 38 | // ROS topic callbacks 39 | void pointcloudCallback(const sensor_msgs::PointCloud2::Ptr& pointcloud_msg); 40 | void loopClosureCallback(const voxgraph_msgs::LoopClosure& loop_closure_msg); 41 | 42 | // ROS timer callbacks 43 | void publishActiveSubmapMeshCallback(); 44 | 45 | // ROS service callbacks 46 | bool publishSeparatedMeshCallback( 47 | std_srvs::Empty::Request& request, // NOLINT 48 | std_srvs::Empty::Response& response); // NOLINT 49 | bool publishCombinedMeshCallback( 50 | std_srvs::Empty::Request& request, // NOLINT 51 | std_srvs::Empty::Response& response); // NOLINT 52 | bool optimizeGraphCallback(std_srvs::Empty::Request& request, // NOLINT 53 | std_srvs::Empty::Response& response); // NOLINT 54 | bool finishMapCallback(std_srvs::Empty::Request& request, // NOLINT 55 | std_srvs::Empty::Response& response); // NOLINT 56 | bool saveToFileCallback( 57 | voxblox_msgs::FilePath::Request& request, // NOLINT 58 | voxblox_msgs::FilePath::Response& response); // NOLINT 59 | bool savePoseHistoryToFileCallback( 60 | voxblox_msgs::FilePath::Request& request, // NOLINT 61 | voxblox_msgs::FilePath::Response& response); // NOLINT 62 | bool saveSeparatedMeshCallback( 63 | voxblox_msgs::FilePath::Request& request, // NOLINT 64 | voxblox_msgs::FilePath::Response& response); // NOLINT 65 | bool saveCombinedMeshCallback( 66 | voxblox_msgs::FilePath::Request& request, // NOLINT 67 | voxblox_msgs::FilePath::Response& response); // NOLINT 68 | bool saveOptimizationTimesCallback( 69 | voxblox_msgs::FilePath::Request& request, // NOLINT 70 | voxblox_msgs::FilePath::Response& response); // NOLINT 71 | 72 | const VoxgraphSubmapCollection& getSubmapCollection() { 73 | return *submap_collection_ptr_; 74 | } 75 | 76 | const PoseGraph::SolverSummaryList& getSolverSummaries() { 77 | return pose_graph_interface_.getSolverSummaries(); 78 | } 79 | 80 | private: 81 | // Node handles 82 | ros::NodeHandle nh_; 83 | ros::NodeHandle nh_private_; 84 | 85 | // Verbosity and debug mode 86 | bool verbose_; 87 | 88 | // Flag and helper to automatically pause rosbags during graph optimization 89 | // NOTE: This is useful when playing bags faster than real-time or when 90 | // experimenting with optimization settings 91 | bool auto_pause_rosbag_; 92 | RosbagHelper rosbag_helper_; 93 | 94 | // Interaction with ROS 95 | void subscribeToTopics(); 96 | void advertiseTopics(); 97 | void advertiseServices(); 98 | void getParametersFromRos(); 99 | 100 | // New submap creation, pose graph optimization and map publishing 101 | void switchToNewSubmap(const ros::Time& current_timestamp); 102 | int optimizePoseGraph(); 103 | void publishMaps(const ros::Time& current_timestamp); 104 | 105 | // Asynchronous handle for the pose graph optimization thread 106 | std::future optimization_async_handle_; 107 | 108 | // ROS topic subscribers 109 | std::string pointcloud_topic_; 110 | int subscriber_queue_length_; 111 | ros::Subscriber pointcloud_subscriber_; 112 | std::string loop_closure_topic_; 113 | int loop_closure_subscriber_queue_length_; 114 | ros::Subscriber loop_closure_subscriber_; 115 | // TODO(victorr): Add support for absolute pose measurements 116 | 117 | // Timers. 118 | ros::Timer update_mesh_timer_; 119 | 120 | // ROS topic publishers 121 | ros::Publisher separated_mesh_pub_; 122 | ros::Publisher active_mesh_pub_; 123 | ros::Publisher combined_mesh_pub_; 124 | ros::Publisher pose_history_pub_; 125 | ros::Publisher loop_closure_links_pub_; 126 | ros::Publisher loop_closure_axes_pub_; 127 | 128 | // ROS service servers 129 | ros::ServiceServer publish_separated_mesh_srv_; 130 | ros::ServiceServer publish_combined_mesh_srv_; 131 | ros::ServiceServer optimize_graph_srv_; 132 | ros::ServiceServer finish_map_srv_; 133 | ros::ServiceServer save_to_file_srv_; 134 | ros::ServiceServer save_pose_history_to_file_srv_; 135 | ros::ServiceServer save_separated_mesh_srv_; 136 | ros::ServiceServer save_combined_mesh_srv_; 137 | ros::ServiceServer save_optimization_times_srv_; 138 | // TODO(victorr): Add srvs to receive absolute pose and loop closure updates 139 | 140 | // Constraints to be used 141 | bool registration_constraints_enabled_; 142 | bool odometry_constraints_enabled_; 143 | bool height_constraints_enabled_; 144 | 145 | // Instantiate the submap collection 146 | VoxgraphSubmap::Config submap_config_; 147 | VoxgraphSubmapCollection::Ptr submap_collection_ptr_; 148 | 149 | // Visualization tools 150 | SubmapVisuals submap_vis_; 151 | LoopClosureVisuals loop_closure_vis_; 152 | 153 | // Interface to ease interaction with the pose graph 154 | PoseGraphInterface pose_graph_interface_; 155 | 156 | // Measurement processors 157 | PointcloudIntegrator pointcloud_integrator_; 158 | 159 | // Map servers, used to share the projected map and submaps with ROS nodes 160 | ProjectedMapServer projected_map_server_; 161 | SubmapServer submap_server_; 162 | LoopClosureEdgeServer loop_closure_edge_server_; 163 | 164 | // Map tracker handles the odometry input and refines it using scan-to-map ICP 165 | MapTracker map_tracker_; 166 | }; 167 | } // namespace voxgraph 168 | 169 | #endif // VOXGRAPH_FRONTEND_VOXGRAPH_MAPPER_H_ 170 | -------------------------------------------------------------------------------- /voxgraph/src/tools/visualization/submap_visuals.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/visualization/submap_visuals.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace voxgraph { 14 | SubmapVisuals::SubmapVisuals(VoxgraphSubmap::Config submap_config, 15 | voxblox::MeshIntegratorConfig mesh_config) 16 | : mesh_config_(std::move(mesh_config)), mesh_opacity_(1.0) { 17 | // Meshing params from ROS params server 18 | // NOTE(alexmillane): The separated mesher *requires* color, so this is 19 | // hard-coded. 20 | combined_submap_mesher_.reset( 21 | new cblox::SubmapMesher(std::move(submap_config), mesh_config_)); 22 | mesh_config_.use_color = true; 23 | separated_submap_mesher_.reset( 24 | new cblox::SubmapMesher(std::move(submap_config), mesh_config_)); 25 | } 26 | 27 | void SubmapVisuals::publishMesh(const voxblox::MeshLayer::Ptr& mesh_layer_ptr, 28 | const std::string& submap_frame, 29 | const ros::Publisher& publisher, 30 | const voxblox::ColorMode& color_mode) const { 31 | // Create a marker containing the mesh 32 | visualization_msgs::Marker marker; 33 | voxblox::fillMarkerWithMesh(mesh_layer_ptr, color_mode, &marker); 34 | marker.header.frame_id = submap_frame; 35 | // Adapt mesh opacity 36 | marker.color.a = mesh_opacity_; 37 | for (std_msgs::ColorRGBA& color : marker.colors) { 38 | color.a = mesh_opacity_; 39 | } 40 | // Update the marker's transform each time its TF frame is updated: 41 | marker.frame_locked = true; 42 | publisher.publish(marker); 43 | } 44 | 45 | void SubmapVisuals::publishMesh( 46 | const cblox::SubmapCollection& submap_collection, 47 | const cblox::SubmapID& submap_id, const voxblox::Color& submap_color, 48 | const std::string& submap_frame, const ros::Publisher& publisher) const { 49 | // Get a pointer to the submap 50 | VoxgraphSubmap::ConstPtr submap_ptr = 51 | submap_collection.getSubmapConstPtr(submap_id); 52 | CHECK_NOTNULL(submap_ptr); 53 | 54 | // Generate the mesh 55 | auto mesh_layer_ptr = 56 | std::make_shared(submap_collection.block_size()); 57 | 58 | voxblox::MeshIntegrator reference_mesh_integrator( 59 | mesh_config_, submap_ptr->getTsdfMap().getTsdfLayer(), 60 | mesh_layer_ptr.get()); 61 | reference_mesh_integrator.generateMesh(false, false); 62 | separated_submap_mesher_->colorMeshLayer(submap_color, mesh_layer_ptr.get()); 63 | 64 | // Publish mesh 65 | publishMesh(mesh_layer_ptr, submap_frame, publisher); 66 | } 67 | 68 | void SubmapVisuals::publishSeparatedMesh( 69 | const cblox::SubmapCollection& submap_collection, 70 | const std::string& mission_frame, const ros::Publisher& publisher) { 71 | auto mesh_layer_ptr = 72 | std::make_shared(submap_collection.block_size()); 73 | separated_submap_mesher_->generateSeparatedMesh(submap_collection, 74 | mesh_layer_ptr.get()); 75 | publishMesh(mesh_layer_ptr, mission_frame, publisher); 76 | } 77 | 78 | void SubmapVisuals::publishCombinedMesh( 79 | const cblox::SubmapCollection& submap_collection, 80 | const std::string& mission_frame, const ros::Publisher& publisher) { 81 | auto mesh_layer_ptr = 82 | std::make_shared(submap_collection.block_size()); 83 | combined_submap_mesher_->generateCombinedMesh(submap_collection, 84 | mesh_layer_ptr.get()); 85 | publishMesh(mesh_layer_ptr, mission_frame, publisher, 86 | voxblox::ColorMode::kNormals); 87 | } 88 | 89 | void SubmapVisuals::saveSeparatedMesh( 90 | const std::string& filepath, 91 | const cblox::SubmapCollection& submap_collection) { 92 | auto mesh_layer_ptr = 93 | std::make_shared(submap_collection.block_size()); 94 | separated_submap_mesher_->generateSeparatedMesh(submap_collection, 95 | mesh_layer_ptr.get()); 96 | voxblox::outputMeshLayerAsPly(filepath, *mesh_layer_ptr); 97 | } 98 | 99 | void SubmapVisuals::saveCombinedMesh( 100 | const std::string& filepath, 101 | const cblox::SubmapCollection& submap_collection) { 102 | auto mesh_layer_ptr = 103 | std::make_shared(submap_collection.block_size()); 104 | combined_submap_mesher_->generateCombinedMesh(submap_collection, 105 | mesh_layer_ptr.get()); 106 | voxblox::outputMeshLayerAsPly(filepath, *mesh_layer_ptr); 107 | } 108 | 109 | void SubmapVisuals::publishBox(const BoxCornerMatrix& box_corner_matrix, 110 | const voxblox::Color& box_color, 111 | const std::string& frame_id, 112 | const std::string& name_space, 113 | const ros::Publisher& publisher) const { 114 | // Create and configure the marker 115 | visualization_msgs::Marker marker; 116 | marker.header.frame_id = frame_id; 117 | marker.header.stamp = ros::Time(); 118 | marker.ns = name_space; 119 | marker.id = 0; 120 | marker.type = visualization_msgs::Marker::LINE_LIST; 121 | marker.action = visualization_msgs::Marker::ADD; 122 | marker.pose.orientation.w = 1.0; // Set to unit quaternion 123 | marker.scale.x = 0.1; 124 | voxblox::colorVoxbloxToMsg(box_color, &marker.color); 125 | marker.frame_locked = false; 126 | 127 | // Add the box edges to the marker's 128 | std::bitset<3> endpoint_a_idx, endpoint_b_idx; 129 | for (unsigned int i = 0; i < 8; i++) { 130 | endpoint_a_idx = std::bitset<3>(i); 131 | for (unsigned int j = i + 1; j < 8; j++) { 132 | endpoint_b_idx = std::bitset<3>(j); 133 | // The endpoints describe an edge of the box if the 134 | // Hamming distance between both endpoint indices is 1 135 | if ((endpoint_a_idx ^ endpoint_b_idx).count() == 1) { 136 | // Add the edge's endpoints to the marker's line list 137 | geometry_msgs::Point point_msg; 138 | tf::pointEigenToMsg(box_corner_matrix.col(i).cast(), point_msg); 139 | marker.points.push_back(point_msg); 140 | tf::pointEigenToMsg(box_corner_matrix.col(j).cast(), point_msg); 141 | marker.points.push_back(point_msg); 142 | } 143 | } 144 | } 145 | 146 | // Publish the visualization 147 | publisher.publish(marker); 148 | } 149 | 150 | void SubmapVisuals::publishPoseHistory( 151 | const VoxgraphSubmapCollection& submap_collection, 152 | const std::string& mission_frame, const ros::Publisher& publisher) const { 153 | // Create the pose history message 154 | nav_msgs::Path pose_history_msg; 155 | pose_history_msg.header.stamp = ros::Time::now(); 156 | pose_history_msg.header.frame_id = mission_frame; 157 | pose_history_msg.poses = submap_collection.getPoseHistory(); 158 | 159 | // Publish the message 160 | publisher.publish(pose_history_msg); 161 | } 162 | 163 | // TODO(victorr): Implement TSDF visualization 164 | 165 | } // namespace voxgraph 166 | -------------------------------------------------------------------------------- /voxgraph/src/tools/evaluation/map_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include "voxgraph/tools/evaluation/map_evaluation.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "voxgraph/backend/constraint/cost_functions/registration_cost_function.h" 11 | 12 | namespace voxgraph { 13 | MapEvaluation::MapEvaluation(const ros::NodeHandle& node_handle, 14 | const std::string& ground_truth_tsdf_file_path) 15 | : node_handle_(node_handle) { 16 | // Load the ground truth TSDF layer 17 | // NOTE: The initial layer will be overwritten but the ptr cannot be null 18 | auto ground_truth_tsdf_layer_ptr = std::make_shared(1, 1); 19 | voxblox::io::LoadLayer(ground_truth_tsdf_file_path, 20 | &ground_truth_tsdf_layer_ptr); 21 | ground_truth_map_ptr_ = std::make_shared( 22 | Transformation(), 0, *ground_truth_tsdf_layer_ptr); 23 | 24 | // Advertise the visualization topics 25 | ground_truth_layer_pub_ = 26 | node_handle_.advertise>( 27 | "tsdf_ground_truth_ptcloud", 1, true); 28 | ground_truth_mesh_pub_ = node_handle_.advertise( 29 | "tsdf_ground_truth_mesh", 1, true); 30 | rmse_error_pub_ = node_handle_.advertise>( 31 | "tsdf_rmse_error", 1, true); 32 | rmse_error_slice_pub_ = 33 | node_handle_.advertise>( 34 | "tsdf_rmse_error_slice", 1, true); 35 | 36 | // Publish the ground truth TSDF pointcloud 37 | pcl::PointCloud ground_truth_layer_ptcloud_msg; 38 | ground_truth_layer_ptcloud_msg.header.frame_id = "mission"; 39 | voxblox::createSurfaceDistancePointcloudFromTsdfLayer( 40 | *ground_truth_tsdf_layer_ptr, 0.6, &ground_truth_layer_ptcloud_msg); 41 | ground_truth_layer_pub_.publish(ground_truth_layer_ptcloud_msg); 42 | 43 | // Generate the ground truth TSDF mesh 44 | auto ground_truth_mesh_layer_ptr = std::make_shared( 45 | ground_truth_tsdf_layer_ptr->block_size()); 46 | voxblox::MeshIntegrator mesh_integrator( 47 | voxblox::MeshIntegratorConfig(), *ground_truth_tsdf_layer_ptr, 48 | ground_truth_mesh_layer_ptr.get()); 49 | mesh_integrator.generateMesh(false, false); 50 | 51 | // Publish the ground truth TSDF mesh 52 | visualization_msgs::Marker marker; 53 | voxblox::fillMarkerWithMesh(ground_truth_mesh_layer_ptr, 54 | voxblox::ColorMode::kNormals, &marker); 55 | marker.header.frame_id = "mission"; 56 | ground_truth_mesh_pub_.publish(marker); 57 | } 58 | 59 | MapEvaluation::EvaluationDetails MapEvaluation::evaluate( 60 | const VoxgraphSubmapCollection& submap_collection) { 61 | // Get the voxel size and number of voxels per side 62 | voxblox::FloatingPoint voxel_size = 63 | ground_truth_map_ptr_->getTsdfMap().voxel_size(); 64 | size_t voxels_per_side = 65 | ground_truth_map_ptr_->getTsdfMap().getTsdfLayer().voxels_per_side(); 66 | CHECK_EQ(submap_collection.getConfig().tsdf_voxel_size, voxel_size) 67 | << "Submap collection and ground truth must have equal voxel size."; 68 | 69 | // Load the submap collection's projected TSDF map into a VoxgraphSubmap 70 | // NOTE: This is done in order to use our SubmapRegistrationHelper (see below) 71 | Transformation T_identity; 72 | TsdfLayer projected_tsdf_layer( 73 | submap_collection.getProjectedMap()->getTsdfLayer()); 74 | VoxgraphSubmap::Ptr projected_map_ptr = 75 | std::make_shared(T_identity, 1, projected_tsdf_layer); 76 | 77 | // Align the ground truth map to the submap collection's projected map 78 | // NOTE: Moving the submap collection would be more intuitive, but that would 79 | // require updating all visuals published during the mapping stage 80 | projected_map_ptr->finishSubmap(); 81 | ground_truth_map_ptr_->finishSubmap(); 82 | alignSubmapAtoSubmapB(ground_truth_map_ptr_, projected_map_ptr); 83 | // Apply the transform (interpolate TSDF and ESDF layers into mission frame) 84 | Transformation T_projected_map__ground_truth = 85 | ground_truth_map_ptr_->getPose(); 86 | ground_truth_map_ptr_->transformSubmap(T_projected_map__ground_truth); 87 | 88 | // Compute RMSE of the projected w.r.t. the ground truth map 89 | EsdfLayer error_layer(voxel_size, voxels_per_side); 90 | voxblox::utils::VoxelEvaluationDetails evaluation_details; 91 | voxblox::utils::evaluateLayersRmse( 92 | ground_truth_map_ptr_->getEsdfMap().getEsdfLayer(), 93 | projected_map_ptr->getEsdfMap().getEsdfLayer(), 94 | VoxelEvaluationMode::kIgnoreErrorBehindTestSurface, &evaluation_details, 95 | &error_layer); 96 | std::cout << evaluation_details.toString() << std::endl; 97 | 98 | // Publish the RMSE error layer for visualization in Rviz 99 | PointcloudMsg rmse_error_msg; 100 | PointcloudMsg rmse_error_slice_msg; 101 | 102 | rmse_error_msg.header.frame_id = "mission"; 103 | rmse_error_slice_msg.header.frame_id = "mission"; 104 | 105 | voxblox::createDistancePointcloudFromEsdfLayer(error_layer, &rmse_error_msg); 106 | voxblox::createDistancePointcloudFromEsdfLayerSlice( 107 | error_layer, 2, 3 * voxel_size, &rmse_error_slice_msg); 108 | 109 | rmse_error_pub_.publish(rmse_error_msg); 110 | rmse_error_slice_pub_.publish(rmse_error_slice_msg); 111 | 112 | return EvaluationDetails{evaluation_details, 113 | T_projected_map__ground_truth.inverse()}; 114 | } 115 | 116 | void MapEvaluation::alignSubmapAtoSubmapB( 117 | voxgraph::VoxgraphSubmap::Ptr submap_A, 118 | voxgraph::VoxgraphSubmap::ConstPtr submap_B) { 119 | CHECK_NOTNULL(submap_A); 120 | CHECK_NOTNULL(submap_B); 121 | 122 | // Create and configure Ceres the problem 123 | ceres::Problem problem; 124 | ceres::LossFunction* loss_function = nullptr; // No robust loss function 125 | ceres::Solver::Summary summary; 126 | ceres::Solver::Options ceres_options; 127 | ceres_options.max_num_iterations = 200; 128 | ceres_options.parameter_tolerance = 1e-12; 129 | RegistrationCostFunction::Config cost_config; 130 | cost_config.use_esdf_distance = true; 131 | cost_config.sampling_ratio = -1; 132 | cost_config.registration_point_type = 133 | VoxgraphSubmap::RegistrationPointType::kVoxels; 134 | 135 | // Add the parameter blocks to the optimization 136 | double layer_B_pose[4] = {0, 0, 0, 0}; 137 | problem.AddParameterBlock(layer_B_pose, 4); 138 | problem.SetParameterBlockConstant(layer_B_pose); 139 | double layer_A_pose[4] = {0, 0, 0, 0}; 140 | problem.AddParameterBlock(layer_A_pose, 4); 141 | 142 | // Create and add the submap alignment cost function to the problem 143 | ceres::CostFunction* cost_function = 144 | new RegistrationCostFunction(submap_B, submap_A, cost_config); 145 | problem.AddResidualBlock(cost_function, loss_function, layer_B_pose, 146 | layer_A_pose); 147 | 148 | // Run the solver 149 | ceres::Solve(ceres_options, &problem, &summary); 150 | std::cout << summary.FullReport() << std::endl; 151 | std::cout << "Aligned pose: " << layer_A_pose[0] << ", " << layer_A_pose[1] 152 | << ", " << layer_A_pose[2] << ", " << layer_A_pose[3] << std::endl; 153 | 154 | // Update the pose of submap A 155 | // clang-format off 156 | Transformation::Vector6 T_before_after_vec; 157 | T_before_after_vec << layer_A_pose[0], layer_A_pose[1], layer_A_pose[2], 158 | 0, 0, layer_A_pose[3]; 159 | submap_A->setPose(Transformation::exp(T_before_after_vec)); 160 | // clang-format on 161 | } 162 | } // namespace voxgraph 163 | -------------------------------------------------------------------------------- /voxgraph/config/voxgraph_mapper.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /Voxgraph1 10 | - /Voxgraph1/Meshes1 11 | Splitter Ratio: 0.6882352828979492 12 | Tree Height: 1088 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 1000 55 | Reference Frame: 56 | Value: true 57 | - Class: rviz/TF 58 | Enabled: true 59 | Frame Timeout: 15 60 | Frames: 61 | All Enabled: false 62 | camera0: 63 | Value: true 64 | imu: 65 | Value: true 66 | mission: 67 | Value: true 68 | odom: 69 | Value: true 70 | os1_lidar: 71 | Value: true 72 | os1_sensor: 73 | Value: true 74 | penguin/current_reference: 75 | Value: true 76 | sensor_voxgraph: 77 | Value: true 78 | voxgraph_active_submap: 79 | Value: true 80 | Marker Scale: 1 81 | Name: TF 82 | Show Arrows: true 83 | Show Axes: true 84 | Show Names: true 85 | Tree: 86 | mission: 87 | voxgraph_active_submap: 88 | odom: 89 | imu: 90 | camera0: 91 | {} 92 | os1_sensor: 93 | os1_lidar: 94 | {} 95 | sensor_voxgraph: 96 | {} 97 | penguin/current_reference: 98 | {} 99 | Update Interval: 0 100 | Value: true 101 | - Class: rviz/Group 102 | Displays: 103 | - Alpha: 1 104 | Buffer Length: 1 105 | Class: rviz/Path 106 | Color: 25; 255; 0 107 | Enabled: true 108 | Head Diameter: 0.30000001192092896 109 | Head Length: 0.20000000298023224 110 | Length: 0.30000001192092896 111 | Line Style: Lines 112 | Line Width: 0.029999999329447746 113 | Name: Path 114 | Offset: 115 | X: 0 116 | Y: 0 117 | Z: 0 118 | Pose Color: 255; 85; 255 119 | Pose Style: None 120 | Radius: 0.029999999329447746 121 | Shaft Diameter: 0.10000000149011612 122 | Shaft Length: 0.10000000149011612 123 | Topic: /voxgraph_mapper/pose_history 124 | Unreliable: false 125 | Value: true 126 | - Class: rviz/Marker 127 | Enabled: true 128 | Marker Topic: /voxgraph_mapper/pose_graph 129 | Name: Pose graph 130 | Namespaces: 131 | optimized: true 132 | Queue Size: 100 133 | Value: true 134 | - Class: rviz/Group 135 | Displays: 136 | - Class: rviz/Marker 137 | Enabled: true 138 | Marker Topic: /voxgraph_mapper/combined_mesh 139 | Name: Merged map 140 | Namespaces: 141 | mesh: true 142 | Queue Size: 100 143 | Value: true 144 | - Class: rviz/Marker 145 | Enabled: false 146 | Marker Topic: /voxgraph_mapper/separated_mesh 147 | Name: Individual submaps 148 | Namespaces: 149 | {} 150 | Queue Size: 100 151 | Value: false 152 | Enabled: true 153 | Name: Meshes 154 | Enabled: true 155 | Name: Voxgraph 156 | Enabled: true 157 | Global Options: 158 | Background Color: 255; 255; 255 159 | Default Light: true 160 | Fixed Frame: mission 161 | Frame Rate: 30 162 | Name: root 163 | Tools: 164 | - Class: rviz/Interact 165 | Hide Inactive Objects: true 166 | - Class: rviz/MoveCamera 167 | - Class: rviz/Select 168 | - Class: rviz/FocusCamera 169 | - Class: rviz/Measure 170 | - Class: rviz/SetInitialPose 171 | Theta std deviation: 0.2617993950843811 172 | Topic: /initialpose 173 | X std deviation: 0.5 174 | Y std deviation: 0.5 175 | - Class: rviz/SetGoal 176 | Topic: /move_base_simple/goal 177 | - Class: rviz/PublishPoint 178 | Single click: true 179 | Topic: /clicked_point 180 | Value: true 181 | Views: 182 | Current: 183 | Class: rviz/Orbit 184 | Distance: 83.7422866821289 185 | Enable Stereo Rendering: 186 | Stereo Eye Separation: 0.05999999865889549 187 | Stereo Focal Distance: 1 188 | Swap Stereo Eyes: false 189 | Value: false 190 | Focal Point: 191 | X: 7.814975738525391 192 | Y: 1.5188809633255005 193 | Z: -4.198982238769531 194 | Focal Shape Fixed Size: false 195 | Focal Shape Size: 0.05000000074505806 196 | Invert Z Axis: false 197 | Name: Current View 198 | Near Clip Distance: 0.10000000149011612 199 | Pitch: 0.4547974169254303 200 | Target Frame: 201 | Value: Orbit (rviz) 202 | Yaw: 5.588576793670654 203 | Saved: ~ 204 | Window Geometry: 205 | Displays: 206 | collapsed: false 207 | Height: 1385 208 | Hide Left Dock: false 209 | Hide Right Dock: true 210 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bd0000003efc0100000002fb0000000800540069006d00650100000000000009bd000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000861000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 211 | Selection: 212 | collapsed: false 213 | Time: 214 | collapsed: false 215 | Tool Properties: 216 | collapsed: false 217 | Views: 218 | collapsed: true 219 | Width: 2493 220 | X: 1987 221 | Y: 43 222 | --------------------------------------------------------------------------------