Fix misaligned image sizes (#35)

- Can set sensor mode for public api - Image sizes not aligned to 32x16 are supported by padding up while copying the image - Changed tester output to .yuv as this format is more readable than P7
cedricve · Sep 21, 2017 · 9ddf7bc · 9ddf7bc
1 parent c4c0dc1
commit 9ddf7bc
Show file tree

Hide file tree

Showing 7 changed files with 108 additions and 19 deletions.
diff --git a/README.md b/README.md
@@ -34,6 +34,8 @@ RGB Mode
 BGR Mode
    - 1280x960: 14 fps,  640x480 : 29.29fps,  320x240 : 29.24fps
 
+Note: when using the full resolution video callbacks with the full resolution of the Raspberry Pi Camera v2, you will likely get an error such as `mmal: mmal_vc_port_enable: failed to enable port vc.ril.camera:out:1(BGR3): ENOSPC`. In order to fix this increase your GPU memory to at least 256MB.
+
 Color conversion is the most time consuming part. We still need to improve that part. Go to src/private and check if you can contribute!
 
 Note: the library is compiled with the options: -Wall -ffunction-sections  -fomit-frame-pointer -O2 -ffast-math -DNDEBUG -mcpu=arm1176jzf-s  -mfpu=vfp -mfloat-abi=hard -ftree-vectorize

diff --git a/src/private/private_impl.cpp b/src/private/private_impl.cpp
@@ -106,6 +106,7 @@ namespace raspicam {
             commitParameters();
             camera_video_port   = State.camera_component->output[MMAL_CAMERA_VIDEO_PORT];
             callback_data.pstate = &State;
+            callback_data.inst = this;
             // assign data to use for callback
             camera_video_port->userdata = ( struct MMAL_PORT_USERDATA_T * ) &callback_data;
             State.camera_component->control->userdata = ( struct MMAL_PORT_USERDATA_T * ) &callback_data;
@@ -257,10 +258,10 @@ namespace raspicam {
             }
 
             video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
-        
+
             //set sensor mode
-            if ( state->sensor_mode != 0 && mmal_port_parameter_set_uint32 ( camera->control, 
-                                                    MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG, 
+            if ( state->sensor_mode != 0 && mmal_port_parameter_set_uint32 ( camera->control,
+                                                    MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG,
                                                     state->sensor_mode)  != MMAL_SUCCESS)
             {
                 cerr << __func__ << ": Failed to set sensmode.";
@@ -536,8 +537,57 @@ namespace raspicam {
                 if( buffer->length &&
                         ( pData->_userCallback || pData->wantToGrab )){
                     mmal_buffer_header_mem_lock ( buffer );
-                    pData->_buffData.resize ( buffer->length );
-                    memcpy ( pData->_buffData.data,buffer->data,buffer->length );
+
+                    Private_Impl *self = pData->inst;
+
+                    int width = pData->pstate->width;
+                    int height = pData->pstate->height;
+                    RASPICAM_FORMAT fmt = pData->pstate->captureFtm;
+                    bool encoded = false; // So far only unencoded formats can be configured
+
+                    // For unencoded formats, the buffer is padding to blocks
+                    // TODO: According to picamera ('Under certain circumstances (non-resized, non-YUV, video-port captures), the resolution is rounded to 16x16 blocks instead of 32x16. Adjust your resolution rounding accordingly')
+                    int bufferWidth = VCOS_ALIGN_UP(width, 32);
+                    int bufferHeight = VCOS_ALIGN_UP(height, 16);
+
+                    if ( bufferWidth == width || encoded ) {
+                        pData->_buffData.resize ( buffer->length );
+                        memcpy ( pData->_buffData.data,buffer->data,buffer->length );
+                    }
+                    else {
+
+                        pData->_buffData.resize ( self->getImageTypeSize( fmt ) );
+
+                        int bpp = 1;
+                        if(fmt == RASPICAM_FORMAT_RGB || fmt == RASPICAM_FORMAT_BGR) {
+                            bpp = 3;
+                        }
+
+                        for(int i = 0; i < height; i++) {
+                            memcpy ( pData->_buffData.data + i*width*bpp, buffer->data + i*bufferWidth*bpp, width*bpp);
+                        }
+
+                        if ( fmt == RASPICAM_FORMAT_YUV420 ) {
+                            // Starting points in both buffers
+                            uint8_t *outUV = pData->_buffData.data + width*height;
+                            uint8_t *bufferUV = buffer->data + bufferHeight*bufferWidth;
+
+                            width /= 2;
+                            height /= 2;
+                            bufferWidth /= 2;
+                            bufferHeight /= 2;
+
+                            for(int plane = 0; plane < 2; plane++) {
+                                for(int i = 0; i < height; i++) {
+                                    memcpy ( outUV + i*width, bufferUV + i*bufferWidth, width );
+                                }
+                                outUV += width*height;
+                                bufferUV += bufferWidth*bufferHeight;
+                            }
+                        }
+
+                    }
+
                     pData->wantToGrab =false;
                     hasGrabbed=true;
                     mmal_buffer_header_mem_unlock ( buffer );
@@ -890,4 +940,3 @@ namespace raspicam {
         }
     };
 };
-
diff --git a/src/private/private_impl.h b/src/private/private_impl.h
@@ -42,7 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <mutex>
 #include <string>
 #include "raspicamtypes.h"
-#include "private_types.h" 
+#include "private_types.h"
 #include "threadcondition.h"
 namespace raspicam {
     namespace _private
@@ -73,6 +73,7 @@ namespace raspicam {
 
 
                 RASPIVID_STATE *pstate;            /// pointer to our state in case required in callback
+                Private_Impl *inst;
                 std::mutex _mutex;
                 ThreadCondition Thcond;
                 bool wantToGrab;
@@ -304,5 +305,3 @@ namespace raspicam {
 };
 
 #endif
-
-
diff --git a/src/raspicam.cpp b/src/raspicam.cpp
@@ -141,8 +141,12 @@ namespace raspicam {
     void RaspiCam::setFrameRate ( int frames_per_second ) {
         _impl->setFrameRate ( frames_per_second );
     }
+    void RaspiCam::setSensorMode ( int mode ) {
+        _impl->setSensorMode ( mode );
+    }
+
+
 
-
     RASPICAM_FORMAT RaspiCam::getFormat()const{return _impl->getFormat( ); }
     unsigned int RaspiCam::getWidth() const{return _impl->getWidth() ;}
     unsigned int RaspiCam::getHeight() const{return _impl->getHeight()  ;}
@@ -171,4 +175,3 @@ namespace raspicam {
 
 
 };
-
diff --git a/src/raspicam.h b/src/raspicam.h
@@ -146,6 +146,7 @@ namespace raspicam {
         void setHorizontalFlip ( bool hFlip );
         void setVerticalFlip ( bool vFlip );
         void setFrameRate ( int frames_per_second );
+        void setSensorMode ( int mode );
 
         //Accessors
         RASPICAM_FORMAT getFormat() const;
@@ -184,4 +185,3 @@ namespace raspicam {
     };
 };
 #endif
-
diff --git a/test.sh b/test.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+# Run this script to test the library's functionality of various formats and sizes
+OPTS=""
+
+func () {
+	f=$1_$2x$3
+	mkdir -p "$f"; cd "$f"
+	../../build/utils/raspicam_test $OPTS -$1 -w $2 -h $3
+	cd ..
+}
+
+func2 () {
+	func $1 3280 2464
+	func $1 1640 1232
+	func $1 1280 720
+}
+
+
+mkdir -p out; cd out
+
+func2 rgb
+func2 yuv
+func2 gr
+
+cd ..
diff --git a/utils/raspicam_test.cpp b/utils/raspicam_test.cpp
@@ -112,7 +112,7 @@ void processCommandLine ( int argc,char **argv,raspicam::RaspiCam &Camera ) {
 
     if ( findParam ( "-gr",argc,argv ) !=-1 )
       Camera.setFormat(raspicam::RASPICAM_FORMAT_GRAY);
-    if ( findParam ( "-yuv",argc,argv ) !=-1 ) 
+    if ( findParam ( "-yuv",argc,argv ) !=-1 )
       Camera.setFormat(raspicam::RASPICAM_FORMAT_YUV420);
     if ( findParam ( "-test_speed",argc,argv ) !=-1 )
         doTestSpeedOnly=true;
@@ -176,12 +176,23 @@ void saveImage ( string filepath,unsigned char *data,raspicam::RaspiCam &Camera
     } else if ( Camera.getFormat()==raspicam::RASPICAM_FORMAT_GRAY ) {
         outFile<<"P5\n";
     } else if ( Camera.getFormat()==raspicam::RASPICAM_FORMAT_YUV420 ) { //made up format
-        outFile<<"P7\n";
+        // Openable as a .yuv format
+        outFile.write ( ( char* ) data,Camera.getImageBufferSize() );
+        return;
     }
     outFile<<Camera.getWidth() <<" "<<Camera.getHeight() <<" 255\n";
     outFile.write ( ( char* ) data,Camera.getImageBufferSize() );
 }
 
+string getExtension ( raspicam::RaspiCam &Camera ) {
+    if ( Camera.getFormat()==raspicam::RASPICAM_FORMAT_YUV420 ) {
+        return ".yuv";
+    }
+    else {
+        return ".ppm";
+    }
+}
+
 struct CallBackData_t{
     raspicam::RaspiCam* Camera;
     unsigned char *data;
@@ -202,7 +213,7 @@ void vidInCallback(void* data){
             std::stringstream fn;
             fn<<"image";
             if (camData->i<10) fn<<"0";
-            fn<<camData->i<<".ppm";
+            fn<<camData->i<<getExtension(*camData->Camera);
             saveImage ( fn.str(),camData->data,*(camData->Camera) );
             cerr<<"Saving "<<fn.str()<<endl;
         }
@@ -225,11 +236,11 @@ int main ( int argc,char **argv ) {
         return -1;
     }
 
-  
+
     raspicam::RaspiCam Camera;
     processCommandLine ( argc,argv,Camera );
     cout<<"Connecting to camera"<<endl;
-    
+
     if ( !Camera.open() ) {
         cerr<<"Error opening camera"<<endl;
         return -1;
@@ -267,15 +278,15 @@ int main ( int argc,char **argv ) {
                     std::stringstream fn;
                     fn<<"image";
                     if (i<10) fn<<"0";
-                    fn<<i<<".ppm";
+                    fn<<i<<getExtension(Camera);
                     saveImage ( fn.str(),data,Camera );
                     cerr<<"Saving "<<fn.str()<<endl;
                 }
             }
         }while(++i<nFramesCaptured || nFramesCaptured==0);//stops when nFrames captured or at infinity lpif nFramesCaptured<0
     }
     timer.end();
-    if ( !doTestSpeedOnly )    cout<<endl<<"Images saved in imagexx.ppm"<<endl;
+    if ( !doTestSpeedOnly )    cout<<endl<<"Images saved in imagexx"<<getExtension(Camera)<<endl;