Get color frame according to index (there can be more than one)

Author: OhadMeir

common/post-processing-filters.cpp

@@ -131,6 +131,10 @@ void post_processing_filters::map_id_frameset_to_frameset(rs2::frameset first, r
         {
             second_f = second.get_infrared_frame(f.get_profile().stream_index());
         }
+        else if( f.get_profile().stream_type() == RS2_STREAM_COLOR )
+        {
+            second_f = second.get_color_frame( f.get_profile().stream_index() );
+        }
         else
         {
             second_f = second.first_or_default(f.get_profile().stream_type());

include/librealsense2/hpp/rs_frame.hpp

@@ -1135,16 +1135,27 @@ namespace rs2
         * Retrieve the first color frame, if no frame is found, search for the color frame from IR stream. If one still can't be found, return an empty frame instance.
         * \return video_frame - first found color frame.
         */
-        video_frame get_color_frame() const
+        video_frame get_color_frame( const size_t index = 0 ) const
         {
-            auto f = first_or_default(RS2_STREAM_COLOR);
+            frame f;
+
+            foreach_rs( [&f, index]( const frame & frm ) {
+                if( frm.get_profile().stream_type() == RS2_STREAM_COLOR &&
+                    frm.get_profile().stream_index() == index )
+                    f = frm;
+            } );
 
-            if (!f)
+            if( ! f )
             {
-                auto ir = first_or_default(RS2_STREAM_INFRARED);
-                if (ir && ir.get_profile().format() == RS2_FORMAT_RGB8)
-                    f = ir;
+                // Color frame can also come from infrared sensor
+                foreach_rs( [&f, index]( const frame & frm ) {
+                    if( frm.get_profile().stream_type() == RS2_STREAM_INFRARED &&
+                        frm.get_profile().stream_index() == index &&
+                        frm.get_profile().format() == RS2_FORMAT_RGB8 )
+                        f = frm;
+                } );
             }
+
             return f;
         }