xref: /aosp_15_r20/external/dynamic_depth/internal/dynamic_depth/earth_pose.cc (revision a62be0856e8e1158f43b03e41bbad10f4d005fde)

#include "dynamic_depth/earth_pose.h"

#include <math.h>

#include <cmath>

#include "android-base/logging.h"
#include "dynamic_depth/const.h"

using ::dynamic_depth::xmpmeta::xml::Deserializer;
using ::dynamic_depth::xmpmeta::xml::Serializer;

namespace dynamic_depth {
namespace {

const char kLatitude[] = "Latitude";
const char kLongitude[] = "Longitude";
const char kAltitude[] = "Altitude";
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/earthpose/";

const std::vector<float> NormalizeQuaternion(const std::vector<float>& quat) {
  if (quat.size() < 4) {
    return std::vector<float>();
  }
  float length = std::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.
EarthPose::EarthPose() : timestamp_(-1) {}

// Public methods.
void EarthPose::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::EarthPose(), kNamespaceHref);
}

std::unique_ptr<EarthPose> EarthPose::FromData(
    const std::vector<double>& 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<EarthPose> earth_pose(new EarthPose());
  if (position.size() >= 3) {
    earth_pose->position_ = position;
  }

  if (orientation.size() >= 4) {
    earth_pose->orientation_ = NormalizeQuaternion(orientation);
  }

  if (timestamp >= 0) {
    earth_pose->timestamp_ = timestamp;
  }

  return earth_pose;
}

std::unique_ptr<EarthPose> EarthPose::FromDeserializer(
    const Deserializer& parent_deserializer) {
  std::unique_ptr<Deserializer> deserializer =
      parent_deserializer.CreateDeserializer(
          DynamicDepthConst::Namespace(DynamicDepthConst::EarthPose()),
          DynamicDepthConst::EarthPose());
  if (deserializer == nullptr) {
    return nullptr;
  }
  std::unique_ptr<EarthPose> earth_pose(new EarthPose());
  if (!earth_pose->ParseEarthPoseFields(*deserializer)) {
    return nullptr;
  }
  return earth_pose;
}

bool EarthPose::HasPosition() const { return position_.size() == 3; }
bool EarthPose::HasOrientation() const { return orientation_.size() == 4; }

const std::vector<double>& EarthPose::GetPosition() const { return position_; }

const std::vector<float>& EarthPose::GetOrientation() const {
  return orientation_;
}

int64 EarthPose::GetTimestamp() const { return timestamp_; }

bool EarthPose::Serialize(Serializer* serializer) const {
  if (serializer == nullptr) {
    LOG(ERROR) << "Serializer is null";
    return false;
  }

  if (!HasPosition() && !HasOrientation()) {
    LOG(ERROR) << "Device pose has neither position nor orientation";
    return false;
  }

  bool success = true;
  if (position_.size() == 3) {
    success &=
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kLatitude,
                                  std::to_string(position_[0])) &&
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kLongitude,
                                  std::to_string(position_[1])) &&
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kAltitude,
                                  std::to_string(position_[2]));
  }

  if (orientation_.size() == 4) {
    success &=
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kRotationX,
                                  std::to_string(orientation_[0])) &&
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kRotationY,
                                  std::to_string(orientation_[1])) &&
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kRotationZ,
                                  std::to_string(orientation_[2])) &&
        serializer->WriteProperty(DynamicDepthConst::EarthPose(), kRotationW,
                                  std::to_string(orientation_[3]));
  }

  if (timestamp_ >= 0) {
    serializer->WriteProperty(DynamicDepthConst::EarthPose(), kTimestamp,
                              std::to_string(timestamp_));
  }

  return success;
}

// Private methods.
bool EarthPose::ParseEarthPoseFields(const Deserializer& deserializer) {
  double lat, lon, alt;
  // If a position field is present, the rest must be as well.
  if (deserializer.ParseDouble(DynamicDepthConst::EarthPose(), kLatitude,
                               &lat)) {
    if (!deserializer.ParseDouble(DynamicDepthConst::EarthPose(), kLongitude,
                                  &lon)) {
      return false;
    }
    if (!deserializer.ParseDouble(DynamicDepthConst::EarthPose(), kAltitude,
                                  &alt)) {
      return false;
    }
    position_ = {lat, lon, alt};
  }

  // Same for orientation.
  float x, y, z, w;
  if (deserializer.ParseFloat(DynamicDepthConst::EarthPose(), kRotationX, &x)) {
    if (!deserializer.ParseFloat(DynamicDepthConst::EarthPose(), kRotationY,
                                 &y)) {
      return false;
    }
    if (!deserializer.ParseFloat(DynamicDepthConst::EarthPose(), kRotationZ,
                                 &z)) {
      return false;
    }
    if (!deserializer.ParseFloat(DynamicDepthConst::EarthPose(), kRotationW,
                                 &w)) {
      return false;
    }
    orientation_ = std::vector<float>({x, y, z, w});
  }

  if (position_.size() < 3 && orientation_.size() < 4) {
    return false;
  }

  deserializer.ParseLong(DynamicDepthConst::EarthPose(), kTimestamp,
                         &timestamp_);
  return true;
}

}  // namespace dynamic_depth