Add new file

environment

+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2015 Intel Corporation. All Rights Reserved.
+
+#include "environment.h"
+
+namespace librealsense
+{
+    extrinsics_graph::extrinsics_graph()
+        : _locks_count(0)
+    {
+        _id = std::make_shared<lazy<rs2_extrinsics>>([]()
+        {
+            return identity_matrix();
+        });
+    }
+
+    extrinsics_graph::extrinsics_lock extrinsics_graph::lock()
+    {
+        extrinsics_lock l(*this);
+        return l;
+    }
+
+    void extrinsics_graph::register_same_extrinsics(const stream_interface& from, const stream_interface& to)
+    {
+        register_extrinsics(from, to, _id);
+    }
+
+    void extrinsics_graph::register_extrinsics(const stream_interface& from, const stream_interface& to, std::weak_ptr<lazy<rs2_extrinsics>> extr)
+    {
+        std::lock_guard<std::mutex> lock(_mutex);
+
+        // First, trim any dead stream, to make sure we are not keep gaining memory
+        cleanup_extrinsics();
+
+        // Second, register new extrinsics
+        auto from_idx = find_stream_profile(from);
+        // If this is a new index, add it to the map preemptively,
+        // This way find on to will be able to return another new index
+        if (_extrinsics.find(from_idx) == _extrinsics.end())
+            _extrinsics.insert({from_idx, {}});
+
+        auto to_idx = find_stream_profile(to);
+
+        _extrinsics[from_idx][to_idx] = extr;
+        _extrinsics[to_idx][from_idx] = std::shared_ptr<lazy<rs2_extrinsics>>(nullptr);
+    }
+
+    void extrinsics_graph::register_extrinsics(const stream_interface & from, const stream_interface & to, rs2_extrinsics extr)
+    {
+        auto lazy_extr = std::make_shared<lazy<rs2_extrinsics>>([=]() {return extr; });
+        _external_extrinsics.push_back(lazy_extr);
+        register_extrinsics(from, to, lazy_extr);
+    }
+
+    void extrinsics_graph::cleanup_extrinsics()
+    {
+        if (_locks_count.load()) return;
+
+        auto counter = 0;
+        std::vector<int> invalid_ids;
+        for (auto&& kvp : _streams)
+        {
+            if (!kvp.second.lock())
+            {
+                auto dead_id = kvp.first;
+                // Delete all extrinsics going out of this stream
+                _extrinsics.erase(dead_id);
+                ++counter;
+                invalid_ids.push_back(dead_id);
+            }
+        }
+
+        for (auto dead_id : invalid_ids)
+        {
+            _streams.erase(dead_id);
+            for (auto&& elem : _extrinsics)
+            {
+                // Delete any extrinsics going into the stream
+                elem.second.erase(dead_id);
+                ++counter;
+            }
+        }
+
+        if (!invalid_ids.empty())
+            LOG_INFO("Found " << invalid_ids.size() << " unreachable streams, " << counter << " extrinsics deleted");
+    }
+
+    int extrinsics_graph::find_stream_profile(const stream_interface& p)
+    {
+        auto sp = p.shared_from_this();
+        auto max = 0;
+        for (auto&& kvp : _streams)
+        {
+            max = std::max(max, kvp.first);
+            if (kvp.second.lock().get() == sp.get())
+                return kvp.first;
+        }
+        _streams[max + 1] = sp;
+        return max + 1;
+
+    }
+
+    bool extrinsics_graph::try_fetch_extrinsics(const stream_interface& from, const stream_interface& to, rs2_extrinsics* extr)
+    {
+        std::lock_guard<std::mutex> lock(_mutex);
+        cleanup_extrinsics();
+        auto from_idx = find_stream_profile(from);
+        auto to_idx = find_stream_profile(to);
+
+        if (from_idx == to_idx)
+        {
+            *extr = identity_matrix();
+            return true;
+        }
+
+        std::set<int> visited;
+        return try_fetch_extrinsics(from_idx, to_idx, visited, extr);
+    }
+
+    bool extrinsics_graph::try_fetch_extrinsics(int from, int to, std::set<int>& visited, rs2_extrinsics* extr)
+    {
+        if (visited.count(from)) return false;
+
+        auto it = _extrinsics.find(from);
+        if (it != _extrinsics.end())
+        {
+            auto back_edge = fetch_edge(to, from);
+            auto fwd_edge = fetch_edge(from, to);
+
+            // Make sure both parts of the edge are still available
+            if (fwd_edge.get() || back_edge.get())
+            {
+                if (fwd_edge.get())
+                    *extr = fwd_edge->operator*(); // Evaluate the expression
+                else
+                    *extr = inverse(back_edge->operator*());
+
+                return true;
+            }
+            else
+            {
+                visited.insert(from);
+                for (auto&& kvp : it->second)
+                {
+                    auto new_from = kvp.first;
+                    auto way = kvp.second;
+
+                    // Lock down the edge in both directions to ensure we can evaluate the extrinsics
+                    back_edge = fetch_edge(new_from, from);
+                    fwd_edge = fetch_edge(from, new_from);
+
+                    if ((back_edge.get() || fwd_edge.get()) &&
+                        try_fetch_extrinsics(new_from, to, visited, extr))
+                    {
+                        const auto local = [&]() {
+                            if (fwd_edge.get())
+                                return fwd_edge->operator*(); // Evaluate the expression
+                            else
+                                return inverse(back_edge->operator*());
+                        }();
+
+                        auto pose = to_pose(local) * to_pose(*extr);
+                        *extr = from_pose(pose);
+                        return true;
+                    }
+                }
+            }
+        } // If there are no extrinsics from from, there are none to it, so it is completely isolated
+        return false;
+    }
+
+    std::shared_ptr<lazy<rs2_extrinsics>> extrinsics_graph::fetch_edge(int from, int to)
+    {
+        auto it = _extrinsics.find(from);
+        if (it != _extrinsics.end())
+        {
+            auto it2 = it->second.find(to);
+            if (it2 != it->second.end())
+            {
+                return it2->second.lock();
+            }
+        }
+
+        return nullptr;
+    }
+
+
+    environment& environment::get_instance()
+    {
+        static environment env;
+        return env;
+    }
+
+    extrinsics_graph& environment::get_extrinsics_graph()
+    {
+        return _extrinsics;
+    }
+
+    void environment::set_time_service(std::shared_ptr<platform::time_service> ts)
+    {
+        _ts = ts;
+    }
+
+    std::shared_ptr<platform::time_service> environment::get_time_service()
+    {
+        return _ts;
+    }
+}
\ No newline at end of file