Align stream profile

src/proc/align.cpp

@@ -9,6 +9,7 @@
 #include "proc/synthetic-stream.h"
 #include "environment.h"
 #include "align.h"
+#include "stream.h"
 
 namespace librealsense
 {
@@ -114,7 +115,58 @@ namespace librealsense
     {
         align_other_to_depth(other_aligned_to_z, [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; }, z_intrin, z_to_other, other_intrin, other_pixels, other_format);
     }
-
+
+    int align::get_unique_id(const std::shared_ptr<stream_profile_interface>& original_profile,
+        const std::shared_ptr<stream_profile_interface>& to_profile,
+        const std::shared_ptr<stream_profile_interface>& aligned_profile)
+    {
+        //align_stream_unique_ids holds a cache of mapping between the 2 streams that created the new aligned stream
+        // to it stream id. 
+        //When an aligned frame is created from other streams (but with the same instance of this class)
+        // the from_to pair will be different so a new id will be added to the cache.
+        //This allows the user to pass different streams to this class and for every pair of from_to
+        //the user will always get the same stream id for the aligned stream.
+        auto from_to = std::make_pair(original_profile->get_unique_id(), to_profile->get_unique_id());
+        auto it = align_stream_unique_ids.find(from_to);
+        if (it != align_stream_unique_ids.end())
+        {
+            return it->second;
+        }
+        else
+        {
+            int new_id = aligned_profile->get_unique_id();
+            align_stream_unique_ids[from_to] = new_id;
+            return new_id;
+        }
+    }
+    std::shared_ptr<stream_profile_interface> align::create_aligned_profile(
+        const std::shared_ptr<stream_profile_interface>& original_profile, 
+        const std::shared_ptr<stream_profile_interface>& to_profile)
+    {
+        auto aligned_profile = original_profile->clone();
+        int aligned_unique_id = get_unique_id(original_profile, to_profile, aligned_profile);
+        aligned_profile->set_unique_id(aligned_unique_id);
+        environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*aligned_profile, *original_profile);
+        aligned_profile->set_stream_index(original_profile->get_stream_index());
+        aligned_profile->set_stream_type(original_profile->get_stream_type());
+        aligned_profile->set_format(original_profile->get_format());
+        aligned_profile->set_framerate(original_profile->get_framerate());
+        if (auto original_video_profile = As<video_stream_profile_interface>(original_profile))
+        {
+            if (auto to_video_profile = As<video_stream_profile_interface>(to_profile))
+            {
+                if (auto aligned_video_profile = As<video_stream_profile_interface>(aligned_profile))
+                {
+                    aligned_video_profile->set_dims(to_video_profile->get_width(), to_video_profile->get_height());
+                    auto aligned_intrinsics = original_video_profile->get_intrinsics();
+                    aligned_intrinsics.width = to_video_profile->get_width();
+                    aligned_intrinsics.height = to_video_profile->get_height();
+                    aligned_video_profile->set_intrinsics([aligned_intrinsics]() { return aligned_intrinsics; });
+                }
+            }
+        }
+        return aligned_profile;
+    }
     void align::on_frame(frame_holder frameset, librealsense::synthetic_source_interface* source)
     {
         auto composite = As<composite_frame>(frameset.frame);
@@ -240,7 +292,9 @@ namespace librealsense
             {
                 //Align a stream to depth
                 auto aligned_bytes_per_pixel = other_frame->get_bpp() / 8;
-                aligned_frame = source->allocate_video_frame(other_profile,
+                auto aligned_profile = create_aligned_profile(other_profile, depth_profile);
+                aligned_frame = source->allocate_video_frame(
+                    aligned_profile,
                     other_frame,
                     aligned_bytes_per_pixel,
                     depth_frame->get_width(),
@@ -268,9 +322,10 @@ namespace librealsense
             {
                 //Align depth to some stream
                 auto aligned_bytes_per_pixel = depth_frame->get_bpp() / 8;
+                auto aligned_profile = create_aligned_profile(depth_profile, other_profile);
                 aligned_frame = source->allocate_video_frame(
-                    depth_profile,
-                    depth_frame,
+                    aligned_profile,
+                    depth_frame, 
                     aligned_bytes_per_pixel,
                     other_intrinsics.width,
                     other_intrinsics.height,

src/proc/align.h

@@ -3,6 +3,8 @@
 
 #pragma once
 
+#include <map>
+#include <utility>
 #include "core/processing.h"
 #include "proc/synthetic-stream.h"
 #include "image.h"
@@ -17,6 +19,13 @@ namespace librealsense
 
     private:
         void on_frame(frame_holder frameset, librealsense::synthetic_source_interface* source);
+        std::shared_ptr<stream_profile_interface> create_aligned_profile(
+            const std::shared_ptr<stream_profile_interface>& original_profile,
+            const std::shared_ptr<stream_profile_interface>& to_profile);
+        int get_unique_id(const std::shared_ptr<stream_profile_interface>& original_profile,
+            const std::shared_ptr<stream_profile_interface>& to_profile,
+            const std::shared_ptr<stream_profile_interface>& aligned_profile);
         rs2_stream _to_stream_type;
+        std::map<std::pair<int, int>, int> align_stream_unique_ids;
     };
 }