1 #include "dynamic_depth/pose.h"
2
3 #include <math.h>
4
5 #include <cmath>
6
7 #include "android-base/logging.h"
8 #include "dynamic_depth/const.h"
9
10 using ::dynamic_depth::xmpmeta::xml::Deserializer;
11 using ::dynamic_depth::xmpmeta::xml::Serializer;
12
13 namespace dynamic_depth {
14 namespace {
15
16 const char kPositionX[] = "PositionX";
17 const char kPositionY[] = "PositionY";
18 const char kPositionZ[] = "PositionZ";
19 const char kRotationX[] = "RotationX";
20 const char kRotationY[] = "RotationY";
21 const char kRotationZ[] = "RotationZ";
22 const char kRotationW[] = "RotationW";
23 const char kTimestamp[] = "Timestamp";
24 const char kNamespaceHref[] = "http://ns.google.com/photos/dd/1.0/pose/";
25
NormalizeQuaternion(const std::vector<float> & quat)26 const std::vector<float> NormalizeQuaternion(const std::vector<float>& quat) {
27 if (quat.size() < 4) {
28 return std::vector<float>();
29 }
30 float length = std::sqrt((quat[0] * quat[0]) + (quat[1] * quat[1]) +
31 (quat[2] * quat[2]) + (quat[3] * quat[3]));
32 const std::vector<float> normalized = {quat[0] / length, quat[1] / length,
33 quat[2] / length, quat[3] / length};
34 return normalized;
35 }
36
37 } // namespace
38
39 // Private constructor.
Pose()40 Pose::Pose() : timestamp_(-1) {}
41
42 // Public methods.
GetNamespaces(std::unordered_map<string,string> * ns_name_href_map)43 void Pose::GetNamespaces(std::unordered_map<string, string>* ns_name_href_map) {
44 if (ns_name_href_map == nullptr) {
45 LOG(ERROR) << "Namespace list or own namespace is null";
46 return;
47 }
48 ns_name_href_map->emplace(DynamicDepthConst::Pose(), kNamespaceHref);
49 }
50
FromData(const std::vector<float> & position,const std::vector<float> & orientation,const int64 timestamp)51 std::unique_ptr<Pose> Pose::FromData(const std::vector<float>& position,
52 const std::vector<float>& orientation,
53 const int64 timestamp) {
54 if (position.empty() && orientation.empty()) {
55 LOG(ERROR) << "Either position or orientation must be provided";
56 return nullptr;
57 }
58
59 std::unique_ptr<Pose> pose(new Pose());
60 if (position.size() >= 3) {
61 pose->position_ = position;
62 }
63
64 if (orientation.size() >= 4) {
65 pose->orientation_ = NormalizeQuaternion(orientation);
66 }
67
68 if (timestamp >= 0) {
69 pose->timestamp_ = timestamp;
70 }
71
72 return pose;
73 }
74
FromDeserializer(const Deserializer & parent_deserializer,const char * parent_namespace)75 std::unique_ptr<Pose> Pose::FromDeserializer(
76 const Deserializer& parent_deserializer, const char* parent_namespace) {
77 std::unique_ptr<Deserializer> deserializer =
78 parent_deserializer.CreateDeserializer(parent_namespace,
79 DynamicDepthConst::Pose());
80 if (deserializer == nullptr) {
81 return nullptr;
82 }
83 std::unique_ptr<Pose> pose(new Pose());
84 if (!pose->ParsePoseFields(*deserializer)) {
85 return nullptr;
86 }
87 return pose;
88 }
89
HasPosition() const90 bool Pose::HasPosition() const { return position_.size() == 3; }
HasOrientation() const91 bool Pose::HasOrientation() const { return orientation_.size() == 4; }
92
GetPosition() const93 const std::vector<float>& Pose::GetPosition() const { return position_; }
94
GetOrientation() const95 const std::vector<float>& Pose::GetOrientation() const { return orientation_; }
96
GetTimestamp() const97 int64 Pose::GetTimestamp() const { return timestamp_; }
98
Serialize(Serializer * serializer) const99 bool Pose::Serialize(Serializer* serializer) const {
100 if (serializer == nullptr) {
101 LOG(ERROR) << "Serializer is null";
102 return false;
103 }
104
105 if (!HasPosition() && !HasOrientation()) {
106 LOG(ERROR) << "Camera pose has neither position nor orientation";
107 return false;
108 }
109
110 bool success = true;
111 if (position_.size() == 3) {
112 success &= serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionX,
113 std::to_string(position_[0])) &&
114 serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionY,
115 std::to_string(position_[1])) &&
116 serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionZ,
117 std::to_string(position_[2]));
118 }
119
120 if (orientation_.size() == 4) {
121 success &= serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationX,
122 std::to_string(orientation_[0])) &&
123 serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationY,
124 std::to_string(orientation_[1])) &&
125 serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationZ,
126 std::to_string(orientation_[2])) &&
127 serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationW,
128 std::to_string(orientation_[3]));
129 }
130
131 if (timestamp_ >= 0) {
132 serializer->WriteProperty(DynamicDepthConst::Pose(), kTimestamp,
133 std::to_string(timestamp_));
134 }
135
136 return success;
137 }
138
139 // Private methods.
ParsePoseFields(const Deserializer & deserializer)140 bool Pose::ParsePoseFields(const Deserializer& deserializer) {
141 float x, y, z, w;
142 const string& prefix = DynamicDepthConst::Pose();
143 // If a position field is present, the rest must be as well.
144 if (deserializer.ParseFloat(prefix, kPositionX, &x)) {
145 if (!deserializer.ParseFloat(prefix, kPositionY, &y)) {
146 return false;
147 }
148 if (!deserializer.ParseFloat(prefix, kPositionZ, &z)) {
149 return false;
150 }
151 position_ = {x, y, z};
152 }
153
154 // Same for orientation.
155 if (deserializer.ParseFloat(prefix, kRotationX, &x)) {
156 if (!deserializer.ParseFloat(prefix, kRotationY, &y)) {
157 return false;
158 }
159 if (!deserializer.ParseFloat(prefix, kRotationZ, &z)) {
160 return false;
161 }
162 if (!deserializer.ParseFloat(prefix, kRotationW, &w)) {
163 return false;
164 }
165 orientation_ = std::vector<float>({x, y, z, w});
166 }
167
168 if (position_.size() < 3 && orientation_.size() < 4) {
169 return false;
170 }
171
172 deserializer.ParseLong(prefix, kTimestamp, ×tamp_);
173 return true;
174 }
175
176 } // namespace dynamic_depth
177