diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp index 72d91779c45f46071defbbc5227ca692e0ae8588..5919db56fe206b0478e2d0ec819c4cc8f4eee754 100644 --- a/applications/gui2/src/modules/camera.cpp +++ b/applications/gui2/src/modules/camera.cpp @@ -1,6 +1,8 @@ #include "camera.hpp" #include "statistics.hpp" +#include + #include "../views/camera3d.hpp" #include #include @@ -18,6 +20,8 @@ #include +// ==== Camera ================================================================= + using ftl::gui2::Camera; using ftl::codecs::Channel; using ftl::rgbd::Capability; @@ -31,6 +35,17 @@ void Camera::init() { overlay_ = std::unique_ptr (ftl::create(this, "overlay")); + + try { + ftl::file::unpack(file::config_dir() / "poses.bin", poses_); + for (const auto& pose : poses_) { + ftl::operators::Poser::add(pose, frame_id_); + } + LOG(INFO) << "Poses loaded from file (" << poses_.size() << " items)"; + } + catch (msgpack::insufficient_bytes&) { + LOG(INFO) << "No poses loaded from file"; + } } void Camera::update(double delta) { @@ -585,7 +600,7 @@ void Camera::setCursor(int x, int y) { void Camera::setOriginToCursor() { using ftl::calibration::transform::inverse; - + // Check for valid cursor /*if (cursor_normal_.norm() == 0.0f) return; float cursor_length = (cursor_target_ - cursor_pos_).norm(); @@ -617,17 +632,17 @@ void Camera::setOriginToCursor() { } } - cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f); + cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f); cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f); - cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f); + cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f); cursor_ = _cursor(); } void Camera::resetOrigin() { - cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f); + cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f); cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f); cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f); - cursor_ = _cursor(); + cursor_ = _cursor(); if (movable_) { auto *rend = io->feed()->getRenderer(frame_id_); @@ -656,11 +671,14 @@ void Camera::resetOrigin() { void Camera::saveCursorToPoser() { ftl::codecs::Shape3D shape; shape.type = ftl::codecs::Shape3DType::CURSOR; - shape.id = cursor_save_id_++; + shape.id = poses_.size(); shape.label = std::string("Cursor") + std::to_string(shape.id); shape.pose = cursor().inverse().cast(); shape.size = Eigen::Vector3f(0.1f,0.1f,0.1f); + auto k = (*(io->feed()->getFrameSet(frame_id_.frameset())))[frame_id_.source()].name(); + poses_.push_back(shape); + ftl::file::pack(ftl::file::config_dir() / "poses.bin", poses_); ftl::operators::Poser::add(shape, frame_id_); } diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp index 21ed70dc34a986b034a28e246897fbcd55e4e384..f35bb7e33cee7f6b8895b66e08605a7f980e3d98 100644 --- a/applications/gui2/src/modules/camera.hpp +++ b/applications/gui2/src/modules/camera.hpp @@ -4,6 +4,8 @@ #include "../screen.hpp" #include "../views/camera.hpp" +#include + #include #include #include @@ -94,7 +96,7 @@ private: ftl::data::FrameID frame_id_; ftl::codecs::Channel channel_ = ftl::codecs::Channel::Colour; ftl::stream::Feed::Filter *filter_ = nullptr; - std::atomic_bool paused_ = false; // TODO: implement in InputOutput + std::atomic_bool paused_ = false; bool has_seen_frame_ = false; ftl::codecs::Touch point_; bool live_=false; @@ -108,7 +110,6 @@ private: Eigen::Vector3f cursor_pos_; Eigen::Vector3f cursor_target_; Eigen::Vector3f cursor_normal_; - int cursor_save_id_=0; Eigen::Matrix4d cursor_; ftl::data::FrameSetPtr current_fs_; @@ -129,6 +130,11 @@ private: void initiate_(ftl::data::Frame &frame); void _updateCapabilities(ftl::data::Frame &frame); Eigen::Matrix4d _cursor() const; + + std::vector poses_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } diff --git a/components/codecs/CMakeLists.txt b/components/codecs/CMakeLists.txt index 821a11ed93a11f2075bdd1517468a84e0a9ef9a3..5ecf43399a7ceb5ca41edccee26abed452a16c6e 100644 --- a/components/codecs/CMakeLists.txt +++ b/components/codecs/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(BaseCodec OBJECT src/reader.cpp src/channels.cpp src/depth_convert.cu + src/shapes.cpp ) target_include_directories(BaseCodec PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -16,7 +17,7 @@ target_include_directories(BaseCodec PUBLIC ) set_property(TARGET BaseCodec PROPERTY CUDA_ARCHITECTURES OFF) -add_library(OpenCVCodec OBJECT +add_library(OpenCVCodec OBJECT src/opencv_encoder.cpp src/opencv_decoder.cpp ) @@ -32,7 +33,7 @@ $ $ ) -add_library(NvidiaCodec OBJECT +add_library(NvidiaCodec OBJECT src/nvidia_encoder.cpp src/nvidia_decoder.cpp src/Video_Codec_SDK_9.1.23/Samples/NvCodec/NvDecoder/NvDecoder.cpp diff --git a/components/codecs/include/ftl/codecs/shapes.hpp b/components/codecs/include/ftl/codecs/shapes.hpp index 2368660e526a294a468e19bcd8ebf105a4fa9263..9e8f95637d3d8651eb9866a6fde81931eda033f6 100644 --- a/components/codecs/include/ftl/codecs/shapes.hpp +++ b/components/codecs/include/ftl/codecs/shapes.hpp @@ -2,12 +2,32 @@ #define _FTL_CODECS_SHAPES_HPP_ #include - #include namespace ftl { namespace codecs { +// ============================================================================= +// TODO: document which direction the pose transforma is + +class Pose { +public: + Pose(const Eigen::Matrix4d& pose); + Eigen::Vector3d rvec() const; + Eigen::Vector3d tvec() const; + Eigen::Matrix3d rmat() const; + Eigen::Matrix4d mat() const; + +private: + double rvec_[3] = {0.0}; + double tvec_[3] = {0.0}; + +public: + MSGPACK_DEFINE(rvec_, tvec_); +}; + +// ============================================================================= + enum class Shape3DType { UNKNOWN = 0, BOX, diff --git a/components/codecs/src/shapes.cpp b/components/codecs/src/shapes.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6a338431d6e9c0b38f1095362893f024e7a2cce6 --- /dev/null +++ b/components/codecs/src/shapes.cpp @@ -0,0 +1,42 @@ +#include +#include "ftl/codecs/shapes.hpp" + +namespace ftl { +namespace codecs { + +Pose::Pose(const Eigen::Matrix4d& pose) { + const Eigen::Vector4d t = pose.col(3); + for (int i = 0; i < 3; i++) { tvec_[i] = t[i]; } + Eigen::AngleAxis rot(Eigen::Matrix3d(pose.block(0, 0, 3, 3))); + Eigen::Vector3d r = rot.axis(); + for (int i = 0; i < 3; i++) { rvec_[i] = r[i]; } +} + +Eigen::Vector3d Pose::tvec() const { + Eigen::Vector3d v; + for (int i = 0; i < 3; i++) { v[i] = tvec_[i]; } + return v; +} + +Eigen::Vector3d Pose::rvec() const { + Eigen::Vector3d v; + for (int i = 0; i < 3; i++) { v[i] = rvec_[i]; } + return v; +} + +Eigen::Matrix3d Pose::rmat() const { + Eigen::AngleAxis rot; + rot.axis() = rvec(); + return rot.toRotationMatrix(); +} + +Eigen::Matrix4d Pose::mat() const { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.block<3, 3>(0, 0) = rmat(); + T.block<1, 3>(0, 3) = tvec(); + return T; +} + +} + +} \ No newline at end of file diff --git a/components/common/cpp/include/ftl/file.hpp b/components/common/cpp/include/ftl/file.hpp index 765607974d520d270e67acf0b9d7a15f33c8fda1..0230604c5413129502d478221f64706cce0a8433 100644 --- a/components/common/cpp/include/ftl/file.hpp +++ b/components/common/cpp/include/ftl/file.hpp @@ -11,6 +11,10 @@ namespace filesystem = experimental::filesystem; #include #endif +#include +#include +#include + namespace ftl { namespace file { @@ -19,6 +23,24 @@ std::filesystem::path config_dir(); bool is_file(const std::filesystem::path &path); +// wrap msgpack to/from file +template +void pack(const std::filesystem::path& path, const T& obj) { + std::ofstream ofs(path.string(), std::ios_base::trunc); + msgpack::pack(ofs, obj); + ofs.close(); +} + +template +void unpack(const std::filesystem::path& path, T& obj) { + msgpack::object_handle oh; + std::stringstream buf; + std::ifstream ifs(path.string()); + buf << ifs.rdbuf(); + msgpack::unpack(oh, buf.str().data(), buf.str().size()); + oh.get().convert(obj); +} + } } diff --git a/components/streams/src/stream.cpp b/components/streams/src/stream.cpp index 72a8524e97d538ad1f407bcad15ebf76cf3b2517..f9d17d86edb0e9052c2a6842189f5fb8c595cea8 100644 --- a/components/streams/src/stream.cpp +++ b/components/streams/src/stream.cpp @@ -34,13 +34,15 @@ bool operator!=(const std::unordered_set &a, const std::un const std::unordered_set &Stream::available(int fs) const { SHARED_LOCK(mtx_, lk); - if (fs < 0 || static_cast(fs) >= state_.size()) throw FTL_Error("Frameset index out-of-bounds: " << fs); + if (fs < 0 || static_cast(fs) >= state_.size()) + throw FTL_Error("Frameset index out-of-bounds: " << fs); return state_[fs].available; } const std::unordered_set &Stream::selected(int fs) const { SHARED_LOCK(mtx_, lk); - if (fs < 0 || static_cast(fs) >= state_.size()) throw FTL_Error("Frameset index out-of-bounds: " << fs); + if (fs < 0 || static_cast(fs) >= state_.size()) + throw FTL_Error("Frameset index out-of-bounds: " << fs); return state_[fs].selected; } @@ -56,7 +58,8 @@ void Stream::select(int fs, const std::unordered_set &s, b if (fs == 255) return; UNIQUE_LOCK(mtx_, lk); - if (fs < 0 || (!make && static_cast(fs) >= state_.size())) throw FTL_Error("Frameset index out-of-bounds: " << fs); + if (fs < 0 || (!make && static_cast(fs) >= state_.size())) + throw FTL_Error("Frameset index out-of-bounds: " << fs); if (static_cast(fs) >= state_.size()) state_.resize(fs+1); state_[fs].selected = s; }