1 // ---------------------------------------------------------------------------- 2 // - Open3D: www.open3d.org - 3 // ---------------------------------------------------------------------------- 4 // The MIT License (MIT) 5 // 6 // Copyright (c) 2018 www.open3d.org 7 // 8 // Permission is hereby granted, free of charge, to any person obtaining a copy 9 // of this software and associated documentation files (the "Software"), to deal 10 // in the Software without restriction, including without limitation the rights 11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 // copies of the Software, and to permit persons to whom the Software is 13 // furnished to do so, subject to the following conditions: 14 // 15 // The above copyright notice and this permission notice shall be included in 16 // all copies or substantial portions of the Software. 17 // 18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 24 // IN THE SOFTWARE. 25 // ---------------------------------------------------------------------------- 26 27 #pragma once 28 29 #include <vector> 30 #include <memory> 31 32 #include <Core/Utility/IJsonConvertible.h> 33 #include <Core/Utility/Eigen.h> 34 35 namespace three { 36 37 class PoseGraphNode : public IJsonConvertible 38 { 39 public: 40 PoseGraphNode(const Eigen::Matrix4d &pose = Eigen::Matrix4d::Identity()) : pose_(pose)41 pose_(pose) {}; 42 ~PoseGraphNode(); 43 44 public: 45 bool ConvertToJsonValue(Json::Value &value) const override; 46 bool ConvertFromJsonValue(const Json::Value &value) override; 47 48 public: 49 Eigen::Matrix4d pose_; 50 }; 51 52 class PoseGraphEdge : public IJsonConvertible 53 { 54 public: 55 PoseGraphEdge( 56 int source_node_id = -1, int target_node_id = -1, 57 const Eigen::Matrix4d &transformation = Eigen::Matrix4d::Identity(), 58 const Eigen::Matrix6d &information = Eigen::Matrix6d::Identity(), 59 bool uncertain = false, 60 double confidence = 1.0) : source_node_id_(source_node_id)61 source_node_id_(source_node_id), 62 target_node_id_(target_node_id), 63 transformation_(transformation), 64 information_(information), 65 uncertain_(uncertain), 66 confidence_(confidence) {}; 67 ~PoseGraphEdge(); 68 69 public: 70 bool ConvertToJsonValue(Json::Value &value) const override; 71 bool ConvertFromJsonValue(const Json::Value &value) override; 72 73 public: 74 int source_node_id_; 75 int target_node_id_; 76 Eigen::Matrix4d transformation_; 77 Eigen::Matrix6d information_; 78 /// odometry edge has uncertain == false 79 /// loop closure edges has uncertain == true 80 bool uncertain_; 81 /// if uncertain_ is true, it has confidence bounded in [0,1]. 82 /// 1 means reliable, and 0 means unreliable edge. 83 /// This correspondence to line process value in [Choi et al 2015] 84 /// See core/registration/globaloptimization.h for more details. 85 double confidence_; 86 }; 87 88 class PoseGraph : public IJsonConvertible 89 { 90 public: 91 PoseGraph(); 92 ~PoseGraph() override; 93 94 public: 95 bool ConvertToJsonValue(Json::Value &value) const override; 96 bool ConvertFromJsonValue(const Json::Value &value) override; 97 98 public: 99 std::vector<PoseGraphNode> nodes_; 100 std::vector<PoseGraphEdge> edges_; 101 }; 102 103 } // namespace three 104