1 files changed, 176 insertions, 0 deletions
diff --git a/internal/dynamic_depth/pose.cc b/internal/dynamic_depth/pose.cc
new file mode 100644
index 0000000..7e02588
--- /dev/null
+++ b/internal/dynamic_depth/pose.cc
@@ -0,0 +1,176 @@
+#include "dynamic_depth/pose.h"
+
+#include <math.h>
+
+#include "android-base/logging.h"
+#include "dynamic_depth/const.h"
+
+using photos_editing_formats::xml::Deserializer;
+using photos_editing_formats::xml::Serializer;
+
+namespace photos_editing_formats {
+namespace dynamic_depth {
+namespace {
+
+const char kPositionX[] = "PositionX";
+const char kPositionY[] = "PositionY";
+const char kPositionZ[] = "PositionZ";
+const char kRotationX[] = "RotationX";
+const char kRotationY[] = "RotationY";
+const char kRotationZ[] = "RotationZ";
+const char kRotationW[] = "RotationW";
+const char kTimestamp[] = "Timestamp";
+const char kNamespaceHref[] = "http://ns.google.com/photos/dd/1.0/pose/";
+
+const std::vector<float> NormalizeQuaternion(const std::vector<float>& quat) {
+  if (quat.size() < 4) {
+    return std::vector<float>();
+  }
+  float length = sqrt((quat[0] * quat[0]) + (quat[1] * quat[1]) +
+                      (quat[2] * quat[2]) + (quat[3] * quat[3]));
+  const std::vector<float> normalized = {quat[0] / length, quat[1] / length,
+                                         quat[2] / length, quat[3] / length};
+  return normalized;
+}
+
+}  // namespace
+
+// Private constructor.
+Pose::Pose() : timestamp_(-1) {}
+
+// Public methods.
+void Pose::GetNamespaces(std::unordered_map<string, string>* ns_name_href_map) {
+  if (ns_name_href_map == nullptr) {
+    LOG(ERROR) << "Namespace list or own namespace is null";
+    return;
+  }
+  ns_name_href_map->emplace(DynamicDepthConst::Pose(), kNamespaceHref);
+}
+
+std::unique_ptr<Pose> Pose::FromData(const std::vector<float>& position,
+                                     const std::vector<float>& orientation,
+                                     const int64 timestamp) {
+  if (position.empty() && orientation.empty()) {
+    LOG(ERROR) << "Either position or orientation must be provided";
+    return nullptr;
+  }
+
+  std::unique_ptr<Pose> pose(new Pose());
+  if (position.size() >= 3) {
+    pose->position_ = position;
+  }
+
+  if (orientation.size() >= 4) {
+    pose->orientation_ = NormalizeQuaternion(orientation);
+  }
+
+  if (timestamp >= 0) {
+    pose->timestamp_ = timestamp;
+  }
+
+  return pose;
+}
+
+std::unique_ptr<Pose> Pose::FromDeserializer(
+    const Deserializer& parent_deserializer, const char* parent_namespace) {
+  std::unique_ptr<Deserializer> deserializer =
+      parent_deserializer.CreateDeserializer(parent_namespace,
+                                             DynamicDepthConst::Pose());
+  if (deserializer == nullptr) {
+    return nullptr;
+  }
+  std::unique_ptr<Pose> pose(new Pose());
+  if (!pose->ParsePoseFields(*deserializer)) {
+    return nullptr;
+  }
+  return pose;
+}
+
+bool Pose::HasPosition() const { return position_.size() == 3; }
+bool Pose::HasOrientation() const { return orientation_.size() == 4; }
+
+const std::vector<float>& Pose::GetPosition() const { return position_; }
+
+const std::vector<float>& Pose::GetOrientation() const { return orientation_; }
+
+int64 Pose::GetTimestamp() const { return timestamp_; }
+
+bool Pose::Serialize(Serializer* serializer) const {
+  if (serializer == nullptr) {
+    LOG(ERROR) << "Serializer is null";
+    return false;
+  }
+
+  if (!HasPosition() && !HasOrientation()) {
+    LOG(ERROR) << "Camera pose has neither position nor orientation";
+    return false;
+  }
+
+  bool success = true;
+  if (position_.size() == 3) {
+    success &= serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionX,
+                                         std::to_string(position_[0])) &&
+               serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionY,
+                                         std::to_string(position_[1])) &&
+               serializer->WriteProperty(DynamicDepthConst::Pose(), kPositionZ,
+                                         std::to_string(position_[2]));
+  }
+
+  if (orientation_.size() == 4) {
+    success &= serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationX,
+                                         std::to_string(orientation_[0])) &&
+               serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationY,
+                                         std::to_string(orientation_[1])) &&
+               serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationZ,
+                                         std::to_string(orientation_[2])) &&
+               serializer->WriteProperty(DynamicDepthConst::Pose(), kRotationW,
+                                         std::to_string(orientation_[3]));
+  }
+
+  if (timestamp_ >= 0) {
+    serializer->WriteProperty(DynamicDepthConst::Pose(), kTimestamp,
+                              std::to_string(timestamp_));
+  }
+
+  return success;
+}
+
+// Private methods.
+bool Pose::ParsePoseFields(const Deserializer& deserializer) {
+  float x, y, z, w;
+  const string& prefix = DynamicDepthConst::Pose();
+  // If a position field is present, the rest must be as well.
+  if (deserializer.ParseFloat(prefix, kPositionX, &x)) {
+    if (!deserializer.ParseFloat(prefix, kPositionY, &y)) {
+      return false;
+    }
+    if (!deserializer.ParseFloat(prefix, kPositionZ, &z)) {
+      return false;
+    }
+    position_ = {x, y, z};
+  }
+
+  // Same for orientation.
+  if (deserializer.ParseFloat(prefix, kRotationX, &x)) {
+    if (!deserializer.ParseFloat(prefix, kRotationY, &y)) {
+      return false;
+    }
+    if (!deserializer.ParseFloat(prefix, kRotationZ, &z)) {
+      return false;
+    }
+    if (!deserializer.ParseFloat(prefix, kRotationW, &w)) {
+      return false;
+    }
+    orientation_ = std::vector<float>({x, y, z, w});
+  }
+
+  if (position_.size() < 3 && orientation_.size() < 4) {
+    return false;
+  }
+
+  deserializer.ParseLong(prefix, kTimestamp, &timestamp_);
+  return true;
+}
+
+}  // namespace dynamic_depth
+}  // namespace photos_editing_formats