diff options
Diffstat (limited to 'internal/dynamic_depth/earth_pose.cc')
| -rw-r--r-- | internal/dynamic_depth/earth_pose.cc | 190 |
1 files changed, 190 insertions, 0 deletions
diff --git a/internal/dynamic_depth/earth_pose.cc b/internal/dynamic_depth/earth_pose.cc new file mode 100644 index 0000000..a57aa68 --- /dev/null +++ b/internal/dynamic_depth/earth_pose.cc @@ -0,0 +1,190 @@ +#include "dynamic_depth/earth_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 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 = + 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, + ×tamp_); + return true; +} + +} // namespace dynamic_depth +} // namespace photos_editing_formats |