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