Rectifying both streams, scaling to rectified resolution

Author: OhadMeir

include/librealsense2/h/rs_types.h

@@ -193,6 +193,7 @@ typedef enum rs2_extension
     RS2_EXTENSION_DEBUG_STREAM_SENSOR,
     RS2_EXTENSION_CALIBRATION_CHANGE_DEVICE,
     RS2_EXTENSION_ROTATION_FILTER,
+    RS2_EXTENSION_RECTIFICATION_FILTER,
     RS2_EXTENSION_SAFETY_SENSOR,
     RS2_EXTENSION_DEPTH_MAPPING_SENSOR,
     RS2_EXTENSION_LABELED_POINTS,

src/ds/d500/d500-color.cpp

@@ -13,6 +13,7 @@
 #include "platform/platform-utils.h"
 #include <src/metadata-parser.h>
 #include <src/ds/ds-thermal-monitor.h>
+#include <src/proc/rectification-filter.h>
 
 #include <src/ds/features/auto-exposure-roi-feature.h>
 
@@ -248,6 +249,32 @@ namespace librealsense
 
     processing_blocks d500_color_sensor::get_recommended_processing_blocks() const
     {
-        return get_color_recommended_proccesing_blocks();
+        auto res = get_color_recommended_proccesing_blocks();
+
+        std::string dev_name = _owner->get_info( RS2_CAMERA_INFO_NAME );
+        if( dev_name.find( "D555" ) != std::string::npos )
+        {
+            std::vector< uint8_t > table_scope = _owner->get_calibration_table(); // Hold data, table returns as temporary
+            auto table = ds::check_calib< ds::d500_coefficients_table >( table_scope );
+            float3x3 base_intrinsics = {0};
+            base_intrinsics( 0, 0 ) = table->right_coefficients_table.base_instrinsics.fx;
+            base_intrinsics( 1, 1 ) = table->right_coefficients_table.base_instrinsics.fy;
+            base_intrinsics( 2, 0 ) = table->right_coefficients_table.base_instrinsics.ppx;
+            base_intrinsics( 2, 1 ) = table->right_coefficients_table.base_instrinsics.ppy;
+            base_intrinsics( 2, 2 ) = 1;
+            std::vector< float > coeffs{ table->right_coefficients_table.distortion_coeffs,
+                                         table->right_coefficients_table.distortion_coeffs + 5 }; // Using just first 5 coefficients
+            float3x3 rotation = table->right_coefficients_table.rotation_matrix;  // Remove const
+            float3x3 rectified_intrinsics = { 0 };
+            rectified_intrinsics( 0, 0 ) = table->rectified_intrinsics.fx;
+            rectified_intrinsics( 1, 1 ) = table->rectified_intrinsics.fy;
+            rectified_intrinsics( 2, 0 ) = table->rectified_intrinsics.ppx;
+            rectified_intrinsics( 2, 1 ) = table->rectified_intrinsics.ppy;
+            rectified_intrinsics( 2, 2 ) = 1;
+            res.push_back( std::make_shared< rectification_filter >( RS2_STREAM_COLOR, base_intrinsics, coeffs, rotation, rectified_intrinsics,
+                                                                     table->rectified_intrinsics.image_width, table->rectified_intrinsics.image_height ) );
+        }
+
+        return res;
     }
 }

src/ds/d500/d500-device.cpp

@@ -141,12 +141,26 @@ namespace librealsense
         std::string dev_name = _owner->get_info( RS2_CAMERA_INFO_NAME );
         if( dev_name.find( "D555" ) != std::string::npos )
         {
-            auto * table = reinterpret_cast< librealsense::ds::d500_coefficients_table * >( _owner->get_calibration_table().data() );
-
-            res.push_back( std::make_shared< rectification_filter >( table->left_coefficients_table.base_instrinsics,
-                                                                     table->left_coefficients_table.distortion_coeffs,
-                                                                     table->left_coefficients_table.rotation_matrix,
-                                                                     table->rectified_intrinsics ) );
+            std::vector< uint8_t > table_scope = _owner->get_calibration_table(); // Hold data, table returns as temporary
+            auto table = ds::check_calib< ds::d500_coefficients_table >( table_scope );
+            //auto * table = reinterpret_cast< ds::d500_coefficients_table * >( _owner->get_calibration_table().data() );
+            float3x3 base_intrinsics = {0};
+            base_intrinsics( 1, 1 ) = table->left_coefficients_table.base_instrinsics.fy;
+            base_intrinsics( 2, 0 ) = table->left_coefficients_table.base_instrinsics.ppx;
+            base_intrinsics( 2, 1 ) = table->left_coefficients_table.base_instrinsics.ppy;
+            base_intrinsics( 0, 0 ) = table->left_coefficients_table.base_instrinsics.fx;
+            base_intrinsics( 2, 2 ) = 1;
+            std::vector< float > coeffs{ table->left_coefficients_table.distortion_coeffs,
+                                         table->left_coefficients_table.distortion_coeffs + 5 }; // Using just first 5 coefficients
+            float3x3 rotation = table->left_coefficients_table.rotation_matrix; // Remove const
+            float3x3 rectified_intrinsics = { 0 };
+            rectified_intrinsics( 0, 0 ) = table->rectified_intrinsics.fx;
+            rectified_intrinsics( 1, 1 ) = table->rectified_intrinsics.fy;
+            rectified_intrinsics( 2, 0 ) = table->rectified_intrinsics.ppx;
+            rectified_intrinsics( 2, 1 ) = table->rectified_intrinsics.ppy;
+            rectified_intrinsics( 2, 2 ) = 1;
+            res.push_back( std::make_shared< rectification_filter >( RS2_STREAM_INFRARED, base_intrinsics, coeffs, rotation, rectified_intrinsics,
+                                                                     table->rectified_intrinsics.image_width, table->rectified_intrinsics.image_height ) );
         }
 
         return res;

src/proc/color-formats-converter.cpp

@@ -785,55 +785,6 @@ namespace librealsense
             break;
         }
     }
-    // --- Rectification parameters for one camera ---
-    struct RectificationParams
-    {
-        cv::Mat cameraMatrix;
-        cv::Mat distCoeffs;
-        cv::Mat R;
-        cv::Mat newCamMat;
-        cv::Size imageSize;
-    };
-
-    void rectify( uint8_t * const d[], const uint8_t * s, int width, int height, int actual_size )
-    {
-        static bool first_time = true;
-        static cv::Mat map1, map2;
-        static cv::Mat rectified;
-        if( first_time )
-        {
-            RectificationParams params;
-            params.cameraMatrix = ( cv::Mat_< double >( 3, 3 ) << 649.6231, 0, 636.1855, 0, 648.288, 402.3958, 0, 0, 1 );
-            params.distCoeffs = ( cv::Mat_< double >( 1, 5 ) << -0.052214, 0.060396, 0.00053593, -0.0014044, -0.018184 );
-            params.R = ( cv::Mat_< double >( 3, 3 ) << 
-                         0.999949436295546, 0.002088371842696, -0.009836846815234,
-                         -0.002097318108441, 0.999997396311540, -8.992368395849524e-04,
-                         0.009834943262254, 9.198223677947176e-04, 0.999951212718821 );
-            params.newCamMat = ( cv::Mat_< double >( 3, 3 ) << 650.6063, 0, 638.5314, 0, 650.6063, 363.5143, 0, 0, 1 );
-            params.imageSize = cv::Size( 1280, 800 );
-
-            // Preallocate buffer
-            rectified.create( cv::Size( 1280, 800 ), CV_8UC3 );
-
-            // Precompute undistort/rectify maps
-            cv::initUndistortRectifyMap( params.cameraMatrix, params.distCoeffs, params.R,
-                                         params.newCamMat, params.imageSize, CV_16SC2, map1, map2 );
-
-            first_time = false;
-        }
-        
-        if( true )
-        {
-            // Remap to rectified image
-            cv::Mat rgbBuffer( height, width, CV_8UC3, const_cast< uint8_t * >( s ) ); // Uses the data without copying it
-            cv::remap( rgbBuffer, rectified, map1, map2, cv::INTER_LINEAR );
-            memcpy( d[0], rectified.data, actual_size );
-        }
-        else
-        {
-            memcpy( d[0], s, actual_size );
-        }
-    }
 
     void unpack_m420(rs2_format dst_format, rs2_stream dst_stream, uint8_t * const d[], const uint8_t * s, int w, int h, int actual_size)
     {
@@ -847,11 +798,7 @@ namespace librealsense
             unpack_m420<RS2_FORMAT_Y16>(d, s, w, h, actual_size);
             break;
         case RS2_FORMAT_RGB8:
-            uint8_t * unrectified_rgb[1];
-            unrectified_rgb[0] = new uint8_t[w * h * 3];
-            unpack_m420< RS2_FORMAT_RGB8 >( unrectified_rgb, s, w, h, actual_size );
-            rectify( d, unrectified_rgb[0], w, h, actual_size );
-            delete[] unrectified_rgb[0];
+            unpack_m420< RS2_FORMAT_RGB8 >( d, s, w, h, actual_size );
             break;
         case RS2_FORMAT_RGBA8:
             unpack_m420<RS2_FORMAT_RGBA8>(d, s, w, h, actual_size);

src/proc/rectification-filter.cpp

@@ -7,29 +7,49 @@
 
 namespace librealsense
 {
-    rectification_filter::rectification_filter( rsutils::number::float3x3 & k_distorted,
+    rectification_filter::rectification_filter() : stream_filter_processing_block("Rectification Filter")
+    {
+    }
+
+    rectification_filter::rectification_filter( rs2_stream stream_to_rectify,
+                                                rsutils::number::float3x3 & k_distorted,
                                                 std::vector< float > & dist_coeffs,
                                                 rsutils::number::float3x3 & rotation_mat,
-                                                rsutils::number::float3x3 & k_rect )
+                                                rsutils::number::float3x3 & k_rect,
+                                                uint16_t rect_width, uint16_t rect_height )
         : stream_filter_processing_block("Rectification Filter")
+        , _stream_to_rectify( stream_to_rectify )
     {
-        _stream_filter.stream = RS2_STREAM_COLOR;
-        _stream_filter.format = RS2_FORMAT_RGB8;
-        
-        // Initialize rectification parameters with the values from color-formats-converter.cpp
-
         // cv::Mat is raw major, float3x3 is coloumn major
         cv::Mat camera_matrix = ( cv::Mat_< double >( 3, 3 ) << k_distorted( 0, 0 ), k_distorted( 1, 0 ), k_distorted( 2, 0 ),
                                                                 k_distorted( 0, 1 ), k_distorted( 1, 1 ), k_distorted( 2, 1 ),
                                                                 k_distorted( 0, 2 ), k_distorted( 1, 2 ), k_distorted( 2, 2 ) );
 
         cv::Mat coeffs = ( cv::Mat_< double >( 1, 5 ) << dist_coeffs[0], dist_coeffs[1], dist_coeffs[2], dist_coeffs[3], dist_coeffs[4] );
-        cv::Mat R = (cv::Mat_<double>(3, 3) << rotation_mat( 0, 0 ), rotation_mat( 1, 0 ), rotation_mat( 2, 0 ),
-                                               rotation_mat( 0, 1 ), rotation_mat( 1, 1 ), rotation_mat( 2, 1 ), 
-                                               rotation_mat( 0, 2 ), rotation_mat( 1, 2 ), rotation_mat( 2, 2 ) );
-        cv::Mat new_camera_matrix = (cv::Mat_<double>(3, 3) << k_rect( 0, 0 ), k_rect( 1, 0 ), k_rect( 2, 0 ),
-                                                               k_rect( 0, 1 ), k_rect( 1, 1 ), k_rect( 2, 1 ),
-                                                               k_rect( 0, 2 ), k_rect( 1, 2 ), k_rect( 2, 2 ) );
+
+        cv::Mat R = (cv::Mat_<double>(3, 3) << rotation_mat( 0, 0 ), rotation_mat( 0, 1 ), rotation_mat( 0, 2 ),
+                                               rotation_mat( 1, 0 ), rotation_mat( 1, 1 ), rotation_mat( 1, 2 ), 
+                                               rotation_mat( 2, 0 ), rotation_mat( 2, 1 ), rotation_mat( 2, 2 ) );
+
+        // k_rect should be adjusted to new resolution.
+        rsutils::number::float3x3 scaled_k_rect = k_rect;
+
+        auto scale_ratio_x = 1280.0f / static_cast < float >( rect_width );
+        auto scale_ratio_y = 720.0f / static_cast< float >( rect_height );
+        auto scale_ratio = std::max< float >( scale_ratio_x, scale_ratio_y );
+
+        auto crop_x = ( static_cast< float >( rect_width ) * scale_ratio - 1280.0f ) * 0.5f;
+        auto crop_y = ( static_cast< float >( rect_height ) * scale_ratio - 720.0f ) * 0.5f;
+
+        scaled_k_rect( 2, 0 ) = ( scaled_k_rect( 2, 0 ) + 0.5f ) * scale_ratio - crop_x - 0.5f;
+        scaled_k_rect( 2, 1 ) = ( scaled_k_rect( 2, 1 ) + 0.5f ) * scale_ratio - crop_y - 0.5f;
+
+        scaled_k_rect( 0, 0 ) = scaled_k_rect( 0, 0 ) * scale_ratio;
+        scaled_k_rect( 1, 1 ) = scaled_k_rect( 1, 1 ) * scale_ratio;
+
+        cv::Mat new_camera_matrix = (cv::Mat_<double>(3, 3) << scaled_k_rect( 0, 0 ), scaled_k_rect( 1, 0 ), scaled_k_rect( 2, 0 ),
+                                                               scaled_k_rect( 0, 1 ), scaled_k_rect( 1, 1 ), scaled_k_rect( 2, 1 ),
+                                                               scaled_k_rect( 0, 2 ), scaled_k_rect( 1, 2 ), scaled_k_rect( 2, 2 ) );
         cv::Size image_size = cv::Size( 1280, 720 );
 
         // Precompute undistort/rectify maps
@@ -41,11 +61,13 @@ namespace librealsense
 
     bool rectification_filter::should_process( const rs2::frame & frame )
     {
+        if( _rectified_buffer.empty() )
+            return false; // TODO - throw?
+
         if( ! frame || frame.is< rs2::frameset >() )
             return false;
 
-        auto profile = frame.get_profile();
-        return profile.stream_type() == RS2_STREAM_COLOR && profile.format() == RS2_FORMAT_RGB8;
+        return frame.get_profile().format() == RS2_FORMAT_RGB8 && frame.get_profile().stream_type() == _stream_to_rectify;
     }
 
     rs2::frame rectification_filter::process_frame( const rs2::frame_source & source, const rs2::frame & f )
@@ -56,11 +78,7 @@ namespace librealsense
         int width = vf.get_width();
         int height = vf.get_height();
         int bpp = vf.get_bytes_per_pixel();
-        int actual_size = width * height * bpp;
-
-        // Allocate output frame
-        auto target_profile = profile.clone( profile.stream_type(), profile.stream_index(), profile.format() );
-        auto ret = source.allocate_video_frame( target_profile, f, bpp, width, height, width * bpp, RS2_EXTENSION_VIDEO_FRAME );
+        auto ret = source.allocate_video_frame( profile, f, bpp, 1280, 720, 1280 * bpp, RS2_EXTENSION_VIDEO_FRAME );
 
         // Perform rectification
         uint8_t * src = const_cast< uint8_t * >( reinterpret_cast< const uint8_t * >( vf.get_data() ) );
@@ -69,7 +87,7 @@ namespace librealsense
         // Remap to rectified image
         cv::Mat rgb_buffer( height, width, CV_8UC3, src );  // Uses the data without copying it
         cv::remap( rgb_buffer, _rectified_buffer, _map1, _map2, cv::INTER_LINEAR );
-        memcpy( dst, _rectified_buffer.data, actual_size );
+        memcpy( dst, _rectified_buffer.data, 1280 * 720 * bpp );
 
         return ret;
     }

src/proc/rectification-filter.h

@@ -13,26 +13,24 @@ namespace librealsense
     class rectification_filter : public stream_filter_processing_block
     {
     public:
-        rectification_filter( rsutils::number::float3x3 & k_distorted,
+        rectification_filter();
+        rectification_filter( rs2_stream stream_to_rectify,
+                              rsutils::number::float3x3 & k_distorted,
                               std::vector< float > & dist_coeffs,
                               rsutils::number::float3x3 & rotation_mat,
-                              rsutils::number::float3x3 & k_rect );
+                              rsutils::number::float3x3 & k_rect,
+                              uint16_t rect_width, uint16_t rect_height);
 
     protected:
         rs2::frame process_frame(const rs2::frame_source& source, const rs2::frame& f) override;
         bool should_process(const rs2::frame& frame) override;
 
     private:
+        rs2_stream _stream_to_rectify;
+
         cv::Mat _map1;
         cv::Mat _map2;
         cv::Mat _rectified_buffer;
-        
-        // Rectification parameters
-        cv::Mat _camera_matrix;
-        cv::Mat _dist_coeffs;
-        cv::Mat _R;
-        cv::Mat _new_camera_matrix;
-        cv::Size _image_size;
     };
 
     MAP_EXTENSION(RS2_EXTENSION_RECTIFICATION_FILTER, librealsense::rectification_filter);