├── .gitignore ├── .gitmodules ├── iris ├── launch │ ├── rviz.launch │ ├── mono_kitti00.launch │ ├── mono_kitti02.launch │ ├── mono_kitti08.launch │ ├── stereo_kitti00.launch │ ├── stereo_kitti02.launch │ └── stereo_kitti08.launch ├── package.xml ├── src │ ├── core │ │ ├── math.hpp │ │ ├── keypoints_with_normal.hpp │ │ ├── types.hpp │ │ ├── util.hpp │ │ ├── math.cpp │ │ ├── config.hpp │ │ └── util.cpp │ ├── map │ │ ├── parameter.hpp │ │ ├── info.hpp │ │ ├── map.hpp │ │ └── map.cpp │ ├── optimize │ │ ├── averager.hpp │ │ ├── optimizer.hpp │ │ ├── types_gicp.hpp │ │ ├── aligner.hpp │ │ ├── types_gicp.cpp │ │ ├── types_restriction.cpp │ │ ├── types_restriction.hpp │ │ ├── optimizer.cpp │ │ └── aligner.cpp │ ├── system │ │ ├── publisher.cpp │ │ ├── publisher.hpp │ │ ├── system.hpp │ │ └── system.cpp │ ├── publish │ │ ├── publish.hpp │ │ └── publish.cpp │ ├── pcl_ │ │ ├── normal_estimator.cpp │ │ ├── correspondence_estimator.cpp │ │ └── correspondence_estimator.hpp │ └── iris_node.cpp └── CMakeLists.txt ├── config ├── iris_mono_kitti.yaml ├── iris_stereo_kitti.yaml └── iris.rviz ├── openvslam_bridge ├── package.xml ├── src │ ├── stereo_bridge.hpp │ ├── bridge.hpp │ ├── stereo_bridge.cpp │ ├── bridge_node.cpp │ ├── bridge.cpp │ └── stereo_bridge_node.cpp └── CMakeLists.txt ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | devel* 3 | install* 4 | .vscode* 5 | *.cache 6 | *.pcd 7 | *.dbow2 8 | 9 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "openvslam_bridge/3rd/openvslam"] 2 | path = openvslam_bridge/3rd/openvslam 3 | url = https://github.com/MapIV/openvslam.git 4 | -------------------------------------------------------------------------------- /iris/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /config/iris_mono_kitti.yaml: -------------------------------------------------------------------------------- 1 | # transform of camera 2 | Init.transform: [ 0.0, 0.0, 0.0] 3 | # normal vector of camera 4 | Init.normal: [ 1.0, 0.0, 0.0] 5 | # upper vector of camera 6 | Init.upper: [ 0, 0, 1] 7 | # scale 8 | Init.scale: 1.5 9 | 10 | Iris.scale_gain: 50.00 11 | Iris.smooth_gain: 0.0 12 | Iris.latitude_gain: 5.0 13 | Iris.altitude_gain: 0.0 14 | 15 | Iris.distance_min: 2.0 16 | Iris.distance_max: 4.0 17 | 18 | Iris.iteration: 5 19 | Iris.converge_translation: 0.05 20 | Iris.converge_rotation: 0.01 21 | 22 | # Map Parameter 23 | Map.voxel_grid_leaf: 0.500 24 | Map.submap_grid_leaf: 80 25 | Map.normal_search_leaf: 1.0 26 | -------------------------------------------------------------------------------- /config/iris_stereo_kitti.yaml: -------------------------------------------------------------------------------- 1 | # transform of camera 2 | Init.transform: [ 0.0, 0.0, 0.0] 3 | # normal vector of camera 4 | Init.normal: [ 1.0, 0.0, 0.0] 5 | # upper vector of camera 6 | Init.upper: [ 0, 0, 1] 7 | # scale 8 | Init.scale: 1.00 9 | 10 | Iris.scale_gain: 150.00 11 | Iris.smooth_gain: 0.0 12 | Iris.latitude_gain: 0.0 13 | Iris.altitude_gain: 0.0 14 | 15 | Iris.distance_min: 2.0 16 | Iris.distance_max: 5.0 17 | 18 | Iris.iteration: 5 19 | Iris.converge_translation: 0.05 20 | Iris.converge_rotation: 0.01 21 | 22 | # Map Parameter 23 | Map.voxel_grid_leaf: 0.800 24 | Map.submap_grid_leaf: 80 25 | Map.normal_search_leaf: 1.6 26 | -------------------------------------------------------------------------------- /openvslam_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | openvslam_bridge 4 | 0.0.0 5 | The ROS package for brdging OpenVSLAM 6 | 7 | Kento Yabuuchi 8 | BSD 9 | 10 | catkin 11 | 12 | image_transport 13 | cv_bridge 14 | sensor_msgs 15 | geometry_msgs 16 | visualization_msgs 17 | tf 18 | 19 | image_transport 20 | cv_bridge 21 | sensor_msgs 22 | geometry_msgs 23 | visualization_msgs 24 | tf 25 | 26 | image_transport 27 | cv_bridge 28 | sensor_msgs 29 | geometry_msgs 30 | visualization_msgs 31 | tf 32 | -------------------------------------------------------------------------------- /iris/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | iris 4 | 0.0.0 5 | The ROS package for Iris 6 | 7 | Kento Yabuuchi 8 | BSD 9 | 10 | catkin 11 | 12 | image_transport 13 | cv_bridge 14 | sensor_msgs 15 | geometry_msgs 16 | visualization_msgs 17 | nav_msgs 18 | tf 19 | 20 | image_transport 21 | cv_bridge 22 | sensor_msgs 23 | geometry_msgs 24 | visualization_msgs 25 | nav_msgs 26 | tf 27 | 28 | image_transport 29 | cv_bridge 30 | sensor_msgs 31 | geometry_msgs 32 | visualization_msgs 33 | nav_msgs 34 | tf 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020, Map IV, Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright notice, 9 | this list of conditions and the following disclaimer in the documentation 10 | and/or other materials provided with the distribution. 11 | * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | may be used to endorse or promote products derived from this software 13 | without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /iris/launch/mono_kitti00.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /iris/launch/mono_kitti02.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /iris/launch/mono_kitti08.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /iris/launch/stereo_kitti00.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /iris/launch/stereo_kitti02.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /iris/launch/stereo_kitti08.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /iris/src/core/math.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | 29 | namespace iris 30 | { 31 | namespace so3 32 | { 33 | Eigen::Matrix3f hat(const Eigen::Vector3f& xi); 34 | 35 | Eigen::Vector3f log(const Eigen::Matrix3f& R); 36 | 37 | Eigen::Matrix3f exp(const Eigen::Vector3f& xi); 38 | } // namespace so3 39 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/core/keypoints_with_normal.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/types.hpp" 28 | 29 | namespace iris 30 | { 31 | struct KeypointsWithNormal { 32 | pcXYZ::Ptr cloud; 33 | pcNormal::Ptr normals; 34 | 35 | KeypointsWithNormal() : cloud(new pcXYZ), 36 | normals(new pcNormal) 37 | { 38 | } 39 | }; 40 | } // namespace iris -------------------------------------------------------------------------------- /openvslam_bridge/src/stereo_bridge.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "bridge.hpp" 28 | 29 | namespace iris 30 | { 31 | class BridgeStereoOpenVSLAM : public BridgeOpenVSLAM 32 | { 33 | public: 34 | BridgeStereoOpenVSLAM() {} 35 | 36 | void setup(const std::string& config_path, const std::string& vocab_path) override; 37 | void execute(const cv::Mat& image0, const cv::Mat& image1); 38 | }; 39 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/core/types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "pcl_/correspondence_estimator.hpp" 28 | #include 29 | #include 30 | #include 31 | 32 | namespace iris 33 | { 34 | using pcXYZ = pcl::PointCloud; 35 | using pcNormal = pcl::PointCloud; 36 | using pcXYZIN = pcl::PointCloud; 37 | using xyzin = pcl::PointXYZINormal; 38 | using crrspEstimator = iris::pcl_::CorrespondenceEstimationBackProjection; 39 | using crrspRejector = pcl::registration::CorrespondenceRejectorDistance; 40 | 41 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/map/parameter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | 29 | namespace iris 30 | { 31 | namespace map 32 | { 33 | struct Parameter { 34 | Parameter( 35 | const std::string& pcd_file, 36 | float voxel_grid_leaf, 37 | float normal_search_radius, 38 | float submap_grid_leaf) 39 | : pcd_file(pcd_file), 40 | voxel_grid_leaf(voxel_grid_leaf), 41 | normal_search_radius(normal_search_radius), 42 | submap_grid_leaf(submap_grid_leaf) {} 43 | 44 | std::string pcd_file; 45 | float voxel_grid_leaf; 46 | float normal_search_radius; 47 | float submap_grid_leaf; 48 | 49 | std::string toString() const 50 | { 51 | std::stringstream ss; 52 | ss << pcd_file << " " << std::to_string(voxel_grid_leaf) << " " << std::to_string(normal_search_radius); 53 | return ss.str(); 54 | } 55 | }; 56 | 57 | } // namespace map 58 | } // namespace iris -------------------------------------------------------------------------------- /openvslam_bridge/src/bridge.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace iris 33 | { 34 | class BridgeOpenVSLAM 35 | { 36 | public: 37 | BridgeOpenVSLAM() {} 38 | ~BridgeOpenVSLAM(); 39 | 40 | void virtual setup(const std::string& config_path, const std::string& vocab_path); 41 | void execute(const cv::Mat& image); 42 | void requestReset(); 43 | 44 | void getLandmarksAndNormals(pcl::PointCloud::Ptr& vslam_data, float height) const; 45 | void setCriteria(unsigned int recollection_, float accuracy_); 46 | std::pair getCriteria() const; 47 | 48 | // return openvslam::tracker_state_t 49 | int getState() const; 50 | 51 | cv::Mat getFrame() const; 52 | 53 | Eigen::Matrix4f getCameraPose() const; 54 | 55 | protected: 56 | unsigned int recollection = 0; 57 | float accuracy = -1; 58 | 59 | std::shared_ptr SLAM_ptr = nullptr; 60 | }; 61 | } // namespace iris 62 | -------------------------------------------------------------------------------- /iris/src/core/util.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/types.hpp" 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace iris 34 | { 35 | namespace util 36 | { 37 | float getScale(const Eigen::MatrixXf& A); 38 | Eigen::Matrix4f applyScaling(const Eigen::Matrix4f& T, float scale); 39 | Eigen::Matrix3f normalizeRotation(const Eigen::MatrixXf& A); 40 | Eigen::Matrix4f normalizePose(const Eigen::Matrix4f& T); 41 | 42 | Eigen::Matrix4f make3DPoseFrom2DPose(float x, float y, float nx, float ny); 43 | 44 | // load 45 | void loadMap( 46 | const std::string& pcd_file, 47 | pcl::PointCloud::Ptr& cloud, 48 | pcl::PointCloud::Ptr& normals, 49 | float grid_leaf, float radius); 50 | 51 | // normal 52 | void transformNormals(const pcNormal& source, pcNormal& target, const Eigen::Matrix4f& T); 53 | void transformXYZINormal(const pcXYZIN::Ptr& all, const pcXYZ::Ptr& points, const pcNormal::Ptr& normals, const Eigen::Matrix4f& T); 54 | 55 | // randomize 56 | Eigen::Matrix3f randomRotation(); 57 | void shufflePointCloud(pcXYZ::Ptr& cloud); 58 | 59 | } // namespace util 60 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/core/math.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "core/math.hpp" 27 | 28 | namespace iris 29 | { 30 | namespace so3 31 | { 32 | namespace 33 | { 34 | constexpr float EPSILON = 1e-7f; 35 | } 36 | Eigen::Matrix3f hat(const Eigen::Vector3f& xi) 37 | { 38 | Eigen::Matrix3f S; 39 | // clang-format off 40 | S << 0, -xi(2), xi(1), 41 | xi(2), 0, -xi(0), 42 | -xi(1), xi(0), 0; 43 | // clang-format on 44 | return S; 45 | } 46 | 47 | Eigen::Vector3f log(const Eigen::Matrix3f& R) 48 | { 49 | Eigen::Vector3f xi = Eigen::Vector3f::Zero(); 50 | float w_length = static_cast(std::acos((R.trace() - 1.0f) * 0.5f)); 51 | if (w_length > EPSILON) { 52 | Eigen::Vector3f tmp; 53 | tmp << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1); 54 | xi = 1.0f / (2.0f * static_cast(std::sin(w_length))) * tmp * w_length; 55 | } 56 | return xi; 57 | } 58 | 59 | Eigen::Matrix3f exp(const Eigen::Vector3f& xi) 60 | { 61 | float theta = xi.norm(); 62 | Eigen::Vector3f axis = xi.normalized(); 63 | 64 | float cos = std::cos(theta); 65 | float sin = std::sin(theta); 66 | 67 | auto tmp1 = cos * Eigen::Matrix3f::Identity(); 68 | auto tmp2 = (1 - cos) * (axis * axis.transpose()); 69 | auto tmp3 = sin * hat(axis); 70 | return tmp1 + tmp2 + tmp3; 71 | } 72 | } // namespace so3 73 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/map/info.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | #include 29 | 30 | namespace iris 31 | { 32 | namespace map 33 | { 34 | struct Info { 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | 37 | float x = std::numeric_limits::quiet_NaN(); 38 | float y = std::numeric_limits::quiet_NaN(); 39 | float theta = std::numeric_limits::quiet_NaN(); 40 | 41 | Info() {} 42 | Info(float x, float y, float theta) : x(x), y(y), theta(theta) {} 43 | 44 | Eigen::Vector2f xy() const { return Eigen::Vector2f(x, y); } 45 | 46 | std::string toString() const 47 | { 48 | return std::to_string(x) + " " + std::to_string(y) + " " + std::to_string(theta); 49 | } 50 | 51 | bool isEqual(const Info& a, const Info& b) const 52 | { 53 | constexpr float EPSILON = 1e-7f; 54 | if (std::fabs(a.x - b.x) > EPSILON) 55 | return false; 56 | if (std::fabs(a.y - b.y) > EPSILON) 57 | return false; 58 | if (std::fabs(a.theta - b.theta) > EPSILON) 59 | return false; 60 | return true; 61 | } 62 | 63 | bool operator==(const Info& other) const 64 | { 65 | return isEqual(*this, other); 66 | } 67 | bool operator!=(const Info& other) const 68 | { 69 | return !isEqual(*this, other); 70 | } 71 | }; 72 | 73 | } // namespace map 74 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/optimize/averager.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/math.hpp" 28 | #include "core/util.hpp" 29 | 30 | namespace iris 31 | { 32 | namespace optimize 33 | { 34 | Eigen::Vector3f calcAverageTransform(const Eigen::Matrix3f& R, const Eigen::Vector3f& t, int n) 35 | { 36 | Eigen::Matrix3f A = Eigen::Matrix3f::Zero(); 37 | 38 | for (int i = 0; i < n; i++) { 39 | Eigen::Matrix3f tmp = Eigen::Matrix3f::Identity(); 40 | for (int j = 0; j < i; j++) { 41 | tmp = R * tmp; 42 | } 43 | A += tmp; 44 | } 45 | 46 | return A.inverse() * t; 47 | } 48 | 49 | Eigen::Matrix4f calcVelocity(const std::list& poses) 50 | { 51 | Eigen::Matrix4f T0 = util::normalizePose(*std::next(poses.begin())); // The latest pose seems to be not reliable 52 | Eigen::Matrix4f Tn = util::normalizePose(*std::prev(poses.end())); 53 | const int dt = static_cast(poses.size()) - 2; 54 | 55 | Eigen::Matrix4f tmp = T0 * Tn.inverse(); 56 | Eigen::Matrix3f R = tmp.topLeftCorner(3, 3); 57 | Eigen::Vector3f t = tmp.topRightCorner(3, 1); 58 | Eigen::Matrix3f root_R = so3::exp(so3::log(R) / dt); 59 | 60 | Eigen::Matrix4f V = Eigen::Matrix4f::Identity(); 61 | V.topLeftCorner(3, 3) = root_R; 62 | V.topRightCorner(3, 1) = calcAverageTransform(root_R, t, dt); 63 | return V; 64 | } 65 | } // namespace optimize 66 | } // namespace iris 67 | -------------------------------------------------------------------------------- /iris/src/optimize/optimizer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/keypoints_with_normal.hpp" 28 | #include "core/types.hpp" 29 | #include "map/map.hpp" 30 | #include 31 | 32 | namespace iris 33 | { 34 | namespace optimize 35 | { 36 | struct Gain { 37 | // for Solver 38 | float scale = 0; 39 | float latitude = 0; 40 | float altitude = 0; 41 | float smooth = 0; 42 | }; 43 | 44 | struct Config { 45 | // for Optimizer 46 | float threshold_translation = 0; 47 | float threshold_rotation = 0; 48 | float distance_max; 49 | float distance_min; 50 | int iteration; 51 | float ref_scale = 1; 52 | Gain gain; 53 | }; 54 | 55 | struct Outcome { 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | pcl::CorrespondencesPtr correspondences = nullptr; 58 | Eigen::Matrix4f T_align; 59 | }; 60 | 61 | class Optimizer 62 | { 63 | public: 64 | void setConfig(const Config& config_) { config = config_; } 65 | 66 | Outcome optimize( 67 | const std::shared_ptr& map_ptr, 68 | const pcXYZIN::Ptr& vslam_data, 69 | const Eigen::Matrix4f& offset_camera, 70 | crrspEstimator& estimator, 71 | const Eigen::Matrix4f& T_initial_align, 72 | const std::list>& vllm_history); 73 | 74 | private: 75 | Config config; 76 | crrspRejector distance_rejector; 77 | }; 78 | } // namespace optimize 79 | } // namespace iris -------------------------------------------------------------------------------- /iris/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | # == Project == 4 | project(iris) 5 | 6 | # == Check C++17 == 7 | include(CheckCXXCompilerFlag) 8 | enable_language(CXX) 9 | check_cxx_compiler_flag("-std=gnu++17" COMPILER_SUPPORTS_CXX17) 10 | if(NOT ${COMPILER_SUPPORTS_CXX17}) 11 | message(FATAL_ERROR "${CMAKE_CXX_COMPILER} doesn't support C++17\n") 12 | endif() 13 | 14 | # == Use C++17 == 15 | set(CMAKE_CXX_STANDARD 17) 16 | message("Compiler:\n\t${CMAKE_CXX_COMPILER} (using C++17)") 17 | 18 | # == Set default build type to release == 19 | if(NOT CMAKE_BUILD_TYPE) 20 | set(CMAKE_BUILD_TYPE "RELEASE") 21 | endif() 22 | string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE) 23 | message("Build Type:\n\t${CMAKE_BUILD_TYPE}") 24 | 25 | # == Clear "CMAKE_CXX_FLAGS" == 26 | set(CMAKE_CXX_FLAGS "") 27 | set(CMAKE_CXX_FLAGS 28 | "${CMAKE_CXX_FLAGS} -pipe -fopenmp -Ofast -lstdc++fs -mfpmath=both -mtune=native" 29 | )# -mtune=native 30 | 31 | # == Set warning flags == 32 | set(CXX_WARNING_FLAGS 33 | -Wall 34 | -Wextra 35 | -Wconversion 36 | -Wswitch-default 37 | -Wdisabled-optimization 38 | -Wformat 39 | -Winit-self 40 | -Woverloaded-virtual 41 | -Wfloat-equal 42 | -Wno-old-style-cast 43 | -Wno-pragmas) 44 | foreach(FLAG IN LISTS CXX_WARNING_FLAGS) 45 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}") 46 | endforeach() 47 | message("CMAKE_CXX_FLAGS:\n\t${CMAKE_CXX_FLAGS}") 48 | 49 | # == Point Cloud Library == 50 | find_package(PCL QUIET REQUIRED COMPONENTS common io registration visualization kdtree) 51 | include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) 52 | link_directories(${PCL_LIBRARY_DIRS}) 53 | message(STATUS "PCL version:\n\t${PCL_VERSION}") 54 | 55 | # == OpenCV == 56 | find_package(OpenCV 3.2 REQUIRED) 57 | message(STATUS "OpenCV version:\n\t${OpenCV_VERSION}") 58 | 59 | # == Eigen3 == 60 | find_package(Eigen3 REQUIRED) 61 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) 62 | message(STATUS "Eigen3 version:\n\t${EIGEN3_VERSION_STRING}") 63 | 64 | # yaml-cpp 65 | find_package(yaml-cpp REQUIRED) 66 | 67 | # == g2o == 68 | find_package(g2o REQUIRED) 69 | set(G2O_LIBS 70 | g2o::core 71 | g2o::stuff 72 | g2o::types_sba 73 | g2o::types_sim3 74 | g2o::solver_dense 75 | g2o::solver_eigen 76 | g2o::solver_csparse 77 | g2o::csparse_extension 78 | ${CXSPARSE_LIBRARIES} 79 | ${SUITESPARSE_LIBRARIES}) 80 | message(STATUS "g2o version:\n\t${g2o_VERSION}") 81 | 82 | # == Catkin == 83 | find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport tf nav_msgs) 84 | catkin_package(CATKIN_DEPENDS cv_bridge image_transport tf nav_msgs) 85 | 86 | # == Headers and sources == 87 | include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) 88 | include_directories(${CMAKE_CURRENT_LIST_DIR}/src) 89 | file(GLOB SOURCES src/**/*.cpp) 90 | message(STATUS "${SOURCES}") 91 | 92 | # == Executable == 93 | add_executable(iris_node src/iris_node.cpp ${SOURCES}) 94 | target_link_libraries(iris_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${G2O_LIBS} 95 | ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES}) 96 | -------------------------------------------------------------------------------- /openvslam_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | # == Project == 4 | project(openvslam_bridge) 5 | 6 | # == Check C++17 == 7 | include(CheckCXXCompilerFlag) 8 | enable_language(CXX) 9 | check_cxx_compiler_flag("-std=gnu++17" COMPILER_SUPPORTS_CXX17) 10 | if(NOT ${COMPILER_SUPPORTS_CXX17}) 11 | message(FATAL_ERROR "${CMAKE_CXX_COMPILER} doesn't support C++17\n") 12 | endif() 13 | 14 | # == Use C++17 == 15 | set(CMAKE_CXX_STANDARD 17) 16 | message("Compiler:\n\t${CMAKE_CXX_COMPILER} (using C++17)") 17 | 18 | # == Set default build type to release == 19 | if(NOT CMAKE_BUILD_TYPE) 20 | set(CMAKE_BUILD_TYPE "RELEASE") 21 | endif() 22 | string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE) 23 | message("Build Type:\n\t${CMAKE_BUILD_TYPE}") 24 | 25 | # == Clear "CMAKE_CXX_FLAGS" == 26 | set(CMAKE_CXX_FLAGS "") 27 | set(CMAKE_CXX_FLAGS 28 | "${CMAKE_CXX_FLAGS} -pipe -fopenmp -Ofast -lstdc++fs -mfpmath=both -mtune=native" 29 | )# -mtune=native 30 | 31 | # == Set warning flags == 32 | set(CXX_WARNING_FLAGS 33 | -Wall 34 | -Wextra 35 | -Wconversion 36 | -Wswitch-default 37 | -Wdisabled-optimization 38 | -Wformat 39 | -Winit-self 40 | -Woverloaded-virtual 41 | -Wfloat-equal 42 | -Wno-old-style-cast 43 | -Wno-pragmas) 44 | foreach(FLAG IN LISTS CXX_WARNING_FLAGS) 45 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}") 46 | endforeach() 47 | message("CMAKE_CXX_FLAGS:\n\t${CMAKE_CXX_FLAGS}") 48 | 49 | # == Point Cloud Library == 50 | find_package(PCL QUIET REQUIRED COMPONENTS common io registration visualization kdtree) 51 | include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) 52 | link_directories(${PCL_LIBRARY_DIRS}) 53 | message(STATUS "PCL version:\n\t${PCL_VERSION}") 54 | 55 | # == OpenCV == 56 | find_package(OpenCV 3.2 REQUIRED) 57 | message(STATUS "OpenCV version:\n\t${OpenCV_VERSION}") 58 | 59 | # == Eigen3 == 60 | find_package(Eigen3 REQUIRED) 61 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) 62 | message(STATUS "Eigen3 version:\n\t${EIGEN3_VERSION_STRING}") 63 | 64 | # == Catkin == 65 | find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport tf) 66 | catkin_package(CATKIN_DEPENDS cv_bridge image_transport tf) 67 | 68 | # == Openvslam == 69 | set(BUILD_EXAMPLES OFF) # disable building examples 70 | set(USE_PANGOLIN_VIEWER OFF) # disbale buidling viewer 71 | add_subdirectory(3rd/openvslam) 72 | 73 | # == Headers and sources == 74 | include_directories(${CMAKE_CURRENT_LIST_DIR}/src) 75 | # cmake-format: off 76 | include_directories( 77 | SYSTEM ${catkin_INCLUDE_DIRS} 78 | ${CMAKE_CURRENT_LIST_DIR}/3rd/openvslam/src 79 | ${CMAKE_CURRENT_LIST_DIR}/3rd/spdlog/include) 80 | # cmake-format: on 81 | 82 | # == Executable == 83 | add_executable(openvslam_bridge_node src/bridge_node.cpp src/bridge.cpp) 84 | target_link_libraries(openvslam_bridge_node ${catkin_LIBRARIES} ${OpenCV_LIBS} openvslam) 85 | 86 | add_executable(openvslam_stereo_bridge_node src/stereo_bridge_node.cpp src/bridge.cpp src/stereo_bridge.cpp) 87 | target_link_libraries(openvslam_stereo_bridge_node ${catkin_LIBRARIES} ${OpenCV_LIBS} openvslam) -------------------------------------------------------------------------------- /iris/src/system/publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "system/publisher.hpp" 27 | 28 | namespace iris 29 | { 30 | bool Publisher::pop(Publication& p) 31 | { 32 | std::lock_guard lock(mtx); 33 | 34 | if (flags[(id + 1) % 2] == false) { 35 | return false; 36 | } 37 | 38 | p = publication[(id + 1) % 2]; 39 | flags[(id + 1) % 2] = false; 40 | return true; 41 | } 42 | 43 | // NOTE: There are many redundant copies 44 | void Publisher::push( 45 | const Eigen::Matrix4f& T_align, 46 | const Eigen::Matrix4f& iris_camera, 47 | const Eigen::Matrix4f& offset_camera, 48 | const pcXYZIN::Ptr& raw_data, 49 | const std::vector>& iris_trajectory, 50 | const std::vector>& offset_trajectory, 51 | const pcl::CorrespondencesPtr& corre, 52 | const map::Info& localmap_info) 53 | { 54 | Publication& tmp = publication[id]; 55 | 56 | tmp.T_align = T_align; 57 | tmp.iris_camera = util::normalizePose(iris_camera); 58 | tmp.offset_camera = util::normalizePose(offset_camera); 59 | tmp.iris_trajectory = iris_trajectory; 60 | tmp.offset_trajectory = offset_trajectory; 61 | tmp.localmap_info = localmap_info; 62 | *tmp.correspondences = *corre; 63 | 64 | util::transformXYZINormal(raw_data, tmp.cloud, tmp.normals, T_align); 65 | 66 | { 67 | std::lock_guard lock(mtx); 68 | flags[id] = true; 69 | id = (id + 1) % 2; 70 | } 71 | } 72 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/publish/publish.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace iris 34 | { 35 | void publishImage(image_transport::Publisher& publisher, const cv::Mat& image); 36 | 37 | void publishPose(const Eigen::Matrix4f& T, const std::string& child_frame_id); 38 | void publishPointcloud(ros::Publisher& publisher, const pcl::PointCloud::Ptr& cloud); 39 | void publishPath(ros::Publisher& publisher, const std::vector>& path); 40 | void publishCorrespondences(ros::Publisher& publisher, 41 | const pcl::PointCloud::Ptr& source, 42 | const pcl::PointCloud::Ptr& target, 43 | const pcl::CorrespondencesPtr& correspondences); 44 | void publishNormal(ros::Publisher& publisher, 45 | const pcl::PointCloud::Ptr& cloud, 46 | const pcl::PointCloud::Ptr& normals); 47 | void publishCovariance(ros::Publisher& publisher, 48 | const pcl::PointCloud::Ptr& cloud, 49 | const pcl::PointCloud::Ptr& normals); 50 | 51 | void publishResetPointcloud(ros::Publisher& publisher); 52 | void publishResetCorrespondences(ros::Publisher& publisher); 53 | 54 | std::function imageCallbackGenerator(cv::Mat& subscribed_image); 55 | std::function compressedImageCallbackGenerator(cv::Mat& subscribed_image); 56 | 57 | } // namespace iris 58 | -------------------------------------------------------------------------------- /iris/src/optimize/types_gicp.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace iris 36 | { 37 | namespace optimize 38 | { 39 | using g2o::Matrix3; 40 | using g2o::Vector3; 41 | using g2o::VertexSim3Expmap; 42 | 43 | class EdgeGICP 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | // point positions 48 | Vector3 pos0, pos1; 49 | // unit normals 50 | Vector3 normal0, normal1; 51 | // rotation matrix for normal 52 | Matrix3 R0, R1; 53 | double weight; 54 | bool plane2plane; 55 | 56 | EdgeGICP(); 57 | 58 | // set up rotation matrix for pos0, pos1 59 | void makeRot0(); // for target 60 | void makeRot1(); // for source 61 | 62 | // returns a precision matrix for point-plane 63 | Matrix3 prec0(number_t e); // for target 64 | Matrix3 prec1(number_t e); // for source 65 | 66 | // return a covariance matrix for plane-plane 67 | Matrix3 cov0(number_t e); // for target 68 | Matrix3 cov1(number_t e); // for source 69 | }; 70 | 71 | class Edge_Sim3_GICP : public g2o::BaseUnaryEdge<3, EdgeGICP, VertexSim3Expmap> 72 | { 73 | public: 74 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 75 | Edge_Sim3_GICP(bool pl_pl = false) : plane2plane(pl_pl) {} 76 | Matrix3 cov0, cov1; 77 | bool plane2plane; 78 | 79 | virtual bool read(std::istream&) { return false; } 80 | virtual bool write(std::ostream&) const { return false; } 81 | void computeError(); 82 | }; 83 | } // namespace optimize 84 | } // namespace iris 85 | -------------------------------------------------------------------------------- /iris/src/system/publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/keypoints_with_normal.hpp" 28 | #include "core/util.hpp" 29 | #include "map/info.hpp" 30 | #include 31 | #include 32 | #include 33 | 34 | namespace iris 35 | { 36 | struct Publication { 37 | Publication() : cloud(new pcXYZ), normals(new pcNormal), correspondences(new pcl::Correspondences) {} 38 | 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | Eigen::Matrix4f iris_camera; 42 | Eigen::Matrix4f T_align; 43 | Eigen::Matrix4f offset_camera; 44 | std::vector> iris_trajectory; 45 | std::vector> offset_trajectory; 46 | map::Info localmap_info; 47 | 48 | pcXYZ::Ptr cloud; 49 | pcNormal::Ptr normals; 50 | pcl::CorrespondencesPtr correspondences; 51 | }; 52 | 53 | // OBSL: thread safe publisher 54 | class Publisher 55 | { 56 | public: 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | Publisher() 59 | { 60 | flags[0] = flags[1] = false; 61 | id = 0; 62 | } 63 | 64 | bool pop(Publication& p); 65 | void push( 66 | const Eigen::Matrix4f& T_align, 67 | const Eigen::Matrix4f& iris_camera, 68 | const Eigen::Matrix4f& offset_camera, 69 | const pcXYZIN::Ptr& raw_data, 70 | const std::vector>& iris_trajectory, 71 | const std::vector>& offset_trajectory, 72 | const pcl::CorrespondencesPtr& corre, 73 | const map::Info& localmap_info); 74 | 75 | private: 76 | int id; 77 | bool flags[2]; 78 | Publication publication[2]; 79 | mutable std::mutex mtx; 80 | }; 81 | 82 | } // namespace iris -------------------------------------------------------------------------------- /openvslam_bridge/src/stereo_bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "stereo_bridge.hpp" 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | namespace iris 41 | { 42 | 43 | void BridgeStereoOpenVSLAM::setup(const std::string& config_path, const std::string& vocab_path) 44 | { 45 | // setup logger 46 | spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$"); 47 | spdlog::set_level(spdlog::level::info); 48 | 49 | // load configuration 50 | std::shared_ptr cfg; 51 | try { 52 | cfg = std::make_shared(config_path); 53 | } catch (const std::exception& e) { 54 | std::cerr << e.what() << std::endl; 55 | exit(EXIT_FAILURE); 56 | } 57 | 58 | // run tracking 59 | if (cfg->camera_->setup_type_ != openvslam::camera::setup_type_t::Stereo) { 60 | std::cout << "Invalid setup type: " + cfg->camera_->get_setup_type_string() << std::endl; 61 | exit(EXIT_FAILURE); 62 | } 63 | 64 | // build a SLAM system 65 | SLAM_ptr = std::make_shared(cfg, vocab_path); 66 | SLAM_ptr->startup(); 67 | SLAM_ptr->disable_loop_detector(); 68 | } 69 | 70 | void BridgeStereoOpenVSLAM::execute(const cv::Mat& image0, const cv::Mat& image1) 71 | { 72 | SLAM_ptr->feed_stereo_frame(image0, image1, 0.05, cv::Mat{}); 73 | 74 | if (SLAM_ptr->get_frame_publisher()->get_tracking_state() == openvslam::tracker_state_t::Lost) { 75 | std::cout << "\n\033[33m ##### Request Reset #####\n\033[m" << std::endl; 76 | requestReset(); 77 | } 78 | } 79 | 80 | 81 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/optimize/aligner.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/types.hpp" 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace iris 35 | { 36 | namespace optimize 37 | { 38 | class Aligner 39 | { 40 | public: 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | Aligner(float scale_gain, float latitude_gain, float altitude_gain, float smooth_gain) 43 | : scale_gain(scale_gain), 44 | latitude_gain(latitude_gain), 45 | altitude_gain(altitude_gain), 46 | smooth_gain(smooth_gain) {} 47 | 48 | Aligner() : Aligner(0, 0, 0, 0) {} 49 | 50 | ~Aligner() {} 51 | 52 | Eigen::Matrix4f estimate7DoF( 53 | Eigen::Matrix4f& T, 54 | const pcXYZIN::Ptr& source_clouds, 55 | const pcl::PointCloud::Ptr& target, 56 | const pcl::CorrespondencesPtr& correspondances, 57 | const Eigen::Matrix4f& offset_camera, 58 | const std::list>& history, 59 | const double ref_scale, 60 | const pcl::PointCloud::Ptr& target_normals = nullptr); 61 | 62 | private: 63 | float scale_gain = 0; 64 | float latitude_gain = 0; 65 | float altitude_gain = 0; 66 | float smooth_gain = 0; 67 | 68 | void setVertexSim3(g2o::SparseOptimizer& optimizer, Eigen::Matrix4f& T); 69 | void setVertexSE3(g2o::SparseOptimizer& optimizer, Eigen::Matrix4f& T); 70 | 71 | void setEdgeRestriction( 72 | g2o::SparseOptimizer& optimizer, 73 | const Eigen::Matrix4f& offset_camera, 74 | const Eigen::Matrix4f& T, 75 | double ref_scale); 76 | 77 | void setEdge7DoFGICP( 78 | g2o::SparseOptimizer& optimizer, 79 | const pcXYZIN::Ptr& source_clouds, 80 | const pcl::PointCloud::Ptr& target, 81 | const pcl::CorrespondencesPtr& correspondances, 82 | const Eigen::Vector3f& camera, 83 | const pcl::PointCloud::Ptr& target_normals); 84 | }; 85 | } // namespace optimize 86 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/optimize/types_gicp.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "optimize/types_gicp.hpp" 27 | 28 | namespace iris 29 | { 30 | namespace optimize 31 | { 32 | EdgeGICP::EdgeGICP() 33 | { 34 | pos0.setZero(); 35 | pos1.setZero(); 36 | normal0 << 0, 0, 1; 37 | normal1 << 0, 0, 1; 38 | R0.setIdentity(); 39 | R1.setIdentity(); 40 | } 41 | 42 | void EdgeGICP::makeRot0() 43 | { 44 | Vector3 y; 45 | y << 0, 1, 0; 46 | R0.row(2) = normal0; 47 | y = y - normal0(1) * normal0; 48 | y.normalize(); 49 | R0.row(1) = y; 50 | R0.row(0) = normal0.cross(R0.row(1)); 51 | } 52 | 53 | void EdgeGICP::makeRot1() 54 | { 55 | Vector3 y; 56 | y << 0, 1, 0; 57 | R1.row(2) = normal1; 58 | y = y - normal1(1) * normal1; 59 | y.normalize(); 60 | R1.row(1) = y; 61 | R1.row(0) = normal1.cross(R1.row(1)); 62 | } 63 | 64 | Matrix3 EdgeGICP::prec0(number_t e) 65 | { 66 | makeRot0(); 67 | Matrix3 prec; 68 | prec << e, 0, 0, 69 | 0, e, 0, 70 | 0, 0, 1; 71 | return R0.transpose() * prec * R0; 72 | } 73 | 74 | Matrix3 EdgeGICP::prec1(number_t e) 75 | { 76 | makeRot1(); 77 | Matrix3 prec; 78 | prec << e, 0, 0, 79 | 0, e, 0, 80 | 0, 0, 1; 81 | return R1.transpose() * prec * R1; 82 | } 83 | 84 | Matrix3 EdgeGICP::cov0(number_t e) 85 | { 86 | makeRot0(); 87 | Matrix3 cov; 88 | cov << 1, 0, 0, 89 | 0, 1, 0, 90 | 0, 0, e; 91 | return R0.transpose() * cov * R0; 92 | } 93 | Matrix3 EdgeGICP::cov1(number_t e) 94 | { 95 | makeRot1(); 96 | Matrix3 cov; 97 | cov << 1, 0, 0, 98 | 0, 1, 0, 99 | 0, 0, e; 100 | return R1.transpose() * cov * R1; 101 | } 102 | 103 | void Edge_Sim3_GICP::computeError() 104 | { 105 | // from to 106 | const VertexSim3Expmap* vp0 = static_cast(_vertices[0]); 107 | // get vp1 point into vp0 frame could be more efficient if we computed this transform just once 108 | Vector3 p1 = vp0->estimate().map(measurement().pos1); 109 | 110 | //TODO: 111 | // Euclidean distance 112 | // _error = measurement().weight * (p1 - measurement().pos0); 113 | _error = (p1 - measurement().pos0); 114 | 115 | if (!plane2plane) 116 | return; 117 | 118 | // NOTE: re-define the information matrix for Plane2Plane ICP 119 | // const Matrix3 R = vp0->estimate().rotation().matrix(); 120 | // information() = (cov0 + R * cov1 * R.transpose()).inverse(); 121 | // information() = (cov0).inverse(); 122 | } 123 | 124 | } // namespace optimize 125 | } // namespace iris 126 | -------------------------------------------------------------------------------- /iris/src/core/config.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include 28 | #include 29 | #include 30 | 31 | namespace iris 32 | { 33 | struct Config { 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | Config() {} 36 | 37 | Config(const std::string& yaml_file) 38 | { 39 | init(yaml_file); 40 | } 41 | 42 | void init(const std::string& yaml_file) 43 | { 44 | YAML::Node node; 45 | try { 46 | node = YAML::LoadFile(yaml_file); 47 | } catch (YAML::ParserException& e) { 48 | std::cout << e.what() << "\n"; 49 | std::cout << "can not open " << yaml_file << std::endl; 50 | exit(1); 51 | } 52 | 53 | { 54 | Eigen::Vector3f t(node["Init.transform"].as>().data()); 55 | Eigen::Vector3f n(node["Init.normal"].as>().data()); 56 | Eigen::Vector3f u(node["Init.upper"].as>().data()); 57 | float s = node["Init.scale"].as(); 58 | 59 | Eigen::Matrix3f R; 60 | n.normalize(); 61 | u.normalize(); 62 | R.row(2) = n; 63 | R.row(1) = (n.dot(u) * n - u).normalized(); // Gram–Schmidt orthonormalization 64 | R.row(0) = R.row(1).cross(R.row(2)); 65 | 66 | Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); 67 | T.topLeftCorner(3, 3) = s * R.transpose(); 68 | T.topRightCorner(3, 1) = t; 69 | T_init = T; 70 | } 71 | 72 | // clang-format off 73 | iteration = node["Iris.iteration"].as(); 74 | scale_gain = node["Iris.scale_gain"].as(); 75 | latitude_gain = node["Iris.latitude_gain"].as(); 76 | altitude_gain = node["Iris.altitude_gain"].as(); 77 | smooth_gain = node["Iris.smooth_gain"].as(); 78 | 79 | distance_min = node["Iris.distance_min"].as(); 80 | distance_max = node["Iris.distance_max"].as(); 81 | converge_translation = node["Iris.converge_translation"].as(); 82 | converge_rotation = node["Iris.converge_rotation"].as(); 83 | 84 | normal_search_leaf = node["Map.normal_search_leaf"].as(); 85 | voxel_grid_leaf = node["Map.voxel_grid_leaf"].as(); 86 | submap_grid_leaf = node["Map.submap_grid_leaf"].as(); 87 | // clang-format on 88 | } 89 | 90 | float distance_min, distance_max; 91 | float scale_gain, latitude_gain, smooth_gain, altitude_gain; 92 | int iteration; 93 | 94 | float converge_translation, converge_rotation; 95 | float normal_search_leaf, voxel_grid_leaf, submap_grid_leaf; 96 | 97 | Eigen::Matrix4f T_init; 98 | }; 99 | } // namespace iris -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Modifications are underway. 2 | 3 | 4 | # *Iris* 5 | * Visual localization in pre-build pointcloud maps. 6 | * ~~**OpenVSLAM** and **VINS-mono** can be used.~~ Modifications are underway. 7 | 8 | 9 | ## Video 10 | [![](https://img.youtube.com/vi/a_BnifwBZC8/0.jpg)](https://www.youtube.com/watch?v=a_BnifwBZC8) 11 | 12 | 13 | ## Submodule 14 | * [OpenVSLAM forked by MapIV](https://github.com/MapIV/openvslam.git) 15 | > ~~[original repository (xdspacelab)](https://github.com/xdspacelab/openvslam)~~ 16 | 17 | 18 | ## Dependency 19 | If you are using ROS, you only need to install `g2o` and `DBoW2`. 20 | * [ROS](http://wiki.ros.org/) 21 | * [OpenCV](https://opencv.org/) >= 3.2 22 | * [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) 23 | * [PCL](https://pointclouds.org/) 24 | * [g2o](https://github.com/RainerKuemmerle/g2o) 25 | * [DBow2](https://github.com/shinsumicco/DBoW2.git) 26 | * Please use the custom version released in [https://github.com/shinsumicco/DBoW2](https://github.com/shinsumicco/DBoW2) 27 | 28 | > ~~see also: [openvslam](https://openvslam.readthedocs.io/en/master/installation.html#dependencies).~~ Modifications are underway. 29 | 30 | ## How to Build 31 | ```bash 32 | mkdir -p catkin_ws/src 33 | cd catkin_ws/src 34 | git clone --recursive https://github.com/MapIV/iris.git 35 | cd .. 36 | catkin_make 37 | ``` 38 | 39 | ## How to Run with Sample Data 40 | ### Download sample data 41 | 1. visual feature file: `orb_vocab.dbow` from [URL](https://www.dropbox.com/s/z8vodds9y6yxg0p/orb_vocab.dbow2?dl=0) 42 | 2. pointcloud map : `kitti_00.pcd` from [URL](https://www.dropbox.com/s/tzdqtsl1p7v1ylo/kitti_00.pcd?dl=0) 43 | 3. rosbag : `kitti_00_stereo.bag` from [URL](https://www.dropbox.com/s/kfouz9gkjefpvb5/kitti_00_stereo.bag?dl=0) 44 | 45 | ### Run with sample data 46 | #### Stereo camera sample 47 | ```bash 48 | roscd iris/../../../ 49 | # download sample data to here (orb_voceb.dbow, kitti_00.pcd, kitti_00_stereo.bag) 50 | ls # > build devel install src orb_vocab.dbow kitti_00.pcd kitti_00_stereo.bag 51 | roslaunch iris stereo_kitti00.launch 52 | roslaunch iris rviz.launch # (on another terminal) 53 | rosbag play kitti_00_stereo.bag # (on another terminal) 54 | ``` 55 | > If the estimated position is misaligned, it can be corrected using `2D Pose Estimate` in rviz. 56 | 57 | #### Monocular camera sample 58 | ```bash 59 | roscd iris/../../../ 60 | # download sample data to here (orb_voceb.dbow, kitti_00.pcd, kitti_00_stereo.bag) 61 | ls # > build devel install src orb_vocab.dbow kitti_00.pcd kitti_00_stereo.bag 62 | roslaunch iris mono_kitti00.launch 63 | roslaunch iris rviz.launch # (on another terminal) 64 | rosbag play kitti_00_stereo.bag # (on another terminal) 65 | ``` 66 | > If the estimated position is misaligned, it can be corrected using `2D Pose Estimate` in rviz. 67 | 68 | 69 | ## How to Run with Your Data 70 | ### All you need to prepare 71 | 1. pointcloud map file (*.pcd) 72 | 1. rosbag (*.bag) 73 | 1. Config file for iris such as `config/sample_iris_config.yaml` 74 | 1. (only if you use OpenVSLAM) Config file for vSLAM such as `config/sample_openvslam_config.yaml` 75 | 76 | ### Run 77 | ```bash 78 | roslaunch iris openvslam.launch iris_config_path:=... 79 | rosbag play yours.bag # (on another terminal) 80 | ``` 81 | 82 | ## License 83 | ~~Iris is provided under the BSD 3-Clause License.~~ 84 | Modifications are underway. 85 | 86 | The following files are derived from third-party libraries. 87 | * `iris/src/optimize/types_gicp.hpp` : part of [g2o](https://github.com/RainerKuemmerle/g2o) (BSD) 88 | * `iris/src/optimize/types_gicp.cpp` : part of [g2o](https://github.com/RainerKuemmerle/g2o) (BSD) 89 | * `iris/src/pcl_/correspondence_estimator.hpp` : part of [pcl](https://github.com/PointCloudLibrary/pcl) (BSD) 90 | * `iris/src/pcl_/correspondence_estimator.cpp` : part of [pcl](https://github.com/PointCloudLibrary/pcl) (BSD) 91 | * `iris/src/pcl_/normal_estimator.hpp` : part of [pcl](https://github.com/PointCloudLibrary/pcl) (BSD) 92 | * `iris/src/pcl_/normal_estimator.cpp` : part of [pcl](https://github.com/PointCloudLibrary/pcl) (BSD) 93 | 94 | 95 | ## Reference 96 | - T. Caselitz, B. Steder, M. Ruhnke, and W. Burgard, “Monocular camera localization in 3d lidar maps,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016, pp. 1926–1931. 97 | > http://www.lifelong-navigation.eu/files/caselitz16iros.pdf 98 | -------------------------------------------------------------------------------- /iris/src/map/map.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/types.hpp" 28 | #include "core/util.hpp" 29 | #include "map/info.hpp" 30 | #include "map/parameter.hpp" 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace iris 40 | { 41 | namespace map 42 | { 43 | 44 | struct HashForPair { 45 | template 46 | size_t operator()(const std::pair& p) const 47 | { 48 | auto hash1 = std::hash{}(p.first); 49 | auto hash2 = std::hash{}(p.second); 50 | 51 | // https://stackoverflow.com/questions/4948780/magic-number-in-boosthash-combine 52 | size_t seed = 0; 53 | seed ^= hash1 + 0x9e3779b9 + (seed << 6) + (seed >> 2); 54 | seed ^= hash2 + 0x9e3779b9 + (seed << 6) + (seed >> 2); 55 | return seed; 56 | } 57 | }; 58 | 59 | class Map 60 | { 61 | public: 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | Map(const Parameter& parameter, const Eigen::Matrix4f& T_init); 64 | 65 | // If the map updates then return true. 66 | bool informCurrentPose(const Eigen::Matrix4f& T); 67 | 68 | // This informs viewer of whether local map updated or not 69 | Info getLocalmapInfo() const 70 | { 71 | return localmap_info; 72 | } 73 | 74 | const pcXYZ::Ptr getTargetCloud() const 75 | { 76 | return local_target_cloud; 77 | } 78 | 79 | const pcXYZ::Ptr getSparseCloud() const 80 | { 81 | return all_sparse_cloud; 82 | } 83 | 84 | const pcNormal::Ptr getTargetNormals() const 85 | { 86 | return local_target_normals; 87 | } 88 | 89 | private: 90 | const std::string cache_file; 91 | const Parameter parameter; 92 | const std::string cache_cloud_file = "iris_cloud.pcd"; 93 | const std::string cache_normals_file = "iris_normals.pcd"; 94 | const std::string cache_sparse_file = "iris_sparse_cloud.pcd"; 95 | 96 | // whole point cloud 97 | pcXYZ::Ptr all_target_cloud; 98 | pcNormal::Ptr all_target_normals; 99 | pcXYZ::Ptr all_sparse_cloud; 100 | 101 | // valid point cloud 102 | pcXYZ::Ptr local_target_cloud; 103 | pcNormal::Ptr local_target_normals; 104 | 105 | // divided point cloud 106 | std::unordered_map, pcXYZ, HashForPair> submap_cloud; 107 | std::unordered_map, pcNormal, HashForPair> submap_normals; 108 | 109 | // [x,y,theta] 110 | Eigen::Vector3f last_grid_center; 111 | Info localmap_info; 112 | 113 | 114 | bool isRecalculationNecessary() const; 115 | bool isUpdateNecessary(const Eigen::Matrix4f& T) const; 116 | void updateLocalmap(const Eigen::Matrix4f& T); 117 | 118 | // return [0,2*pi] 119 | float yawFromPose(const Eigen::Matrix4f& T) const; 120 | 121 | // return [0,pi] 122 | float subtractAngles(float a, float b) const 123 | { 124 | // a,b \in [0,2\pi] 125 | float d = std::fabs(a - b); 126 | if (d > 3.14159f) 127 | return 2.f * 3.14159f - d; 128 | return d; 129 | } 130 | }; 131 | } // namespace map 132 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/pcl_/normal_estimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | 41 | #ifndef PCL_FEATURES_IMPL_NORMAL_3D_H_ 42 | #define PCL_FEATURES_IMPL_NORMAL_3D_H_ 43 | 44 | #include "pcl_/normal_estimator.hpp" 45 | 46 | namespace iris 47 | { 48 | namespace pcl_ 49 | { 50 | /////////////////////////////////////////////////////////////////////////////////////////// 51 | template 52 | void NormalEstimation::computeFeature(PointCloudOut& output) 53 | { 54 | // Allocate enough space to hold the results 55 | // \note This resize is irrelevant for a radiusSearch (). 56 | std::vector nn_indices(k_); 57 | std::vector nn_dists(k_); 58 | 59 | output.is_dense = true; 60 | // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense 61 | if (input_->is_dense) { 62 | // Iterating over the entire index vector 63 | for (size_t idx = 0; idx < indices_->size(); ++idx) { 64 | if (this->searchForNeighbors((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || !computePointNormal(*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) { 65 | output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN(); 66 | 67 | output.is_dense = false; 68 | continue; 69 | } 70 | 71 | flipNormalTowardsViewpoint(input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, 72 | output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); 73 | } 74 | } else { 75 | // Iterating over the entire index vector 76 | for (size_t idx = 0; idx < indices_->size(); ++idx) { 77 | if (!isFinite((*input_)[(*indices_)[idx]]) || this->searchForNeighbors((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || !computePointNormal(*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) { 78 | output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN(); 79 | 80 | output.is_dense = false; 81 | continue; 82 | } 83 | 84 | flipNormalTowardsViewpoint(input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, 85 | output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); 86 | } 87 | } 88 | } 89 | template class NormalEstimation; 90 | 91 | } // namespace pcl_ 92 | } // namespace iris 93 | 94 | #define PCL_INSTANTIATE_NormalEstimation(T, NT) template class PCL_EXPORTS pcl::NormalEstimation; 95 | 96 | #endif // PCL_FEATURES_IMPL_NORMAL_3D_H_ -------------------------------------------------------------------------------- /iris/src/optimize/types_restriction.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "optimize/types_restriction.hpp" 27 | 28 | namespace iris 29 | { 30 | namespace optimize 31 | { 32 | void Edge_Scale_Restriction::computeError() 33 | { 34 | const VertexSim3Expmap* vp0 = static_cast(_vertices[0]); 35 | double scale = vp0->estimate().scale(); 36 | const double ref_scale = measurement(); 37 | 38 | double diff = (ref_scale - scale); 39 | double e = 0; 40 | if (diff > 0) e = diff; 41 | if (diff < 0) e = -diff / (scale + 1e-6); 42 | 43 | _error(0) = gain * e; 44 | } 45 | 46 | void Edge_Altitude_Restriction::computeError() 47 | { 48 | const VertexSim3Expmap* vp0 = static_cast(_vertices[0]); 49 | Eigen::Vector3d now = vp0->estimate().map(measurement()); 50 | _error(0) = gain * now.z(); 51 | } 52 | 53 | void Edge_Latitude_Restriction::computeError() 54 | { 55 | const VertexSim3Expmap* vp0 = static_cast(_vertices[0]); 56 | 57 | // Because Visual-SLAM hundle the direction in front of camera as the Z-axis, 58 | // the alignment transform contains the rotation which converts Z-axis(in front of camera) to X-axis(in front of base_link) 59 | Eigen::Matrix3d R = vp0->estimate().rotation().toRotationMatrix() * offset_rotation; 60 | Eigen::Vector3d b(0, -1, 0); 61 | 62 | Eigen::Vector3d Rb = R * b; 63 | // Rb get (0,0,1) when the camera doesn't pitch and roll. 64 | // If the camera roll, Rb gets approximately (0, e, 1-e). 65 | // If the camera pitch, Rb gets approximately (e, 0, 1-e). 66 | // Therefore, 1-Rb.z() means how the camera roll or pitch. 67 | 68 | double swing = 1 - Rb.z(); 69 | 70 | // an angle of the camera rolling and pitching larger than acos(0.75) = 41[deg] 71 | if (swing > 0.20) 72 | _error(0) = 1e4; // infinity loss 73 | // the angle is enough small. 74 | else 75 | _error(0) = gain * swing; 76 | } 77 | 78 | void Edge_Euclid_Restriction::computeError() 79 | { 80 | const VertexSim3Expmap* vp0 = static_cast(_vertices[0]); 81 | 82 | // Eigen::Matrix3d R = vp0->estimate().rotation().toRotationMatrix(); 83 | Eigen::Vector3d t = vp0->estimate().translation(); 84 | // double s = vp0->estimate().scale(); 85 | 86 | // double e1 = (R - R_init).trace(); 87 | double e2 = (t - t_init).norm(); 88 | // double e3 = (s - s_init); 89 | 90 | // TODO: NOTE: 91 | _error(0) = gain * e2; 92 | } 93 | 94 | // void Edge_Smooth_Restriction::computeError() 95 | // { 96 | // TODO: 97 | // const VertexSim3Expmap* vp0 = static_cast(_vertices[0]); 98 | 99 | // double min= 100 | // Eigen::Vector3f t0 = measurement().at(0); 101 | // for (size_t i = 1, N = measurement().size(); i < N - 1; i++) { 102 | // Eigen::Vector3f t1 = measurement().at(i); 103 | 104 | // t0 = t1; 105 | // } 106 | // Eigen::Vector3d older = measurement().older_pos; 107 | // Eigen::Vector3d old = measurement().old_pos; 108 | // Eigen::Vector3d now = vp0->estimate().map(measurement().camera_pos); 109 | 110 | // double old_vel = (old - older).norm(); 111 | // double vel = (now - old).norm(); 112 | 113 | // double thr = 0.1; 114 | // double dv = vel - old_vel; 115 | // // Usually, the larger the velocity, the better 116 | // if (dv < thr) 117 | // _error(0) = gain * (thr - dv) + 0.5 * (vel - old_vel); 118 | // else 119 | // _error(0) = 0.5 * (vel - old_vel); 120 | // } 121 | 122 | } // namespace optimize 123 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/optimize/types_restriction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/util.hpp" 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace iris 38 | { 39 | namespace optimize 40 | { 41 | using g2o::VertexSim3Expmap; 42 | 43 | class VelocityModel 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | Eigen::Vector3d camera_pos, old_pos, older_pos; 49 | 50 | VelocityModel() 51 | { 52 | old_pos.setZero(); 53 | older_pos.setZero(); 54 | camera_pos.setZero(); 55 | } 56 | 57 | double velocity() const 58 | { 59 | return (old_pos - older_pos).norm(); 60 | } 61 | }; 62 | 63 | class Edge_Scale_Restriction : public g2o::BaseUnaryEdge<1, double, VertexSim3Expmap> 64 | { 65 | private: 66 | double gain; 67 | 68 | public: 69 | Edge_Scale_Restriction(double gain = 1.0) : gain(gain) {} 70 | 71 | virtual bool read(std::istream&) { return false; } 72 | virtual bool write(std::ostream&) const { return false; } 73 | void computeError(); 74 | }; 75 | 76 | class Edge_Altitude_Restriction : public g2o::BaseUnaryEdge<1, Eigen::Vector3d, VertexSim3Expmap> 77 | { 78 | private: 79 | double gain; 80 | 81 | public: 82 | Edge_Altitude_Restriction(double gain = 1.0) : gain(gain) {} 83 | 84 | virtual bool read(std::istream&) { return false; } 85 | virtual bool write(std::ostream&) const { return false; } 86 | void computeError(); 87 | }; 88 | 89 | class Edge_Latitude_Restriction : public g2o::BaseUnaryEdge<1, double, VertexSim3Expmap> 90 | { 91 | private: 92 | Eigen::Matrix3d offset_rotation; 93 | double gain; 94 | 95 | public: 96 | Edge_Latitude_Restriction(const Eigen::Matrix3d& offset_rotation, double gain = 1.0) : offset_rotation(offset_rotation), 97 | gain(gain) {} 98 | 99 | virtual bool read(std::istream&) { return false; } 100 | virtual bool write(std::ostream&) const { return false; } 101 | void computeError(); 102 | }; 103 | 104 | class Edge_Euclid_Restriction : public g2o::BaseUnaryEdge<1, double, VertexSim3Expmap> 105 | { 106 | private: 107 | Eigen::Matrix3d R_init; 108 | Eigen::Vector3d t_init; 109 | double s_init; 110 | double gain; 111 | 112 | public: 113 | Edge_Euclid_Restriction(const Eigen::Matrix4f& T_init, double gain = 1.0) : gain(gain) 114 | { 115 | Eigen::Matrix3f sR = T_init.topLeftCorner(3, 3); 116 | R_init = util::normalizeRotation(sR).cast(); 117 | t_init = T_init.topRightCorner(3, 1).cast(); 118 | s_init = util::getScale(sR); 119 | } 120 | 121 | virtual bool read(std::istream&) { return false; } 122 | virtual bool write(std::ostream&) const { return false; } 123 | void computeError(); 124 | }; 125 | 126 | // class Edge_Smooth_Restriction : public g2o::BaseUnaryEdge<1, std::vector, VertexSim3Expmap> 127 | // { 128 | // private: 129 | // double gain; 130 | 131 | // public: 132 | // Edge_Smooth_Restriction(double gain = 1.0) : gain(gain) {} 133 | 134 | // virtual bool read(std::istream&) { return false; } 135 | // virtual bool write(std::ostream&) const { return false; } 136 | // void computeError(); 137 | // }; 138 | 139 | 140 | } // namespace optimize 141 | } // namespace iris 142 | -------------------------------------------------------------------------------- /iris/src/system/system.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | #include "core/config.hpp" 28 | #include "core/types.hpp" 29 | #include "core/util.hpp" 30 | #include "map/map.hpp" 31 | #include "map/parameter.hpp" 32 | #include "optimize/optimizer.hpp" 33 | #include "system/publisher.hpp" 34 | #include 35 | #include 36 | 37 | namespace iris 38 | { 39 | enum IrisState { 40 | Inittializing = 0, 41 | Tracking = 1, 42 | Lost = 2, 43 | Relocalizing = 3, 44 | }; 45 | 46 | class System 47 | { 48 | public: 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | 51 | // ===== for Main ==== 52 | System(const Config& config_, const std::shared_ptr& map_); 53 | 54 | int execute(int vslam_state, const Eigen::Matrix4f& T_vslam, const pcXYZIN::Ptr& vslam_data); 55 | 56 | public: 57 | Eigen::Matrix4f getT() const 58 | { 59 | return T_world; 60 | } 61 | 62 | void setImuPrediction(const Eigen::Matrix4f& T_world_) 63 | { 64 | T_imu = T_world_; 65 | } 66 | 67 | const std::shared_ptr getMap() const 68 | { 69 | return map; 70 | } 71 | 72 | void requestReset() 73 | { 74 | reset_requested.store(true); 75 | } 76 | 77 | bool popPublication(Publication& p) 78 | { 79 | return publisher.pop(p); 80 | } 81 | 82 | optimize::Gain getOptimizeGain() const 83 | { 84 | std::lock_guard lock(optimize_gain_mutex); 85 | return thread_safe_optimize_gain; 86 | } 87 | 88 | void setOptimizeGain(const optimize::Gain& gain_) 89 | { 90 | std::lock_guard lock(optimize_gain_mutex); 91 | thread_safe_optimize_gain = gain_; 92 | } 93 | 94 | Eigen::Matrix4f getTWorld() const 95 | { 96 | return T_world; 97 | } 98 | 99 | void specifyTWorld(const Eigen::Matrix4f& specified_T_world) 100 | { 101 | // std::cout << "last T_align\n" 102 | // << T_align << std::endl; 103 | // std::cout << "last T_world\n" 104 | // << T_world << std::endl; 105 | // std::cout << "last T_vslam\n" 106 | // << last_T_vslam << std::endl; 107 | 108 | float scale = util::getScale(T_world); 109 | auto scaled_new_T_world = util::applyScaling(specified_T_world, scale); 110 | // std::cout << "scaled_new_T_world\n" 111 | // << scaled_new_T_world << std::endl; 112 | T_align = scaled_new_T_world * last_T_vslam.inverse(); 113 | 114 | std::cout << "new T_align\n" 115 | << T_align << std::endl; 116 | } 117 | 118 | private: 119 | void updateOptimizeGain() 120 | { 121 | std::lock_guard lock(optimize_gain_mutex); 122 | optimize_config.gain = thread_safe_optimize_gain; 123 | } 124 | 125 | // ==== private member ==== 126 | optimize::Gain thread_safe_optimize_gain; 127 | optimize::Config optimize_config; 128 | optimize::Optimizer optimizer; 129 | mutable std::mutex optimize_gain_mutex; 130 | 131 | std::vector> iris_trajectory; 132 | std::vector> offset_trajectory; 133 | 134 | IrisState iris_state; 135 | 136 | 137 | std::atomic reset_requested = false; 138 | 139 | Config config; 140 | std::shared_ptr map; 141 | 142 | Eigen::Matrix4f T_align = Eigen::Matrix4f::Identity(); 143 | Eigen::Matrix4f T_world = Eigen::Matrix4f::Identity(); 144 | Eigen::Matrix4f T_imu = Eigen::Matrix4f::Zero(); 145 | Eigen::Matrix4f last_T_vslam = Eigen::Matrix4f::Identity(); 146 | 147 | pcl::CorrespondencesPtr correspondences; 148 | crrspEstimator estimator; 149 | 150 | map::Info localmap_info; 151 | Publisher publisher; 152 | 153 | 154 | // for relozalization 155 | Eigen::Matrix4f iris_velocity; 156 | const int history = 30; 157 | std::list> iris_history; 158 | }; 159 | 160 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/optimize/optimizer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "optimize/optimizer.hpp" 27 | #include "core/util.hpp" 28 | #include "optimize/aligner.hpp" 29 | #include 30 | 31 | namespace iris 32 | { 33 | namespace optimize 34 | { 35 | Outcome Optimizer::optimize( 36 | const std::shared_ptr& map_ptr, 37 | const pcXYZIN::Ptr& vslam_data, 38 | const Eigen::Matrix4f& offset_camera, 39 | crrspEstimator& estimator, 40 | const Eigen::Matrix4f& T_initial_align, 41 | const std::list>& vllm_history) 42 | { 43 | pcXYZ::Ptr tmp_cloud(new pcXYZ); 44 | pcNormal::Ptr tmp_normals(new pcNormal); 45 | pcl::CorrespondencesPtr correspondences(new pcl::Correspondences); 46 | 47 | Eigen::Matrix4f T_align = T_initial_align; 48 | 49 | auto distance_generator = [=](int itr) -> float { 50 | const float max = config.distance_max; 51 | const float min = config.distance_min; 52 | const float N = static_cast(config.iteration); 53 | 54 | if (N <= 1) 55 | return min; 56 | 57 | return min + (N - 1 - static_cast(itr)) * (max - min) / (N - 1); 58 | }; 59 | 60 | for (int itr = 0; itr < config.iteration; itr++) { 61 | std::cout << "itration= \033[32m" << itr << "\033[m"; 62 | 63 | util::transformXYZINormal(vslam_data, tmp_cloud, tmp_normals, T_align); 64 | 65 | // TODO: We should enable the estimator handle the PointXYZINormal 66 | estimator.setInputSource(tmp_cloud); 67 | estimator.setSourceNormals(tmp_normals); 68 | estimator.determineCorrespondences(*correspondences); 69 | 70 | std::cout << " ,raw_correspondences= \033[32m" << correspondences->size() << "\033[m"; 71 | 72 | // NOTE: distance_rejector doesn't seemd to work well. 73 | // Reject too far correspondences 74 | float distance = distance_generator(itr); 75 | distance_rejector.setInputCorrespondences(correspondences); 76 | distance_rejector.setMaximumDistance(distance * distance); 77 | distance_rejector.getCorrespondences(*correspondences); 78 | std::cout << " ,refined_correspondecnes= \033[32m" << correspondences->size() << " @ " << distance << " \033[m" << std::endl; 79 | 80 | if (correspondences->size() < 10) { 81 | std::cout << "\033[33mSuspend optimization iteration because it have not enough correspondences\033[m" << std::endl; 82 | break; 83 | } 84 | 85 | 86 | Eigen::Matrix4f vllm_camera = T_align * offset_camera; 87 | Eigen::Matrix4f last_camera = vllm_camera; 88 | 89 | // Align pointclouds 90 | optimize::Aligner aligner(config.gain.scale, config.gain.latitude, config.gain.altitude, config.gain.smooth); 91 | T_align = aligner.estimate7DoF( 92 | T_align, vslam_data, map_ptr->getTargetCloud(), correspondences, 93 | offset_camera, vllm_history, config.ref_scale, map_ptr->getTargetNormals()); 94 | 95 | // Integrate 96 | vllm_camera = T_align * offset_camera; 97 | 98 | // Get Inovation 99 | float scale = util::getScale(vllm_camera); 100 | float update_transform = (last_camera - vllm_camera).topRightCorner(3, 1).norm(); // called "Euclid distance" 101 | float update_rotation = (last_camera - vllm_camera).topLeftCorner(3, 3).norm() / scale; // called "chordal distance" 102 | std::cout << "update= \033[33m" << update_transform << " \033[m,\033[33m " << update_rotation << "\033[m" << std::endl; 103 | 104 | std::cout << "T_align\n\033[4;36m" 105 | << T_align << "\033[m" << std::endl; 106 | 107 | if (config.threshold_translation > update_transform 108 | && config.threshold_rotation > update_rotation) 109 | break; 110 | } 111 | 112 | Outcome outcome; 113 | outcome.correspondences = correspondences; 114 | outcome.T_align = T_align; 115 | return outcome; 116 | } 117 | 118 | 119 | } // namespace optimize 120 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/system/system.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "system/system.hpp" 27 | #include "optimize/aligner.hpp" 28 | #include "optimize/averager.hpp" 29 | #include "optimize/optimizer.hpp" 30 | #include 31 | #include 32 | 33 | namespace iris 34 | { 35 | 36 | System::System(const Config& config_, const std::shared_ptr& map_) 37 | { 38 | config = config_; 39 | correspondences = pcl::CorrespondencesPtr(new pcl::Correspondences); 40 | map = map_; 41 | 42 | // System starts in initialization mode 43 | iris_state = IrisState::Inittializing; 44 | 45 | // Setup correspondence estimator 46 | estimator.setInputTarget(map->getTargetCloud()); 47 | estimator.setTargetNormals(map->getTargetNormals()); 48 | estimator.setKSearch(10); 49 | 50 | localmap_info = map->getLocalmapInfo(); 51 | 52 | 53 | T_world = config.T_init; 54 | T_align.setIdentity(); 55 | iris_velocity.setZero(); 56 | 57 | // for optimizer module 58 | optimize::Gain gain; 59 | gain.scale = config.scale_gain; 60 | gain.smooth = config.smooth_gain; 61 | gain.altitude = config.altitude_gain; 62 | gain.latitude = config.latitude_gain; 63 | 64 | optimize_config.gain = gain; 65 | optimize_config.distance_min = config.distance_min; 66 | optimize_config.distance_max = config.distance_max; 67 | optimize_config.iteration = config.iteration; 68 | optimize_config.threshold_rotation = config.converge_rotation; 69 | optimize_config.threshold_translation = config.converge_translation; 70 | optimize_config.ref_scale = util::getScale(config.T_init); 71 | 72 | // During the constructor funtion, there is no way to access members from other threads 73 | thread_safe_optimize_gain = optimize_config.gain; 74 | 75 | for (int i = 0; i < history; i++) 76 | iris_history.push_front(T_world); 77 | } 78 | 79 | int System::execute(int vslam_state, const Eigen::Matrix4f& T_vslam, const pcXYZIN::Ptr& vslam_data) 80 | { 81 | // ==================== 82 | if (iris_state == IrisState::Inittializing) { 83 | // NOTE: "2" means openvslam::tracking_state_t::Tracking 84 | if (vslam_state == 2) iris_state = IrisState::Tracking; 85 | T_align = T_world; 86 | } 87 | 88 | // ======================= 89 | if (iris_state == IrisState::Lost) { 90 | std::cerr << "\033[31mIrisState::Lost has not been implemented yet.\033[m" << std::endl; 91 | exit(1); 92 | } 93 | 94 | // ==================== 95 | if (iris_state == IrisState::Relocalizing) { 96 | std::cerr << "\033[31mIrisState::Relocalizing has not been implemented yet.\033[m" << std::endl; 97 | exit(1); 98 | } 99 | 100 | // ==================== 101 | if (iris_state == IrisState::Tracking) { 102 | // Optimization 103 | updateOptimizeGain(); 104 | optimizer.setConfig(optimize_config); 105 | Eigen::Matrix4f T_initial_align = T_align; 106 | 107 | optimize::Outcome outcome = optimizer.optimize( 108 | map, vslam_data, T_vslam, estimator, T_initial_align, iris_history); 109 | correspondences = outcome.correspondences; 110 | T_align = outcome.T_align; 111 | } 112 | 113 | // update the pose in the world 114 | T_world = T_align * T_vslam; 115 | 116 | // Update local map 117 | map->informCurrentPose(T_world); 118 | map::Info new_localmap_info = map->getLocalmapInfo(); 119 | 120 | // Reinitialize correspondencesEstimator 121 | if (localmap_info != new_localmap_info) { 122 | localmap_info = new_localmap_info; 123 | correspondences->clear(); 124 | estimator.setInputTarget(map->getTargetCloud()); 125 | estimator.setTargetNormals(map->getTargetNormals()); 126 | } 127 | 128 | // Update history 129 | iris_history.pop_back(); 130 | iris_history.push_front(T_world); 131 | 132 | // Pubush for the viewer 133 | iris_trajectory.push_back(T_world.topRightCorner(3, 1)); 134 | offset_trajectory.push_back((config.T_init * T_vslam).topRightCorner(3, 1)); 135 | publisher.push( 136 | T_align, T_world, config.T_init * T_vslam, 137 | vslam_data, iris_trajectory, 138 | offset_trajectory, correspondences, localmap_info); 139 | 140 | last_T_vslam = T_vslam; 141 | 142 | return iris_state; 143 | } 144 | } // namespace iris -------------------------------------------------------------------------------- /openvslam_bridge/src/bridge_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "bridge.hpp" 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | void publishPose(const Eigen::Matrix4f& T, const std::string& child_frame_id) 37 | { 38 | static tf::TransformBroadcaster br; 39 | tf::Transform transform; 40 | transform.setFromOpenGLMatrix(T.cast().eval().data()); 41 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", child_frame_id)); 42 | } 43 | 44 | int main(int argc, char* argv[]) 45 | { 46 | ros::init(argc, argv, "openvslam_bridge_node"); 47 | 48 | // Get rosparams 49 | ros::NodeHandle pnh("~"); 50 | bool is_image_compressed; 51 | int recollection = 30; 52 | std::string vocab_path, vslam_config_path, image_topic_name; 53 | pnh.getParam("vocab_path", vocab_path); 54 | pnh.getParam("vslam_config_path", vslam_config_path); 55 | pnh.getParam("image_topic_name0", image_topic_name); 56 | pnh.getParam("is_image_compressed", is_image_compressed); 57 | pnh.getParam("keyframe_recollection", recollection); 58 | ROS_INFO("vocab_path: %s, vslam_config_path: %s, image_topic_name: %s, is_image_compressed: %d", 59 | vocab_path.c_str(), vslam_config_path.c_str(), image_topic_name.c_str(), is_image_compressed); 60 | 61 | 62 | const int lower_threshold_of_points = 1500; 63 | const int upper_threshold_of_points = 2000; 64 | 65 | // Setup subscriber 66 | ros::NodeHandle nh; 67 | image_transport::ImageTransport it(nh); 68 | ros::Time subscribed_stamp; 69 | cv::Mat subscribed_image; 70 | image_transport::TransportHints hints("raw"); 71 | if (is_image_compressed) hints = image_transport::TransportHints("compressed"); 72 | 73 | auto callback = [&subscribed_image, &subscribed_stamp](const sensor_msgs::ImageConstPtr& msg) -> void { 74 | cv_bridge::CvImagePtr cv_ptr; 75 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 76 | subscribed_image = cv_ptr->image.clone(); 77 | subscribed_stamp = msg->header.stamp; 78 | }; 79 | 80 | 81 | image_transport::Subscriber image_subscriber = it.subscribe(image_topic_name, 5, callback, ros::VoidPtr(), hints); 82 | 83 | // Setup publisher 84 | ros::Publisher vslam_publisher = nh.advertise>("iris/vslam_data", 1); 85 | image_transport::Publisher image_publisher = it.advertise("iris/processed_image", 5); 86 | 87 | // Setup for OpenVSLAM 88 | iris::BridgeOpenVSLAM bridge; 89 | bridge.setup(vslam_config_path, vocab_path); 90 | 91 | std::chrono::system_clock::time_point m_start; 92 | ros::Rate loop_rate(20); 93 | float accuracy = 0.5f; 94 | pcl::PointCloud::Ptr vslam_data(new pcl::PointCloud); 95 | std::ofstream ofs_time("vslam_time.csv"); 96 | 97 | // Start main loop 98 | ROS_INFO("start main loop."); 99 | while (ros::ok()) { 100 | if (!subscribed_image.empty()) { 101 | m_start = std::chrono::system_clock::now(); // start timer 102 | ros::Time process_stamp = subscribed_stamp; 103 | 104 | // process OpenVSLAM 105 | bridge.execute(subscribed_image); 106 | bridge.setCriteria(recollection, accuracy); 107 | bridge.getLandmarksAndNormals(vslam_data, std::numeric_limits::max()); 108 | 109 | // Reset input 110 | subscribed_image = cv::Mat(); 111 | 112 | // Update threshold to adjust the number of points 113 | if (vslam_data->size() < lower_threshold_of_points && accuracy > 0.10) accuracy -= 0.01f; 114 | if (vslam_data->size() > upper_threshold_of_points && accuracy < 0.90) accuracy += 0.01f; 115 | std::cout << "accuracy: " << accuracy << std::endl; 116 | { 117 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", bridge.getFrame()).toImageMsg(); 118 | image_publisher.publish(msg); 119 | } 120 | { 121 | pcl_conversions::toPCL(process_stamp, vslam_data->header.stamp); 122 | vslam_data->header.frame_id = "world"; 123 | vslam_publisher.publish(vslam_data); 124 | } 125 | 126 | // Inform processing time 127 | std::stringstream ss; 128 | long time_ms = std::chrono::duration_cast(std::chrono::system_clock::now() - m_start).count(); 129 | ss << "processing time= \033[35m" 130 | << time_ms 131 | << "\033[m ms"; 132 | ofs_time << time_ms << std::endl; 133 | ROS_INFO("%s", ss.str().c_str()); 134 | } 135 | publishPose(bridge.getCameraPose().inverse(), "iris/vslam_pose"); 136 | 137 | // Spin and wait 138 | loop_rate.sleep(); 139 | ros::spinOnce(); 140 | } 141 | 142 | ROS_INFO("Finalize openvslam_bridge::bridge_node"); 143 | return 0; 144 | } -------------------------------------------------------------------------------- /iris/src/pcl_/correspondence_estimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2012-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * $Id$ 37 | * 38 | */ 39 | #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 40 | #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 41 | 42 | #include "pcl_/correspondence_estimator.hpp" 43 | #include 44 | 45 | namespace iris 46 | { 47 | namespace pcl_ 48 | { 49 | // Set epsilon > 1 when the normal vector is considered 50 | Eigen::Matrix3f calcInversedCovariance(const Eigen::Vector3f& n, float epsilon = 1.0f) 51 | { 52 | Eigen::Vector3f n0 = n.normalized(); 53 | Eigen::Vector3f e = Eigen::Vector3f::UnitX(); 54 | if (e.dot(n0) > 1 - 1e-6) e = Eigen::Vector3f::UnitY(); 55 | 56 | Eigen::Vector3f n1 = (e - e.dot(n0) * n0).normalized(); 57 | Eigen::Vector3f n2 = n0.cross(n1); 58 | 59 | Eigen::Matrix3f R; 60 | // |n1_x, n1_y, n1_z| 61 | // R = |n2_x, n2_y, n2_z| 62 | // |n_x , n_y , n_z| 63 | R.row(0) = n1.transpose(); 64 | R.row(1) = n2.transpose(); 65 | R.row(2) = n0.transpose(); 66 | 67 | // clang-format off 68 | Eigen::Matrix3f inv_cov; 69 | inv_cov << epsilon, 0, 0, 70 | 0,epsilon, 0, 71 | 0, 0, 1; 72 | // clang-format on 73 | 74 | return R.transpose() * inv_cov * R; 75 | } 76 | 77 | 78 | /////////////////////////////////////////////////////////////////////////////////////////// 79 | template 80 | bool CorrespondenceEstimationBackProjection::initCompute() 81 | { 82 | if (!source_normals_ || !target_normals_) { 83 | PCL_WARN("[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName().c_str()); 84 | return (false); 85 | } 86 | 87 | return (pcl::registration::CorrespondenceEstimationBase::initCompute()); 88 | } 89 | 90 | /////////////////////////////////////////////////////////////////////////////////////////// 91 | template 92 | void CorrespondenceEstimationBackProjection::determineCorrespondences( 93 | pcl::Correspondences& correspondences, double max_distance) 94 | { 95 | if (!initCompute()) 96 | return; 97 | 98 | correspondences.resize(indices_->size()); 99 | 100 | std::vector nn_indices(k_); 101 | std::vector nn_dists(k_); 102 | 103 | float min_dist = std::numeric_limits::max(); 104 | int min_index = 0; 105 | float min_output_dist = 0; 106 | 107 | pcl::Correspondence corr; 108 | unsigned int nr_valid_correspondences = 0; 109 | 110 | // Check if the template types are the same. If true, avoid a copy. 111 | // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! 112 | if (pcl::isSamePointType()) { 113 | 114 | PointTarget pt; 115 | // Iterate over the input set of source indices 116 | for (std::vector::const_iterator idx_i = indices_->begin(); idx_i != indices_->end(); ++idx_i) { 117 | 118 | min_dist = std::numeric_limits::max(); 119 | Eigen::Vector3f input_point = input_->points[*idx_i].getVector3fMap(); 120 | // Eigen::Vector3f input_normal = source_normals_->points[*idx_i].getNormalVector3fMap(); 121 | 122 | Eigen::Vector3f offset_point = input_point; 123 | tree_->nearestKSearch(PointSource(offset_point.x(), offset_point.y(), offset_point.z()), k_, nn_indices, nn_dists); 124 | 125 | // Find the best correspondence 126 | for (size_t j = 0; j < nn_indices.size(); j++) { 127 | Eigen::Vector3f target_point = target_->points[nn_indices[j]].getVector3fMap(); 128 | Eigen::Vector3f target_normal = target_normals_->points[nn_indices[j]].getNormalVector3fMap(); 129 | Eigen::Matrix3f information_matrix = calcInversedCovariance(target_normal, 0.1f); 130 | 131 | Eigen::Vector3f e = target_point - input_point; 132 | float dist = e.dot(information_matrix * e); 133 | 134 | if (dist < min_dist) { 135 | min_dist = dist; 136 | min_index = nn_indices[j]; 137 | min_output_dist = nn_dists[j]; 138 | } 139 | } 140 | 141 | if (min_dist > max_distance) 142 | continue; 143 | 144 | corr.index_query = *idx_i; 145 | corr.index_match = min_index; 146 | corr.distance = min_output_dist; 147 | correspondences[nr_valid_correspondences++] = corr; 148 | } 149 | } else { 150 | PCL_WARN("called the NOT implemented function in CorrespondenceEstimationBackprojection::determinCorrespondence!\n", getClassName().c_str()); 151 | } 152 | correspondences.resize(nr_valid_correspondences); 153 | deinitCompute(); 154 | } 155 | template class CorrespondenceEstimationBackProjection; 156 | 157 | } // namespace pcl_ 158 | } // namespace iris 159 | 160 | #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 161 | -------------------------------------------------------------------------------- /iris/src/core/util.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "core/util.hpp" 27 | #include "pcl_/normal_estimator.hpp" 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace iris 38 | { 39 | 40 | namespace util 41 | { 42 | 43 | namespace 44 | { 45 | float getScale_(const Eigen::MatrixXf& R) 46 | { 47 | return static_cast(std::sqrt((R.transpose() * R).trace() / 3.0)); 48 | } 49 | 50 | // return the closest rotatin matrix 51 | Eigen::Matrix3f normalize_(const Eigen::Matrix3f& R) 52 | { 53 | Eigen::JacobiSVD svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); 54 | Eigen::Matrix3f U = svd.matrixU(); 55 | Eigen::Matrix3f Vt = svd.matrixV().transpose(); 56 | if (R.determinant() < 0) { 57 | return -U * Vt; 58 | } 59 | 60 | return U * Vt; 61 | } 62 | } // namespace 63 | 64 | Eigen::Matrix4f make3DPoseFrom2DPose(float x, float y, float nx, float ny) 65 | { 66 | Eigen::Matrix4f T; 67 | T.setIdentity(); 68 | 69 | T(0, 3) = x; 70 | T(1, 3) = y; 71 | float theta = std::atan2(ny, nx); 72 | Eigen::Matrix3f R; 73 | R << 0, 0, 1, 74 | -1, 0, 0, 75 | 0, -1, 0; 76 | T.topLeftCorner(3, 3) = Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()).toRotationMatrix() * R; 77 | return T; 78 | } 79 | 80 | Eigen::Matrix4f applyScaling(const Eigen::Matrix4f& T, float scale) 81 | { 82 | Eigen::Matrix3f R = normalizeRotation(T); 83 | Eigen::Matrix4f scaled; 84 | scaled.setIdentity(); 85 | scaled.rightCols(1) = T.rightCols(1); 86 | scaled.topLeftCorner(3, 3) = scale * R; 87 | return scaled; 88 | } 89 | 90 | // get scale factor from rotation matrix 91 | float getScale(const Eigen::MatrixXf& A) 92 | { 93 | if (A.cols() == 3) 94 | return getScale_(A); 95 | else if (A.cols() == 4) 96 | return getScale_(A.topLeftCorner(3, 3)); 97 | return -1; 98 | } 99 | 100 | Eigen::Matrix3f normalizeRotation(const Eigen::MatrixXf& A) 101 | { 102 | if (A.cols() != 3 && A.cols() != 4) { 103 | exit(1); 104 | } 105 | 106 | Eigen::Matrix3f sR = A.topLeftCorner(3, 3); 107 | float scale = getScale(sR); 108 | return normalize_(sR / scale); 109 | } 110 | 111 | Eigen::Matrix4f normalizePose(const Eigen::Matrix4f& sT) 112 | { 113 | Eigen::Matrix4f T = sT; 114 | T.topLeftCorner(3, 3) = normalizeRotation(sT); 115 | return T; 116 | } 117 | 118 | void loadMap( 119 | const std::string& pcd_file, 120 | pcl::PointCloud::Ptr& cloud, 121 | pcl::PointCloud::Ptr& normals, 122 | float grid_leaf, float radius) 123 | { 124 | cloud->clear(); 125 | normals->clear(); 126 | 127 | // Load map pointcloud 128 | pcl::PointCloud::Ptr all_cloud(new pcl::PointCloud()); 129 | pcl::io::loadPCDFile(pcd_file, *all_cloud); 130 | 131 | // filtering 132 | if (grid_leaf > 0) { 133 | pcl::VoxelGrid filter; 134 | filter.setInputCloud(all_cloud); 135 | filter.setLeafSize(grid_leaf, grid_leaf, grid_leaf); 136 | filter.filter(*cloud); 137 | } else { 138 | cloud = all_cloud; 139 | } 140 | 141 | if (grid_leaf > radius) { 142 | std::cerr << "normal_search_leaf" << radius << " must be larger than grid_leaf" << grid_leaf << std::endl; 143 | exit(EXIT_FAILURE); 144 | } 145 | 146 | // normal estimation 147 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 148 | pcl_::NormalEstimation ne; 149 | ne.setSearchSurface(all_cloud); 150 | ne.setInputCloud(cloud); 151 | ne.setSearchMethod(tree); 152 | ne.setRadiusSearch(radius); 153 | ne.compute(*normals); 154 | std::cout << " normal " << normals->size() << ", points" << cloud->size() << ", surface " << all_cloud->size() << std::endl; 155 | 156 | return; 157 | } 158 | 159 | 160 | void transformNormals(const pcNormal& source, pcNormal& target, const Eigen::Matrix4f& T) 161 | { 162 | Eigen::Matrix3f R = normalizeRotation(T); 163 | if (&source != &target) { 164 | target.clear(); 165 | for (const pcl::Normal& n : source) { 166 | Eigen::Vector3f _n = R * n.getNormalVector3fMap(); 167 | target.push_back({_n.x(), _n.y(), _n.z()}); 168 | } 169 | return; 170 | } 171 | 172 | for (pcl::Normal& n : target) { 173 | Eigen::Vector3f _n = R * n.getNormalVector3fMap(); 174 | n = pcl::Normal(_n.x(), _n.y(), _n.z()); 175 | } 176 | return; 177 | } 178 | 179 | void transformXYZINormal(const pcXYZIN::Ptr& all, const pcXYZ::Ptr& points, const pcNormal::Ptr& normals, const Eigen::Matrix4f& T) 180 | { 181 | points->clear(); 182 | normals->clear(); 183 | 184 | Eigen::Matrix3f sR = T.topLeftCorner(3, 3); 185 | Eigen::Matrix3f R = normalizeRotation(T); 186 | Eigen::Vector3f t = T.topRightCorner(3, 1); 187 | 188 | for (const xyzin& a : *all) { 189 | Eigen::Vector3f _p = sR * a.getVector3fMap() + t; 190 | Eigen::Vector3f _n = R * a.getNormalVector3fMap(); 191 | 192 | points->push_back({_p.x(), _p.y(), _p.z()}); 193 | normals->push_back({_n.x(), _n.y(), _n.z()}); 194 | } 195 | return; 196 | } 197 | 198 | Eigen::Matrix3f randomRotation() 199 | { 200 | return Eigen::Quaternionf::UnitRandom().toRotationMatrix(); 201 | } 202 | 203 | void shufflePointCloud(pcXYZ::Ptr& cloud) 204 | { 205 | std::mt19937 rand; 206 | for (size_t i = 0, size = cloud->size(); i < size; i++) { 207 | std::swap(cloud->points.at(i), cloud->points.at(rand() % size)); 208 | } 209 | } 210 | 211 | } // namespace util 212 | } // namespace iris -------------------------------------------------------------------------------- /openvslam_bridge/src/bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "bridge.hpp" 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace iris 42 | { 43 | BridgeOpenVSLAM::~BridgeOpenVSLAM() 44 | { 45 | if (SLAM_ptr == nullptr) 46 | return; 47 | 48 | // wait until the loop BA is finished 49 | while (SLAM_ptr->loop_BA_is_running()) { 50 | std::this_thread::sleep_for(std::chrono::microseconds(5000)); 51 | } 52 | 53 | // shutdown the SLAM process 54 | SLAM_ptr->shutdown(); 55 | } 56 | 57 | void BridgeOpenVSLAM::setup(const std::string& config_path, const std::string& vocab_path) 58 | { 59 | // setup logger 60 | spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$"); 61 | spdlog::set_level(spdlog::level::info); 62 | 63 | // load configuration 64 | std::shared_ptr cfg; 65 | try { 66 | cfg = std::make_shared(config_path); 67 | } catch (const std::exception& e) { 68 | std::cerr << e.what() << std::endl; 69 | exit(EXIT_FAILURE); 70 | } 71 | 72 | // run tracking 73 | if (cfg->camera_->setup_type_ != openvslam::camera::setup_type_t::Monocular) { 74 | std::cout << "Invalid setup type: " + cfg->camera_->get_setup_type_string() << std::endl; 75 | exit(EXIT_FAILURE); 76 | } 77 | 78 | // build a SLAM system 79 | SLAM_ptr = std::make_shared(cfg, vocab_path); 80 | SLAM_ptr->startup(); 81 | SLAM_ptr->disable_loop_detector(); 82 | } 83 | 84 | void BridgeOpenVSLAM::getLandmarksAndNormals(pcl::PointCloud::Ptr& vslam_data, float height) const 85 | { 86 | auto t0 = std::chrono::system_clock::now(); 87 | 88 | if (recollection == 0 || accuracy < 0) { 89 | std::cerr << "ERROR: recollection & accuracy are not set" << std::endl; 90 | exit(1); 91 | } 92 | 93 | std::set local_landmarks; 94 | SLAM_ptr->get_map_publisher()->get_landmarks(local_landmarks); 95 | 96 | vslam_data->clear(); 97 | 98 | if (local_landmarks.empty()) return; 99 | 100 | Eigen::Vector3d t_vslam = SLAM_ptr->get_map_publisher()->get_current_cam_pose().topRightCorner(3, 1); 101 | 102 | unsigned int max_id = SLAM_ptr->get_map_publisher()->get_max_keyframe_id(); 103 | for (const auto local_lm : local_landmarks) { 104 | unsigned int first_observed_id = local_lm->first_keyfrm_id_; 105 | unsigned int last_observed_id = local_lm->last_observed_keyfrm_id_; 106 | if (local_lm->will_be_erased()) continue; 107 | if (local_lm->get_observed_ratio() < accuracy) continue; 108 | if (max_id > recollection && last_observed_id < max_id - recollection) continue; 109 | 110 | const openvslam::Vec3_t pos = local_lm->get_pos_in_world(); 111 | // const openvslam::Vec3_t normal = local_lm->get_obs_mean_normal(); 112 | 113 | // when the distance is 5m or more, the weight is minimum. 114 | // float weight = static_cast(1.0 - (t_vslam - pos).norm() * 0.2); 115 | float weight = 1.0f; 116 | weight = std::min(std::max(weight, 0.1f), 1.0f); 117 | 118 | Eigen::Vector3f t = getCameraPose().inverse().topRightCorner(3, 1); 119 | if (pos.y() - t.y() < -height) continue; 120 | 121 | pcl::PointXYZINormal p; 122 | p.x = static_cast(pos.x()); 123 | p.y = static_cast(pos.y()); 124 | p.z = static_cast(pos.z()); 125 | // p.normal_x = static_cast(normal.x()); 126 | // p.normal_y = static_cast(normal.y()); 127 | // p.normal_z = static_cast(normal.z()); 128 | p.intensity = weight; 129 | vslam_data->push_back(p); 130 | } 131 | 132 | auto t1 = std::chrono::system_clock::now(); 133 | long dt = std::chrono::duration_cast(t1 - t0).count(); 134 | std::cout 135 | << "landmark ratio \033[34m" << vslam_data->size() 136 | << "\033[m / \033[34m" << local_landmarks.size() 137 | << "\033[m" << dt << std::endl; 138 | 139 | return; 140 | } 141 | 142 | int BridgeOpenVSLAM::getState() const 143 | { 144 | return static_cast(SLAM_ptr->get_frame_publisher()->get_tracking_state()); 145 | } 146 | 147 | cv::Mat BridgeOpenVSLAM::getFrame() const 148 | { 149 | return SLAM_ptr->get_frame_publisher()->draw_frame(false); 150 | } 151 | 152 | Eigen::Matrix4f BridgeOpenVSLAM::getCameraPose() const 153 | { 154 | return SLAM_ptr->get_map_publisher()->get_current_cam_pose().cast(); 155 | } 156 | 157 | void BridgeOpenVSLAM::requestReset() 158 | { 159 | if (!SLAM_ptr->reset_is_requested()) 160 | SLAM_ptr->request_reset(); 161 | } 162 | 163 | void BridgeOpenVSLAM::execute(const cv::Mat& image) 164 | { 165 | SLAM_ptr->feed_monocular_frame(image, 0.05, cv::Mat{}); 166 | if (SLAM_ptr->get_frame_publisher()->get_tracking_state() == openvslam::tracker_state_t::Lost) { 167 | std::cout << "\n\033[33m ##### Request Reset #####\n\033[m" << std::endl; 168 | requestReset(); 169 | } 170 | } 171 | 172 | void BridgeOpenVSLAM::setCriteria(unsigned int recollection_, float accuracy_) 173 | { 174 | recollection = recollection_; 175 | accuracy = accuracy_; 176 | 177 | recollection = std::max(recollection, 1u); 178 | accuracy = std::max(accuracy, 0.1f); 179 | accuracy = std::min(accuracy, 1.0f); 180 | } 181 | 182 | std::pair BridgeOpenVSLAM::getCriteria() const 183 | { 184 | return {recollection, accuracy}; 185 | } 186 | 187 | } // namespace iris 188 | -------------------------------------------------------------------------------- /openvslam_bridge/src/stereo_bridge_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "stereo_bridge.hpp" 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | std::function imageCallbackGenerator(cv::Mat& subscribed_image) 40 | { 41 | return [&subscribed_image](const sensor_msgs::ImageConstPtr& msg) -> void { 42 | cv_bridge::CvImagePtr cv_ptr; 43 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 44 | subscribed_image = cv_ptr->image.clone(); 45 | }; 46 | } 47 | 48 | void publishPose(const Eigen::Matrix4f& T, const std::string& child_frame_id) 49 | { 50 | static tf::TransformBroadcaster br; 51 | tf::Transform transform; 52 | transform.setFromOpenGLMatrix(T.cast().eval().data()); 53 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", child_frame_id)); 54 | } 55 | 56 | int main(int argc, char* argv[]) 57 | { 58 | ros::init(argc, argv, "openvslam_stereo_bridge_node"); 59 | 60 | // Get rosparams 61 | ros::NodeHandle pnh("~"); 62 | bool is_image_compressed = true; 63 | bool is_image_color = true; 64 | int recollection = 30; 65 | float height = std::numeric_limits::max(); 66 | std::string vocab_path, vslam_config_path; 67 | std::string image_topic_name0; 68 | std::string image_topic_name1; 69 | pnh.getParam("vocab_path", vocab_path); 70 | pnh.getParam("vslam_config_path", vslam_config_path); 71 | pnh.getParam("image_topic_name0", image_topic_name0); 72 | pnh.getParam("image_topic_name1", image_topic_name1); 73 | pnh.getParam("is_image_compressed", is_image_compressed); 74 | pnh.getParam("is_image_color", is_image_color); 75 | pnh.getParam("keyframe_recollection", recollection); 76 | pnh.getParam("max_height", height); 77 | ROS_INFO("vocab_path: %s, vslam_config_path: %s, image_topic_name: %s, is_image_compressed: %d", 78 | vocab_path.c_str(), vslam_config_path.c_str(), image_topic_name0.c_str(), is_image_compressed); 79 | 80 | const int lower_threshold_of_points = 1000; 81 | const int upper_threshold_of_points = 1500; 82 | 83 | // Setup subscriber 84 | ros::NodeHandle nh; 85 | image_transport::ImageTransport it(nh); 86 | 87 | // Setup image subscriber 88 | message_filters::Subscriber infra1_image_subscriber(nh, image_topic_name0, 1); 89 | message_filters::Subscriber infra2_image_subscriber(nh, image_topic_name1, 1); 90 | message_filters::TimeSynchronizer syncronizer(infra1_image_subscriber, infra2_image_subscriber, 10); 91 | 92 | 93 | // Setup image callback 94 | cv::Mat subscribed_image0, subscribed_image1; 95 | ros::Time subscribed_stamp; 96 | if (is_image_compressed) { 97 | auto image_callback = [is_image_color, &subscribed_image0, &subscribed_image1, &subscribed_stamp](const sensor_msgs::CompressedImageConstPtr& image0, const sensor_msgs::CompressedImageConstPtr& image1) -> void { 98 | subscribed_image0 = cv::imdecode(cv::Mat(image0->data), is_image_color ? 1 : 0 /* '1': bgr, '0': gray*/); 99 | subscribed_image1 = cv::imdecode(cv::Mat(image1->data), is_image_color ? 1 : 0 /* '1': bgr, '0': gray*/); 100 | subscribed_stamp = image0->header.stamp; 101 | }; 102 | syncronizer.registerCallback(boost::bind(image_callback, _1, _2)); 103 | } else { 104 | std::cerr << "Error: Only compressed image is acceptable" << std::endl; 105 | exit(EXIT_FAILURE); 106 | } 107 | 108 | // Setup publisher 109 | ros::Publisher vslam_publisher = nh.advertise>("iris/vslam_data", 1); 110 | image_transport::Publisher image_publisher = it.advertise("iris/processed_image", 5); 111 | 112 | // Setup for OpenVSLAM 113 | iris::BridgeStereoOpenVSLAM bridge; 114 | bridge.setup(vslam_config_path, vocab_path); 115 | 116 | std::chrono::system_clock::time_point m_start; 117 | ros::Rate loop_rate(20); 118 | float accuracy = 0.5f; 119 | pcl::PointCloud::Ptr vslam_data(new pcl::PointCloud); 120 | std::ofstream ofs_time("vslam_time.csv"); 121 | 122 | // Start main loop 123 | ROS_INFO("start main loop."); 124 | while (ros::ok()) { 125 | if (!subscribed_image0.empty() && !subscribed_image1.empty()) { 126 | // start timer 127 | m_start = std::chrono::system_clock::now(); 128 | ros::Time process_stamp = subscribed_stamp; 129 | 130 | // process OpenVSLAM 131 | bridge.execute(subscribed_image0, subscribed_image1); 132 | bridge.setCriteria(recollection, accuracy); 133 | bridge.getLandmarksAndNormals(vslam_data, height); 134 | 135 | // Inform processing time 136 | std::stringstream ss; 137 | long time_ms = std::chrono::duration_cast(std::chrono::system_clock::now() - m_start).count(); 138 | ss << "processing time= \033[35m" 139 | << time_ms 140 | << "\033[m ms"; 141 | ofs_time << time_ms << std::endl; 142 | ROS_INFO("%s", ss.str().c_str()); 143 | 144 | // Reset input 145 | subscribed_image0 = cv::Mat(); 146 | subscribed_image1 = cv::Mat(); 147 | 148 | // Update threshold to adjust the number of points 149 | if (vslam_data->size() < lower_threshold_of_points && accuracy > 0.10) accuracy -= 0.01f; 150 | if (vslam_data->size() > upper_threshold_of_points && accuracy < 0.90) accuracy += 0.01f; 151 | std::cout << "accuracy: " << accuracy << std::endl; 152 | { 153 | cv::Mat img = bridge.getFrame(); 154 | if (!img.empty()) { 155 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); 156 | image_publisher.publish(msg); 157 | } 158 | } 159 | { 160 | pcl_conversions::toPCL(process_stamp, vslam_data->header.stamp); 161 | vslam_data->header.frame_id = "world"; 162 | vslam_publisher.publish(vslam_data); 163 | } 164 | } 165 | publishPose(bridge.getCameraPose().inverse(), "iris/vslam_pose"); 166 | 167 | // Spin and wait 168 | loop_rate.sleep(); 169 | ros::spinOnce(); 170 | } 171 | 172 | ROS_INFO("Finalize openvslam_bridge::bridge_node"); 173 | return 0; 174 | } 175 | -------------------------------------------------------------------------------- /iris/src/optimize/aligner.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "optimize/aligner.hpp" 27 | #include "core/util.hpp" 28 | #include "optimize/types_gicp.hpp" 29 | #include "optimize/types_restriction.hpp" 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace iris 40 | { 41 | namespace optimize 42 | { 43 | Eigen::Matrix4f Aligner::estimate7DoF( 44 | Eigen::Matrix4f& T, 45 | const pcXYZIN::Ptr& source_clouds, 46 | const pcl::PointCloud::Ptr& target, 47 | const pcl::CorrespondencesPtr& correspondances, 48 | const Eigen::Matrix4f& offset_camera, 49 | const std::list>&, 50 | const double ref_scale, 51 | const pcl::PointCloud::Ptr& target_normals) 52 | { 53 | g2o::SparseOptimizer optimizer; 54 | g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( 55 | g2o::make_unique(g2o::make_unique>())); 56 | optimizer.setAlgorithm(solver); 57 | 58 | setVertexSim3(optimizer, T); 59 | setEdge7DoFGICP(optimizer, source_clouds, target, correspondances, offset_camera.topRightCorner(3, 1), target_normals); 60 | setEdgeRestriction(optimizer, offset_camera, T, ref_scale); 61 | 62 | // execute 63 | optimizer.setVerbose(false); 64 | optimizer.initializeOptimization(); 65 | optimizer.computeActiveErrors(); 66 | optimizer.optimize(5); 67 | 68 | // construct output matrix 69 | g2o::VertexSim3Expmap* optimized = dynamic_cast(optimizer.vertices().find(0)->second); 70 | float scale = static_cast(optimized->estimate().scale()); 71 | Eigen::Matrix3f R = optimized->estimate().rotation().matrix().cast(); 72 | Eigen::Vector3f t = optimized->estimate().translation().cast(); 73 | std::cout << "scale= \033[31m" << scale << "\033[m" << std::endl; 74 | 75 | T = Eigen::Matrix4f::Identity(); 76 | T.topLeftCorner(3, 3) = scale * R; 77 | T.topRightCorner(3, 1) = t; 78 | 79 | return T; 80 | } 81 | 82 | void Aligner::setVertexSim3(g2o::SparseOptimizer& optimizer, Eigen::Matrix4f& T) 83 | { 84 | // set up rotation and translation for this node 85 | Eigen::Vector3d t = T.topRightCorner(3, 1).cast(); 86 | Eigen::Matrix3d sR = T.topLeftCorner(3, 3).cast(); 87 | double scale = util::getScale(sR.cast()); 88 | Eigen::Quaterniond q = Eigen::Quaterniond(sR / scale); 89 | g2o::Sim3 sim3(q, t, scale); 90 | 91 | // set up initial parameter 92 | g2o::VertexSim3Expmap* vc = new g2o::VertexSim3Expmap(); 93 | vc->setEstimate(sim3); 94 | vc->setId(0); 95 | 96 | // add to optimizer 97 | optimizer.addVertex(vc); 98 | } 99 | 100 | void Aligner::setEdge7DoFGICP( 101 | g2o::SparseOptimizer& optimizer, 102 | const pcXYZIN::Ptr& source_clouds, 103 | const pcl::PointCloud::Ptr& target, 104 | const pcl::CorrespondencesPtr& correspondances, 105 | const Eigen::Vector3f&, 106 | const pcl::PointCloud::Ptr& target_normals) 107 | { 108 | // get Vertex 109 | g2o::VertexSim3Expmap* vp0 = dynamic_cast(optimizer.vertices().find(0)->second); 110 | const Eigen::Matrix3d R = vp0->estimate().rotation().matrix(); 111 | 112 | for (const pcl::Correspondence& cor : *correspondances) { 113 | // new edge with correct cohort for caching 114 | Edge_Sim3_GICP* e = new Edge_Sim3_GICP(true); 115 | e->setVertex(0, vp0); // set viewpoint 116 | 117 | // calculate the relative 3D position of the point 118 | Eigen::Vector3f pt0, pt1; 119 | pt0 = target->at(cor.index_match).getVector3fMap(); 120 | pt1 = source_clouds->at(cor.index_query).getVector3fMap(); 121 | float weight = source_clouds->at(cor.index_query).intensity; 122 | 123 | EdgeGICP meas; 124 | meas.weight = weight; 125 | // meas.weight = 1.0f / ((camera - pt1).norm() + 1.0f); 126 | meas.pos0 = pt0.cast(); 127 | meas.pos1 = pt1.cast(); 128 | 129 | e->setMeasurement(meas); 130 | 131 | Eigen::Vector3f n0 = target_normals->at(cor.index_match).getNormalVector3fMap(); 132 | Eigen::Vector3f n1 = source_clouds->at(cor.index_query).getNormalVector3fMap(); 133 | 134 | // sometime normal0 has NaN 135 | if (std::isfinite(n0.x())) { 136 | meas.normal0 = n0.cast(); 137 | e->cov0 = meas.cov0(0.05f); // target 138 | } else { 139 | e->cov0 = meas.cov0(1.0f); // target 140 | } 141 | meas.normal1 = n1.cast(); 142 | e->cov1 = meas.cov1(1.00f); // source 143 | // e->information() = (e->cov0 + R * e->cov1 * R.transpose()).inverse(); 144 | e->information() = (e->cov0).inverse(); 145 | 146 | 147 | // set Huber kernel (default delta = 1.0) 148 | g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; 149 | e->setRobustKernel(rk); 150 | optimizer.addEdge(e); 151 | } 152 | } 153 | 154 | void Aligner::setEdgeRestriction( 155 | g2o::SparseOptimizer& optimizer, 156 | const Eigen::Matrix4f& offset_camera, 157 | const Eigen::Matrix4f&, 158 | double ref_scale) 159 | { 160 | g2o::VertexSim3Expmap* vp0 = dynamic_cast(optimizer.vertices().find(0)->second); 161 | 162 | // Add a scale edge 163 | { 164 | Edge_Scale_Restriction* e = new Edge_Scale_Restriction(scale_gain); 165 | e->setVertex(0, vp0); 166 | e->information().setIdentity(); 167 | e->setMeasurement(ref_scale); 168 | optimizer.addEdge(e); 169 | } 170 | 171 | // Add an altitude edge 172 | // { 173 | // Edge_Altitude_Restriction* e = new Edge_Altitude_Restriction(altitude_gain); 174 | // e->setVertex(0, vp0); 175 | // e->information().setIdentity(); 176 | // e->setMeasurement(offset_camera.topRightCorner(3, 1).cast()); 177 | // optimizer.addEdge(e); 178 | // } 179 | 180 | // Add a latitude edge 181 | // { 182 | // Eigen::Matrix3f R = util::normalizeRotation(offset_camera); 183 | // Edge_Latitude_Restriction* e = new Edge_Latitude_Restriction(R.cast(), latitude_gain); 184 | // e->setVertex(0, vp0); 185 | // e->information().setIdentity(); 186 | // e->setMeasurement(0.0); 187 | // optimizer.addEdge(e); 188 | // } 189 | 190 | // TODO: 191 | // // add a const velocity Model Constraint Edge of Scale 192 | // { 193 | // Edge_Smooth_Restriction* e = new Edge_Smooth_Restriction(smooth_gain); 194 | // e->setVertex(0, vp0); 195 | // e->information().setIdentity(); 196 | // VelocityModel model; 197 | // model.camera_pos = offset_camera.topRightCorner(3, 1).cast(); 198 | 199 | // model.old_pos = itr1->topRightCorner(3, 1).cast(); 200 | // model.older_pos = itr2->topRightCorner(3, 1).cast(); 201 | // e->setMeasurement(model); 202 | // optimizer.addEdge(e); 203 | // } 204 | } 205 | 206 | } // namespace optimize 207 | } // namespace iris 208 | -------------------------------------------------------------------------------- /iris/src/map/map.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "map/map.hpp" 27 | #include "core/util.hpp" 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace iris 34 | { 35 | 36 | namespace map 37 | { 38 | Map::Map(const Parameter& parameter, const Eigen::Matrix4f& T_init) 39 | : cache_file("iris.cache"), parameter(parameter), 40 | local_target_cloud(new pcXYZ), 41 | local_target_normals(new pcNormal) 42 | { 43 | bool recalculation_is_necessary = isRecalculationNecessary(); 44 | 45 | if (!recalculation_is_necessary) { 46 | all_target_cloud = pcXYZ::Ptr(new pcXYZ); 47 | all_target_normals = pcNormal::Ptr(new pcNormal); 48 | all_sparse_cloud = pcXYZ::Ptr(new pcXYZ); 49 | 50 | // load cache file 51 | bool flag1 = (pcl::io::loadPCDFile(cache_cloud_file, *all_target_cloud) != -1); 52 | bool flag2 = (pcl::io::loadPCDFile(cache_normals_file, *all_target_normals) != -1); 53 | bool flag3 = (pcl::io::loadPCDFile(cache_sparse_file, *all_sparse_cloud) != -1); 54 | 55 | if (flag1 && flag2 && flag3) 56 | std::cout << "Because of cache hit, cached target point cloud was loaded" << std::endl; 57 | else 58 | recalculation_is_necessary = true; 59 | } 60 | 61 | if (recalculation_is_necessary) { 62 | std::cout << "Because of cache miss, recalculate the target point cloud" << std::endl; 63 | 64 | std::cout << "start loading pointcloud & esitmating normal with leafsize " << parameter.voxel_grid_leaf << " search_radius " << parameter.normal_search_radius << std::endl; 65 | all_target_cloud = pcXYZ::Ptr(new pcXYZ); 66 | all_target_normals = pcNormal::Ptr(new pcNormal); 67 | all_sparse_cloud = pcXYZ::Ptr(new pcXYZ); 68 | util::loadMap(parameter.pcd_file, all_target_cloud, all_target_normals, parameter.voxel_grid_leaf, parameter.normal_search_radius); 69 | 70 | { 71 | pcl::VoxelGrid filter; 72 | filter.setInputCloud(all_target_cloud); 73 | filter.setLeafSize(4 * parameter.voxel_grid_leaf, 4 * parameter.voxel_grid_leaf, 4 * parameter.voxel_grid_leaf); 74 | filter.filter(*all_sparse_cloud); 75 | } 76 | 77 | 78 | // save as cache file 79 | std::cout << "save pointcloud" << std::endl; 80 | pcl::io::savePCDFileBinaryCompressed(cache_cloud_file, *all_target_cloud); 81 | pcl::io::savePCDFileBinaryCompressed(cache_normals_file, *all_target_normals); 82 | pcl::io::savePCDFileBinaryCompressed(cache_sparse_file, *all_sparse_cloud); 83 | 84 | // update cache information 85 | std::ofstream ofs(cache_file); 86 | ofs << parameter.toString(); 87 | } 88 | std::cout << "all_target_cloud_size " << all_target_cloud->size() << std::endl; 89 | 90 | // Calculate the number of submap and its size 91 | std::cout << "It starts making submaps. This may take few seconds." << std::endl; 92 | float L = parameter.submap_grid_leaf; 93 | if (L < 1) { 94 | L = 1; 95 | std::cout << "please set positive number for parameter.submap_grid_leaf" << std::endl; 96 | } 97 | 98 | // Make submaps 99 | for (size_t i = 0; i < all_target_cloud->size(); i++) { 100 | pcl::PointXYZ p = all_target_cloud->at(i); 101 | pcl::Normal n = all_target_normals->at(i); 102 | 103 | int id_x = static_cast(std::floor(p.x / L)); 104 | int id_y = static_cast(std::floor(p.y / L)); 105 | 106 | std::pair key = std::make_pair(id_x, id_y); 107 | submap_cloud[key].push_back(p); 108 | submap_normals[key].push_back(n); 109 | } 110 | 111 | // Construct local map 112 | updateLocalmap(T_init); 113 | } 114 | 115 | bool Map::isRecalculationNecessary() const 116 | { 117 | std::ifstream ifs(cache_file); 118 | // If cahce data doesn't exist, recalculate 119 | if (!ifs) 120 | return true; 121 | std::string data; 122 | 123 | // If cahce data doesn't match with parameter, recalculate 124 | std::getline(ifs, data); 125 | if (data != parameter.toString()) 126 | return true; 127 | 128 | return false; 129 | } 130 | 131 | bool Map::informCurrentPose(const Eigen::Matrix4f& T) 132 | { 133 | bool is_necessary = isUpdateNecessary(T); 134 | if (!is_necessary) 135 | return false; 136 | 137 | updateLocalmap(T); 138 | return true; 139 | } 140 | 141 | bool Map::isUpdateNecessary(const Eigen::Matrix4f& T) const 142 | { 143 | // NOTE: The boundaries of the submap have overlaps in order not to vibrate 144 | 145 | // (1) Condition about the location 146 | float distance = (T.topRightCorner(2, 1) - localmap_info.xy()).cwiseAbs().maxCoeff(); 147 | if (distance > 0.75 * parameter.submap_grid_leaf) { 148 | std::cout << "map update because of the distance condition" << std::endl; 149 | return true; 150 | } 151 | 152 | // (2) Condition about the location 153 | float yaw = yawFromPose(T); 154 | if (subtractAngles(yaw, localmap_info.theta) > 60.f / 180.f * 3.14f) { 155 | std::cout << "map update because of the angle condition" << std::endl; 156 | return true; 157 | } 158 | 159 | 160 | // Then, it need not to update the localmap 161 | return false; 162 | } 163 | 164 | void Map::updateLocalmap(const Eigen::Matrix4f& T) 165 | { 166 | std::cout << "\033[1;4;36m###############" << std::endl; 167 | std::cout << "Update Localmap" << std::endl; 168 | std::cout << "###############\033[m" << std::endl; 169 | 170 | Eigen::Vector3f t = T.topRightCorner(3, 1); 171 | const float L = parameter.submap_grid_leaf; 172 | int id_x = static_cast(std::floor(t.x() / L)); 173 | int id_y = static_cast(std::floor(t.y() / L)); 174 | std::cout << "id_x " << id_x << " id_y " << id_y << std::endl; 175 | 176 | int pattern = static_cast(yawFromPose(T) / (3.14f / 4.0f)); 177 | int x_min, y_min, dx, dy; 178 | float new_info_theta; 179 | switch (pattern) { 180 | case 0: 181 | case 7: 182 | x_min = id_x - 1; 183 | y_min = id_y - 1; 184 | dx = 4; 185 | dy = 3; 186 | new_info_theta = 0; 187 | break; 188 | case 1: 189 | case 2: 190 | x_min = id_x - 1; 191 | y_min = id_y - 1; 192 | dx = 3; 193 | dy = 4; 194 | new_info_theta = 3.1415f * 0.5f; 195 | break; 196 | case 3: 197 | case 4: 198 | x_min = id_x - 2; 199 | y_min = id_y - 1; 200 | dx = 4; 201 | dy = 3; 202 | new_info_theta = 3.1415f; 203 | break; 204 | case 5: 205 | case 6: 206 | default: 207 | x_min = id_x - 1; 208 | y_min = id_y - 2; 209 | dx = 3; 210 | dy = 4; 211 | new_info_theta = 3.1415f * 1.5f; 212 | break; 213 | } 214 | 215 | // Critical section from here 216 | { 217 | local_target_cloud->clear(); 218 | local_target_normals->clear(); 219 | 220 | for (int i = 0; i < dx; i++) { 221 | for (int j = 0; j < dy; j++) { 222 | std::pair key = std::make_pair(x_min + i, y_min + j); 223 | if (submap_cloud.count(key) == 0) { 224 | continue; 225 | } 226 | *local_target_cloud += submap_cloud[key]; 227 | *local_target_normals += submap_normals[key]; 228 | } 229 | } 230 | } 231 | { 232 | localmap_info.x = (static_cast(id_x) + 0.5f) * L, 233 | localmap_info.y = (static_cast(id_y) + 0.5f) * L, 234 | localmap_info.theta = new_info_theta; 235 | } 236 | std::cout << "new map-info: " 237 | << localmap_info.x << ", " 238 | << localmap_info.y << ", " 239 | << localmap_info.theta 240 | << std::endl; 241 | // Critical section until here 242 | } 243 | 244 | float Map::yawFromPose(const Eigen::Matrix4f& T) const 245 | { 246 | Eigen::Matrix3f R = util::normalizeRotation(T); 247 | 248 | // When the optical axis of the camera is pointing to the X-axis 249 | // and the upper side of the camera is pointing to the Z-axis, 250 | // the rotation matrix is as follows, 251 | Eigen::Matrix3f camera_rotate; 252 | camera_rotate << 0, 0, 1, 253 | -1, 0, 0, 254 | 0, -1, 0; 255 | 256 | // Therefore, multiply the inverse rotation matrix of it. 257 | // To extract the rotation on the XY-plane, we calculate how a unit vector is moved by a remained rotation. 258 | Eigen::Vector3f direction = (R * camera_rotate.transpose()) * Eigen::Vector3f::UnitX(); 259 | 260 | float theta = std::atan2(direction.y(), direction.x()); // [-pi,pi] 261 | if (theta < 0) 262 | return theta + 6.28f; 263 | return theta; 264 | } 265 | 266 | } // namespace map 267 | } // namespace iris -------------------------------------------------------------------------------- /iris/src/pcl_/correspondence_estimator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2012-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * $Id$ 37 | * 38 | */ 39 | 40 | #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_H_ 41 | #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_H_ 42 | 43 | #include 44 | #include 45 | 46 | 47 | namespace iris 48 | { 49 | namespace pcl_ 50 | { 51 | /** \brief @b CorrespondenceEstimationBackprojection computes 52 | * correspondences as points in the target cloud which have minimum 53 | * \author Suat Gedikli 54 | * \ingroup registration 55 | */ 56 | template 57 | class CorrespondenceEstimationBackProjection : public pcl::registration::CorrespondenceEstimationBase 58 | { 59 | public: 60 | typedef boost::shared_ptr> Ptr; 61 | typedef boost::shared_ptr> ConstPtr; 62 | 63 | using pcl::registration::CorrespondenceEstimationBase::initCompute; 64 | using pcl::registration::CorrespondenceEstimationBase::initComputeReciprocal; 65 | using pcl::registration::CorrespondenceEstimationBase::input_transformed_; 66 | using pcl::PCLBase::deinitCompute; 67 | using pcl::PCLBase::input_; 68 | using pcl::PCLBase::indices_; 69 | using pcl::registration::CorrespondenceEstimationBase::getClassName; 70 | using pcl::registration::CorrespondenceEstimationBase::point_representation_; 71 | using pcl::registration::CorrespondenceEstimationBase::target_indices_; 72 | 73 | typedef typename pcl::search::KdTree KdTree; 74 | typedef typename pcl::search::KdTree::Ptr KdTreePtr; 75 | 76 | typedef pcl::PointCloud PointCloudSource; 77 | typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 78 | typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 79 | 80 | typedef pcl::PointCloud PointCloudTarget; 81 | typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 82 | typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 83 | 84 | typedef pcl::PointCloud PointCloudNormals; 85 | typedef typename PointCloudNormals::Ptr NormalsPtr; 86 | typedef typename PointCloudNormals::ConstPtr NormalsConstPtr; 87 | 88 | /** \brief Empty constructor. 89 | * 90 | * \note 91 | * Sets the number of neighbors to be considered in the target point cloud (k_) to 10. 92 | */ 93 | CorrespondenceEstimationBackProjection() 94 | : source_normals_(), source_normals_transformed_(), target_normals_(), k_(10) 95 | { 96 | corr_name_ = "CorrespondenceEstimationBackProjection"; 97 | } 98 | 99 | /** \brief Empty destructor */ 100 | virtual ~CorrespondenceEstimationBackProjection() {} 101 | 102 | /** \brief Set the normals computed on the source point cloud 103 | * \param[in] normals the normals computed for the source cloud 104 | */ 105 | inline void 106 | setSourceNormals(const NormalsConstPtr& normals) { source_normals_ = normals; } 107 | 108 | /** \brief Get the normals of the source point cloud 109 | */ 110 | inline NormalsConstPtr 111 | getSourceNormals() const { return (source_normals_); } 112 | 113 | /** \brief Set the normals computed on the target point cloud 114 | * \param[in] normals the normals computed for the target cloud 115 | */ 116 | inline void 117 | setTargetNormals(const NormalsConstPtr& normals) { target_normals_ = normals; } 118 | 119 | /** \brief Get the normals of the target point cloud 120 | */ 121 | inline NormalsConstPtr 122 | getTargetNormals() const { return (target_normals_); } 123 | 124 | 125 | /** \brief See if this rejector requires source normals */ 126 | bool 127 | requiresSourceNormals() const 128 | { 129 | return (true); 130 | } 131 | 132 | /** \brief Blob method for setting the source normals */ 133 | void 134 | setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) 135 | { 136 | NormalsPtr cloud(new PointCloudNormals); 137 | fromPCLPointCloud2(*cloud2, *cloud); 138 | setSourceNormals(cloud); 139 | } 140 | 141 | /** \brief See if this rejector requires target normals*/ 142 | bool 143 | requiresTargetNormals() const 144 | { 145 | return (true); 146 | } 147 | 148 | /** \brief Method for setting the target normals */ 149 | void 150 | setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) 151 | { 152 | NormalsPtr cloud(new PointCloudNormals); 153 | fromPCLPointCloud2(*cloud2, *cloud); 154 | setTargetNormals(cloud); 155 | } 156 | 157 | /** \brief Determine the correspondences between input and target cloud. 158 | * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) 159 | * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target 160 | * point cloud 161 | */ 162 | void determineCorrespondences(pcl::Correspondences& correspondences, 163 | double max_distance = std::numeric_limits::max()); 164 | 165 | /** \brief Determine the reciprocal correspondences between input and target cloud. 166 | * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 167 | * correspondence, and Tgt_i has Src_i as one. 168 | * 169 | * \param[out] correspondences the found correspondences (index of query and target point, distance) 170 | * \param[in] max_distance maximum allowed distance between correspondences 171 | */ 172 | virtual void 173 | determineReciprocalCorrespondences(pcl::Correspondences&, double) {} 174 | 175 | /** \brief Set the number of nearest neighbours to be considered in the target 176 | * point cloud. By default, we use k = 10 nearest neighbors. 177 | * 178 | * \param[in] k the number of nearest neighbours to be considered 179 | */ 180 | inline void 181 | setKSearch(unsigned int k) { k_ = k; } 182 | 183 | /** \brief Get the number of nearest neighbours considered in the target point 184 | * cloud for computing correspondences. By default we use k = 10 nearest 185 | * neighbors. 186 | */ 187 | inline unsigned int 188 | getKSearch() const { return (k_); } 189 | 190 | /** \brief Clone and cast to CorrespondenceEstimationBase */ 191 | virtual boost::shared_ptr> 192 | clone() const 193 | { 194 | Ptr copy(new CorrespondenceEstimationBackProjection(*this)); 195 | return (copy); 196 | } 197 | 198 | protected: 199 | using pcl::registration::CorrespondenceEstimationBase::corr_name_; 200 | using pcl::registration::CorrespondenceEstimationBase::tree_; 201 | using pcl::registration::CorrespondenceEstimationBase::tree_reciprocal_; 202 | using pcl::registration::CorrespondenceEstimationBase::target_; 203 | 204 | /** \brief Internal computation initalization. */ 205 | bool 206 | initCompute(); 207 | 208 | private: 209 | /** \brief The normals computed at each point in the source cloud */ 210 | NormalsConstPtr source_normals_; 211 | 212 | /** \brief The normals computed at each point in the source cloud */ 213 | NormalsPtr source_normals_transformed_; 214 | 215 | /** \brief The normals computed at each point in the target cloud */ 216 | NormalsConstPtr target_normals_; 217 | 218 | /** \brief The number of neighbours to be considered in the target point cloud */ 219 | unsigned int k_; 220 | }; 221 | 222 | 223 | } // namespace pcl_ 224 | } // namespace iris 225 | 226 | // #include "vllm/pcl_/correspondence_estimator_impl.hpp" 227 | 228 | #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_H_ */ 229 | -------------------------------------------------------------------------------- /iris/src/iris_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "core/types.hpp" 27 | #include "map/map.hpp" 28 | #include "publish/publish.hpp" 29 | #include "system/system.hpp" 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | 44 | // 45 | Eigen::Matrix4f listenTransform(tf::TransformListener& listener); 46 | 47 | // 48 | pcl::PointCloud::Ptr vslam_data(new pcl::PointCloud); 49 | bool vslam_update = false; 50 | void callback(const pcl::PointCloud::ConstPtr& msg) 51 | { 52 | *vslam_data = *msg; 53 | if (vslam_data->size() > 0) 54 | vslam_update = true; 55 | } 56 | 57 | // 58 | Eigen::Matrix4f T_recover = Eigen::Matrix4f::Zero(); 59 | pcl::PointCloud::Ptr whole_pointcloud = nullptr; 60 | void callbackForRecover(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) 61 | { 62 | ROS_INFO("/initial_pose is subscribed"); 63 | 64 | float x = static_cast(msg->pose.pose.position.x); 65 | float y = static_cast(msg->pose.pose.position.y); 66 | float qw = static_cast(msg->pose.pose.orientation.w); 67 | float qz = static_cast(msg->pose.pose.orientation.z); 68 | 69 | float z = std::numeric_limits::max(); 70 | 71 | if (whole_pointcloud == nullptr) { 72 | std::cout << "z=0 because whole_pointcloud is nullptr" << std::endl; 73 | z = 0; 74 | } else { 75 | for (const pcl::PointXYZ& p : *whole_pointcloud) { 76 | constexpr float r2 = 5 * 5; // [m^2] 77 | float dx = x - p.x; 78 | float dy = y - p.y; 79 | if (dx * dx + dy * dy < r2) { 80 | z = std::min(z, p.z); 81 | } 82 | } 83 | } 84 | 85 | T_recover.setIdentity(); 86 | T_recover(0, 3) = x; 87 | T_recover(1, 3) = y; 88 | T_recover(2, 3) = z; 89 | float theta = 2 * std::atan2(qz, qw); 90 | Eigen::Matrix3f R; 91 | R << 0, 0, 1, 92 | -1, 0, 0, 93 | 0, -1, 0; 94 | T_recover.topLeftCorner(3, 3) = Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()).toRotationMatrix() * R; 95 | std::cout << "T_recover:\n" 96 | << T_recover << std::endl; 97 | } 98 | 99 | void writeCsv(std::ofstream& ofs, const ros::Time& timestamp, const Eigen::Matrix4f& iris_pose) 100 | { 101 | auto convert = [](const Eigen::MatrixXf& mat) -> Eigen::VectorXf { 102 | Eigen::MatrixXf tmp = mat.transpose(); 103 | return Eigen::VectorXf(Eigen::Map(tmp.data(), mat.size())); 104 | }; 105 | 106 | ofs << std::fixed << std::setprecision(std::numeric_limits::max_digits10); 107 | ofs << timestamp.toSec() << " " << convert(iris_pose).transpose() << std::endl; 108 | } 109 | 110 | int main(int argc, char* argv[]) 111 | { 112 | ros::init(argc, argv, "iris_node"); 113 | ros::NodeHandle nh; 114 | 115 | // Setup subscriber 116 | ros::Subscriber vslam_subscriber = nh.subscribe>("iris/vslam_data", 5, callback); 117 | ros::Subscriber recover_pose_subscriber = nh.subscribe("/initialpose", 5, callbackForRecover); 118 | tf::TransformListener listener; 119 | 120 | // Setup publisher 121 | ros::Publisher target_pc_publisher = nh.advertise>("iris/target_pointcloud", 1, true); 122 | ros::Publisher whole_pc_publisher = nh.advertise>("iris/whole_pointcloud", 1, true); 123 | ros::Publisher source_pc_publisher = nh.advertise>("iris/source_pointcloud", 1); 124 | ros::Publisher iris_path_publisher = nh.advertise("iris/iris_path", 1); 125 | ros::Publisher vslam_path_publisher = nh.advertise("iris/vslam_path", 1); 126 | ros::Publisher correspondences_publisher = nh.advertise("iris/correspondences", 1); 127 | ros::Publisher scale_publisher = nh.advertise("iris/align_scale", 1); 128 | ros::Publisher processing_time_publisher = nh.advertise("iris/processing_time", 1); 129 | // ros::Publisher normal_publisher = nh.advertise("iris/normals", 1); 130 | // ros::Publisher covariance_publisher = nh.advertise("iris/covariances", 1); 131 | iris::Publication publication; 132 | 133 | // Get rosparams 134 | ros::NodeHandle pnh("~"); 135 | std::string config_path, pcd_path; 136 | pnh.getParam("iris_config_path", config_path); 137 | pnh.getParam("pcd_path", pcd_path); 138 | ROS_INFO("config_path: %s, pcd_path: %s", config_path.c_str(), pcd_path.c_str()); 139 | 140 | // Initialize config 141 | iris::Config config(config_path); 142 | 143 | // Load LiDAR map 144 | iris::map::Parameter map_param( 145 | pcd_path, config.voxel_grid_leaf, config.normal_search_leaf, config.submap_grid_leaf); 146 | std::shared_ptr map = std::make_shared(map_param, config.T_init); 147 | 148 | // Initialize system 149 | std::shared_ptr system = std::make_shared(config, map); 150 | 151 | std::chrono::system_clock::time_point m_start; 152 | Eigen::Matrix4f offseted_vslam_pose = config.T_init; 153 | Eigen::Matrix4f iris_pose = config.T_init; 154 | 155 | // Publish map 156 | iris::publishPointcloud(whole_pc_publisher, map->getSparseCloud()); 157 | iris::publishPointcloud(target_pc_publisher, map->getTargetCloud()); 158 | whole_pointcloud = map->getSparseCloud(); 159 | std::ofstream ofs_track("trajectory.csv"); 160 | std::ofstream ofs_time("iris_time.csv"); 161 | 162 | iris::map::Info last_map_info; 163 | 164 | // Start main loop 165 | ros::Rate loop_rate(20); 166 | ROS_INFO("start main loop."); 167 | while (ros::ok()) { 168 | 169 | Eigen::Matrix4f T_vslam = listenTransform(listener); 170 | if (!T_recover.isZero()) { 171 | std::cout << "apply recover pose" << std::endl; 172 | system->specifyTWorld(T_recover); 173 | T_recover.setZero(); 174 | } 175 | 176 | if (vslam_update) { 177 | vslam_update = false; 178 | m_start = std::chrono::system_clock::now(); 179 | ros::Time process_stamp; 180 | pcl_conversions::fromPCL(vslam_data->header.stamp, process_stamp); 181 | 182 | // Execution 183 | system->execute(2, T_vslam, vslam_data); 184 | 185 | // Publish for rviz 186 | system->popPublication(publication); 187 | iris::publishPointcloud(source_pc_publisher, publication.cloud); 188 | iris::publishPath(iris_path_publisher, publication.iris_trajectory); 189 | iris::publishPath(vslam_path_publisher, publication.offset_trajectory); 190 | iris::publishCorrespondences(correspondences_publisher, publication.cloud, map->getTargetCloud(), publication.correspondences); 191 | // iris::publishNormal(normal_publisher, publication.cloud, publication.normals); 192 | // iris::publishCovariance(covariance_publisher, publication.cloud, publication.normals); 193 | 194 | if (last_map_info != map->getLocalmapInfo()) { 195 | iris::publishPointcloud(target_pc_publisher, map->getTargetCloud()); 196 | } 197 | last_map_info = map->getLocalmapInfo(); 198 | std::cout << "map: " << last_map_info.toString() << std::endl; 199 | 200 | 201 | // Processing time 202 | long time_ms = std::chrono::duration_cast(std::chrono::system_clock::now() - m_start).count(); 203 | std::stringstream ss; 204 | ss << "processing time= \033[35m" 205 | << time_ms 206 | << "\033[m ms"; 207 | ofs_time << time_ms << std::endl; 208 | ROS_INFO("Iris/ALIGN: %s", ss.str().c_str()); 209 | { 210 | std_msgs::Float32 scale; 211 | scale.data = iris::util::getScale(publication.T_align); 212 | scale_publisher.publish(scale); 213 | 214 | std_msgs::Float32 processing_time; 215 | processing_time.data = static_cast(time_ms); 216 | processing_time_publisher.publish(processing_time); 217 | } 218 | 219 | offseted_vslam_pose = publication.offset_camera; 220 | iris_pose = publication.iris_camera; 221 | 222 | writeCsv(ofs_track, process_stamp, iris_pose); 223 | } 224 | 225 | iris::publishPose(offseted_vslam_pose, "iris/offseted_vslam_pose"); 226 | iris::publishPose(iris_pose, "iris/iris_pose"); 227 | 228 | 229 | // Spin and wait 230 | ros::spinOnce(); 231 | loop_rate.sleep(); 232 | } 233 | 234 | ROS_INFO("Finalize the system"); 235 | return 0; 236 | } 237 | 238 | 239 | Eigen::Matrix4f listenTransform(tf::TransformListener& listener) 240 | { 241 | tf::StampedTransform transform; 242 | try { 243 | listener.lookupTransform("world", "iris/vslam_pose", ros::Time(0), transform); 244 | } catch (...) { 245 | } 246 | 247 | double data[16]; 248 | transform.getOpenGLMatrix(data); 249 | Eigen::Matrix4d T(data); 250 | return T.cast(); 251 | } -------------------------------------------------------------------------------- /iris/src/publish/publish.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Map IV, Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "publish/publish.hpp" 27 | #include "core/util.hpp" 28 | #include 29 | #include 30 | #include 31 | 32 | namespace iris 33 | { 34 | // h: [0,360] 35 | // s: [0,1] 36 | // v: [0,1] 37 | Eigen::Vector3f convertRGB(Eigen::Vector3f hsv) 38 | { 39 | const float max = hsv(2); 40 | const float min = max * (1 - hsv(1)); 41 | const float H = hsv(0); 42 | const float D = max - min; 43 | if (H < 60) return {max, H / 60.f * D + min, min}; 44 | if (H < 120) return {(120.f - H) / 60.f * D + min, max, min}; 45 | if (H < 180) return {min, max, (H - 120) / 60.f * D + min}; 46 | if (H < 240) return {min, (240.f - H) / 60.f * D + min, max}; 47 | if (H < 300) return {(H - 240.f) / 60.f * D + min, min, max}; 48 | if (H < 360) return {max, min, (360.f - H) / 60.f * D + min}; 49 | return {1.0f, 1.0f, 1.0f}; 50 | } 51 | 52 | void publishPose(const Eigen::Matrix4f& T, const std::string& child_frame_id) 53 | { 54 | static tf::TransformBroadcaster br; 55 | 56 | 57 | tf::Transform transform; 58 | transform.setFromOpenGLMatrix(util::normalizePose(T).cast().eval().data()); 59 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", child_frame_id)); 60 | } 61 | 62 | void publishPointcloud(ros::Publisher& publisher, const pcl::PointCloud::Ptr& cloud) 63 | { 64 | auto tmp = cloud; 65 | pcl_conversions::toPCL(ros::Time::now(), tmp->header.stamp); 66 | tmp->header.frame_id = "world"; 67 | publisher.publish(tmp); 68 | } 69 | 70 | void publishImage(image_transport::Publisher& publisher, const cv::Mat& image) 71 | { 72 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg(); 73 | publisher.publish(msg); 74 | } 75 | 76 | void publishCorrespondences(ros::Publisher& publisher, 77 | const pcl::PointCloud::Ptr& source, 78 | const pcl::PointCloud::Ptr& target, 79 | const pcl::CorrespondencesPtr& correspondences) 80 | { 81 | visualization_msgs::Marker line_strip; 82 | line_strip.header.frame_id = "world"; 83 | line_strip.header.stamp = ros::Time::now(); 84 | line_strip.ns = "correspondences"; 85 | line_strip.action = visualization_msgs::Marker::ADD; 86 | line_strip.pose.orientation.w = 1.0; 87 | line_strip.id = 0; 88 | line_strip.scale.x = 0.15; 89 | line_strip.type = visualization_msgs::Marker::LINE_LIST; 90 | line_strip.color.r = 1.0; 91 | line_strip.color.g = 0.0; 92 | line_strip.color.b = 0.0; 93 | line_strip.color.a = 1.0; 94 | 95 | for (const pcl::Correspondence& c : *correspondences) { 96 | pcl::PointXYZ point1 = source->at(c.index_query); 97 | pcl::PointXYZ point2 = target->at(c.index_match); 98 | geometry_msgs::Point p1, p2; 99 | p1.x = point1.x; 100 | p1.y = point1.y; 101 | p1.z = point1.z; 102 | p2.x = point2.x; 103 | p2.y = point2.y; 104 | p2.z = point2.z; 105 | line_strip.points.push_back(p1); 106 | line_strip.points.push_back(p2); 107 | } 108 | 109 | publisher.publish(line_strip); 110 | } 111 | 112 | void publishNormal(ros::Publisher& publisher, 113 | const pcl::PointCloud::Ptr& cloud, 114 | const pcl::PointCloud::Ptr& normals) 115 | { 116 | visualization_msgs::MarkerArray marker_array; 117 | 118 | geometry_msgs::Vector3 arrow; 119 | arrow.x = 0.2; // length 120 | arrow.y = 0.4; // width 121 | arrow.z = 0.5; // height 122 | 123 | for (size_t id = 0; id < cloud->size(); id++) { 124 | visualization_msgs::Marker marker; 125 | 126 | pcl::PointXYZ p = cloud->at(id); 127 | pcl::Normal n = normals->at(id); 128 | 129 | marker.header.frame_id = "world"; 130 | marker.header.stamp = ros::Time::now(); 131 | marker.ns = "normal"; 132 | marker.action = visualization_msgs::Marker::ADD; 133 | marker.pose.orientation.w = 1.0; 134 | marker.id = static_cast(id); 135 | marker.scale = arrow; 136 | marker.type = visualization_msgs::Marker::ARROW; 137 | marker.color.r = 0.0; 138 | marker.color.g = 1.0; 139 | marker.color.b = 0.0; 140 | marker.color.a = 1.0; 141 | 142 | geometry_msgs::Point p1, p2; 143 | p1.x = p.x; 144 | p1.y = p.y; 145 | p1.z = p.z; 146 | p2.x = p.x + 2.0f * n.normal_x; 147 | p2.y = p.y + 2.0f * n.normal_y; 148 | p2.z = p.z + 2.0f * n.normal_z; 149 | 150 | marker.points.push_back(p1); 151 | marker.points.push_back(p2); 152 | marker_array.markers.push_back(marker); 153 | } 154 | publisher.publish(marker_array); 155 | } 156 | 157 | Eigen::Quaternionf normalToQuaternion(const Eigen::Vector3f& n) 158 | { 159 | return Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f(0, 0, 1), n); 160 | } 161 | 162 | void publishCovariance(ros::Publisher& publisher, 163 | const pcl::PointCloud::Ptr& cloud, 164 | const pcl::PointCloud::Ptr& normals) 165 | { 166 | visualization_msgs::MarkerArray marker_array; 167 | 168 | geometry_msgs::Vector3 diameter; 169 | diameter.x = 4.0; 170 | diameter.y = 4.0; 171 | diameter.z = 1.0; 172 | 173 | for (size_t id = 0; id < cloud->size(); id++) { 174 | visualization_msgs::Marker marker; 175 | marker.header.frame_id = "world"; 176 | marker.header.stamp = ros::Time::now(); 177 | marker.ns = "covariance"; 178 | marker.action = visualization_msgs::Marker::ADD; 179 | 180 | pcl::PointXYZ p = cloud->at(id); 181 | marker.pose.position.x = p.x; 182 | marker.pose.position.y = p.y; 183 | marker.pose.position.z = p.z; 184 | 185 | pcl::Normal n = normals->at(id); 186 | Eigen::Quaternionf q = normalToQuaternion(Eigen::Vector3f(n.normal_x, n.normal_y, n.normal_z)); 187 | marker.pose.orientation.x = q.x(); 188 | marker.pose.orientation.y = q.y(); 189 | marker.pose.orientation.z = q.z(); 190 | marker.pose.orientation.w = q.w(); 191 | 192 | marker.id = static_cast(id); 193 | marker.scale = diameter; 194 | marker.type = visualization_msgs::Marker::SPHERE; 195 | marker.color.r = 1.0f; 196 | marker.color.g = 1.0f; 197 | marker.color.b = 0.0f; 198 | marker.color.a = 0.3f; 199 | marker_array.markers.push_back(marker); 200 | } 201 | 202 | publisher.publish(marker_array); 203 | } 204 | 205 | visualization_msgs::Marker makeMarkerAsLine(const Eigen::Vector3f& s, const Eigen::Vector3f& e, int id) 206 | { 207 | visualization_msgs::Marker line_strip; 208 | line_strip.header.frame_id = "world"; 209 | line_strip.header.stamp = ros::Time::now(); 210 | line_strip.ns = "points_and_lines"; 211 | line_strip.action = visualization_msgs::Marker::ADD; 212 | line_strip.pose.orientation.w = 1.0; 213 | line_strip.id = id; 214 | line_strip.type = visualization_msgs::Marker::LINE_STRIP; 215 | line_strip.color.a = 1.0; 216 | line_strip.scale.x = 0.5; 217 | 218 | { 219 | geometry_msgs::Point p; 220 | p.x = s.x(); 221 | p.y = s.y(); 222 | p.z = s.z(); 223 | line_strip.points.push_back(p); 224 | } 225 | { 226 | geometry_msgs::Point p; 227 | p.x = e.x(); 228 | p.y = e.y(); 229 | p.z = e.z(); 230 | line_strip.points.push_back(p); 231 | } 232 | return line_strip; 233 | } 234 | 235 | void publishPath(ros::Publisher& publisher, const std::vector>& trajectory) 236 | { 237 | nav_msgs::Path path; 238 | 239 | path.header.frame_id = "world"; 240 | path.header.stamp = ros::Time::now(); 241 | for (const Eigen::Vector3f& t : trajectory) { 242 | geometry_msgs::PoseStamped pose_stamped; 243 | pose_stamped.pose.position.x = t.x(); 244 | pose_stamped.pose.position.y = t.y(); 245 | pose_stamped.pose.position.z = t.z(); 246 | 247 | // We don't care about the orientation 248 | pose_stamped.pose.orientation.w = 1; 249 | path.poses.push_back(pose_stamped); 250 | } 251 | publisher.publish(path); 252 | } 253 | 254 | std::function