├── LICENSE.txt ├── RCJ_2014 ├── Makefile ├── RCJ_2014.cpp ├── README.md ├── RaspiCamCV.c └── wiringpi and opencv and raspicam compile command └── README.md /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | -------------------------------------------------------------------------------- /RCJ_2014/Makefile: -------------------------------------------------------------------------------- 1 | # I edited this makefile to make it work after the new software updates. Original by Emil Valkov (robidouille.wordpress.com). 2 | OBJS = objs 3 | 4 | CFLAGS_OPENCV = -I/usr/include/opencv 5 | LDFLAGS2_OPENCV = -lopencv_highgui -lopencv_core -lopencv_legacy -lopencv_video -lopencv_features2d -lopencv_calib3d -lopencv_imgproc -lpthread -lm 6 | 7 | USERLAND_ROOT = $(HOME)/git/raspberrypi/userland 8 | CFLAGS_PI = \ 9 | -I$(USERLAND_ROOT)/host_applications/linux/libs/bcm_host/include \ 10 | -I$(USERLAND_ROOT)/host_applications/linux/apps/raspicam \ 11 | -I$(USERLAND_ROOT) \ 12 | -I$(USERLAND_ROOT)/interface/vcos/pthreads \ 13 | -I$(USERLAND_ROOT)/interface/vmcs_host/linux \ 14 | -I$(USERLAND_ROOT)/interface/mmal \ 15 | 16 | LDFLAGS_PI = -L$(USERLAND_ROOT)/build/lib -lmmal_core -lmmal -l mmal_util -lvcos -lbcm_host 17 | 18 | BUILD_TYPE=debug 19 | #BUILD_TYPE=release 20 | 21 | CFLAGS_COMMON = -Wno-multichar -g $(CFLAGS_OPENCV) $(CFLAGS_PI) -MD 22 | 23 | ifeq ($(BUILD_TYPE), debug) 24 | CFLAGS = $(CFLAGS_COMMON) 25 | endif 26 | ifeq ($(BUILD_TYPE), release) 27 | CFLAGS = $(CFLAGS_COMMON) -O3 28 | endif 29 | 30 | LDFLAGS = 31 | LDFLAGS2 = $(LDFLAGS2_OPENCV) $(LDFLAGS_PI) -lX11 -lXext -lrt -lstdc++ 32 | 33 | RASPICAMCV_OBJS = \ 34 | $(OBJS)/RaspiCamControl.o \ 35 | $(OBJS)/RaspiCLI.o \ 36 | $(OBJS)/RaspiCamCV.o \ 37 | 38 | RASPICAMTEST_OBJS = \ 39 | $(OBJS)/RaspiCamTest.o \ 40 | 41 | TARGETS = libraspicamcv.a raspicamtest 42 | 43 | all: $(TARGETS) 44 | 45 | $(OBJS)/%.o: %.c 46 | gcc -c $(CFLAGS) $< -o $@ 47 | 48 | $(OBJS)/%.o: $(USERLAND_ROOT)/host_applications/linux/apps/raspicam/%.c 49 | gcc -c $(CFLAGS) $< -o $@ 50 | 51 | libraspicamcv.a: $(RASPICAMCV_OBJS) 52 | ar rcs libraspicamcv.a -o $+ 53 | 54 | raspicamtest: $(RASPICAMTEST_OBJS) libraspicamcv.a 55 | gcc $(LDFLAGS) $+ $(LDFLAGS2) -L. -lraspicamcv -o $@ 56 | 57 | clean: 58 | rm -f $(OBJS)/* $(TARGETS) 59 | 60 | -include $(OBJS)/*.d 61 | -------------------------------------------------------------------------------- /RCJ_2014/RCJ_2014.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Line follower algorithm 3 | * 4 | * Version: 1.0 5 | * Date: 2014-10-24 6 | * 7 | * 8 | * Copyright 2014 Arne Baeyens 9 | * 10 | * Licensed under the Apache License, Version 2.0 (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | * 22 | * The object tracking part is mainly made by Kyle Hounslow. Check his excellent tutorial about object tracking: youtube.com/watch?v=bSeFrPrqZ2A 23 | * or the complete source code: dl.dropboxusercontent.com/u/28096936/tuts/objectTrackingTut.cpp. 24 | * Without the work of Pierre Raufast (thinkrpi.wordpress.com) and Emil Valvov (robidouille.wordpress.com) this project would have been much more difficult. 25 | * A big thanks to them. 26 | * 27 | * 2arne.baeyens@gmail.com 28 | * 29 | * 30 | * For compiling, please refer to the README on github or the forum thread. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "RaspiCamCV.h" // Emil Valcov`s library. 40 | 41 | #include 42 | #include 43 | 44 | #define PI 3.14159265 45 | 46 | 47 | using namespace cv; 48 | using namespace std; 49 | 50 | // Debug settings variables. 51 | bool debug = 1; // Debugging on or off. 52 | bool show_images = 1; // Show images or not. 53 | bool debug_mode; // Variable to regulate debugging inside program. 54 | //bool camera_show = 0; // Show camera feed or not. 55 | 56 | // Settings for object tracking and drawing. 57 | const int FRAME_WIDTH = 320; 58 | const int FRAME_HEIGHT = 240; 59 | 60 | /* FUNCTIONS */ 61 | // For scanning and searching the line. 62 | void scancircle( Mat& I, Point Mp, int radius, int look_angle, int width ); 63 | void scanline( Mat& I, Point Mp, int line_radius ); 64 | Point findLine( char scan_mode ); 65 | 66 | // For object tracking. 67 | void drawObject(int x, int y,Mat &frame, Scalar color ); 68 | void trackObject(int *x, int *y, Mat &threshold); 69 | // Variables for green dot. 70 | int green_x = 160, green_y = 120; 71 | bool objectFound = false; 72 | //bool greenField = false; 73 | int green_area = 1; 74 | 75 | // For external communication over UART. 76 | void sendPositionData( void ); 77 | void sendCommand( char command_1, char command_2 ); 78 | void initSerialPort( void ); 79 | int fd; // file descriptor for serial port. 80 | char data_received = 0; // Character to store answer from microcontroller. 81 | 82 | bool test_inimage( Mat& I, short x_cor, short y_cor ); 83 | 84 | void loadTime( float *t_load_pointer ); 85 | 86 | void dataDo( char rx_data ); 87 | 88 | int lineangle( void ); // Returns the angle of the line (0 - 360 degrees), using global variables. 89 | 90 | int first_angle=0; 91 | 92 | 93 | // Data about scans. 94 | uchar scandata[640]; // Array to contain scan values. 95 | short scan_w; // Number of (active) values in scandata array. 96 | Point scanpoint; // Last scanpoint. 97 | Point line_point; // Last x coordinates of found line. 98 | vector line_points(7); // vector om de laatste gevonden punten bij te houden. 99 | Point first_scanpoint; // Eerst gevonden punt. 100 | 101 | int scan_counter = 0; // Variable to count how many scans are done. 102 | 103 | short scan_radius; 104 | short sc_strt, sc_end; 105 | 106 | short new_scan_radius1 = 140; 107 | 108 | // Variables for trackbars. 109 | int scan_height_reg = 220; 110 | int scan_posx_reg = 160; 111 | int scan_radius1_reg = 55; 112 | int scan_radius2_reg = 18; 113 | int look_angle_reg = 180; 114 | int look_width_reg = 160; 115 | 116 | int H_min_reg = 50; 117 | int H_max_reg = 87; 118 | int S_min_reg = 71; 119 | int S_max_reg = 256; // 133 120 | int V_min_reg = 80; // 90 121 | int V_max_reg = 256; // 197 122 | 123 | 124 | // The calculated line error for transmittment 125 | short line_error; 126 | short P_Error, I_Error; 127 | 128 | // Some variables for time measurement. 129 | float time_e; 130 | double time_running = (double)getTickCount()/getTickFrequency(); 131 | double time_old = 0; 132 | 133 | // Matrices. 134 | Mat image; // image from camera. 135 | Mat gray_image; // gray camera image. 136 | Mat HSV_image; // HSV converted image - for object tracking. 137 | Mat thresh_image; 138 | Mat eroded_image; 139 | Mat scan_image( 256, 640, CV_8UC3, Scalar( 255, 255, 255)); // to graph greyscale values on. 140 | Mat D_scan_image( 256, 640, CV_8UC3, Scalar( 255, 255, 255)); // to graph the differences in greyscale values. 141 | //Mat exp_image( 240, 320, CV_8U, Scalar( 255 )); // for experimentation purposes. 142 | 143 | Mat erodeElement = getStructuringElement( MORPH_RECT, Size(4,4)); // matrix element to erode image - eroding removes small spots. 144 | 145 | vector > contours; // Variables for object tracking. 146 | vector hierarchy; 147 | 148 | 149 | int main(int argc, const char** argv){ 150 | RaspiCamCvCapture * capture = raspiCamCvCreateCameraCapture(0); // Index doesn't really matter 151 | 152 | // Some strings for the trackbars. 153 | const string track_1 = "trackbar window 1"; 154 | const string track_2 = "trackbar window 2"; 155 | 156 | namedWindow( "camera image", CV_WINDOW_AUTOSIZE ); 157 | namedWindow( "scan image", CV_WINDOW_AUTOSIZE ); 158 | //namedWindow( "gray image", CV_WINDOW_AUTOSIZE ); 159 | namedWindow( "derivative scan", CV_WINDOW_AUTOSIZE ); 160 | namedWindow( "HSV image", CV_WINDOW_AUTOSIZE ); 161 | namedWindow( "thresh image", CV_WINDOW_AUTOSIZE ); 162 | //namedWindow( "experiment", CV_WINDOW_NORMAL ); 163 | //cvResizeWindow( "experiment", 640, 480 ); 164 | 165 | 166 | // Create some trackbars. 167 | namedWindow( track_1, CV_WINDOW_NORMAL ); 168 | cvResizeWindow( "trackbar window 1", 320, 10 ); 169 | createTrackbar( "scan height", track_1, &scan_height_reg, 239 ); 170 | createTrackbar( "scan pos x", track_1, &scan_posx_reg, 319 ); 171 | createTrackbar( "scan radius 1", track_1, &scan_radius1_reg, 320 ); 172 | createTrackbar( "scan radius 2", track_1, &scan_radius2_reg, 160 ); 173 | createTrackbar( "look angle", track_1, &look_angle_reg, 360 ); 174 | createTrackbar( "look width", track_1, &look_width_reg, 360 ); 175 | 176 | namedWindow( track_2, CV_WINDOW_NORMAL ); 177 | cvResizeWindow( "trackbar window 2", 320, 10 ); 178 | createTrackbar( "H_MIN", track_2, &H_min_reg, 256 ); 179 | createTrackbar( "H_MAX", track_2, &H_max_reg, 256 ); 180 | createTrackbar( "S_MIN", track_2, &S_min_reg, 256 ); 181 | createTrackbar( "S_MAX", track_2, &S_max_reg, 256 ); 182 | createTrackbar( "V_MIN", track_2, &V_min_reg, 256 ); 183 | createTrackbar( "V_MAX", track_2, &V_max_reg, 256 ); 184 | 185 | // Initiate the serial port. Uncomment to enable. 186 | //initSerialPort(); 187 | 188 | first_scanpoint = Point(160, scan_height_reg); 189 | 190 | // Debug settings. 191 | //debug=1; 192 | //show_images=1; 193 | 194 | do { 195 | //cout << "getting new image..." << endl; 196 | image = raspiCamCvQueryFrame(capture); // Load image from camera. 197 | //cout << " new image acquired" << endl; 198 | 199 | // Convert colors. 200 | cvtColor( image, HSV_image, COLOR_RGB2HSV ); 201 | cvtColor( image, gray_image, CV_RGB2GRAY ); // Convert it to a gray image. 202 | 203 | // Threshold and erode image. 204 | inRange( HSV_image, Scalar( H_min_reg, S_min_reg, V_min_reg ), Scalar( H_max_reg, S_max_reg, V_max_reg ), thresh_image ); 205 | erode( thresh_image, eroded_image, erodeElement ); 206 | 207 | if(show_images==1){ 208 | imshow( "HSV image", HSV_image ); 209 | imshow( "thresh image", thresh_image ); 210 | } 211 | trackObject( &green_x, &green_y, eroded_image ); 212 | 213 | if(objectFound==true && show_images==1) drawObject( green_x, green_y, image, Scalar( 0, 255, 0 )); 214 | 215 | /* 216 | * Regarding the following lines of code, in particular the global variables. 217 | * 218 | * The functions scanline (and scancircle) copy pixel values from the given image matrix 219 | * and put these in the global array scandata. In the case of scanline, the copied pixel coordinates are on a line, 220 | * in the case of scancircle they are positioned on part of a circle. 221 | * The objective of these two functions is to make the data easier to manipulate. 222 | * 223 | * The function findLine reads the global array scandata (and some other global variables). 224 | * It first searches the array for the signature of the path line (the line the robot has to follow). 225 | * Then it converts the found array index to the position point 226 | * of the intersection of the detection line (or circle) and the line the path line. It returns this point. 227 | * In addition, it also writes this point to the first position of the global array linepoints. 228 | * 229 | * The function lineangle returns the measured angle of the line the robot has to follow. 230 | * It reads the global array linepoints. It is calculated from the vector from the center of the scan circle 231 | * to the intersection point of the scan circle and the path line. 232 | * 233 | * This process is repeated a few times. For each new circle scan, 234 | * the center of the circle is the intersection point found in the previous scan. 235 | * In this way, a ‘snake’ that follows the line is created. 236 | * 237 | * Global variables are not really necessary here. 238 | * It would be better to use pointers and to pass the the address of the output array/vector as an argument. 239 | * 240 | */ 241 | 242 | /* ZERO SCAN */ 243 | scan_counter=0; 244 | if(debug==1) debug_mode=1; 245 | scanline( gray_image, Point(first_scanpoint.x, scan_height_reg), new_scan_radius1 ); 246 | first_scanpoint = findLine( 0 ); // Keep point in memory for next iteration. 247 | debug_mode=0; 248 | new_scan_radius1 = scan_radius1_reg; 249 | 250 | /* FIRST SCAN */ 251 | scancircle( gray_image, line_points[0], scan_radius2_reg, first_angle, look_width_reg ); 252 | findLine( 1 ); 253 | first_angle=lineangle(); // Keep angle in memory for next iteration. 254 | if(first_angle<-45) first_angle = -45; 255 | else if(first_angle>45) first_angle = 45; 256 | 257 | /* SECOND SCAN */ 258 | scancircle( gray_image, line_points[0], scan_radius2_reg, first_angle, 180 ); 259 | findLine( 1 ); 260 | 261 | /* THIRD SCAN */ 262 | scancircle( gray_image, line_points[0], scan_radius2_reg, lineangle(), 180 ); 263 | findLine( 1 ); 264 | 265 | P_Error=first_scanpoint.x-image.cols/2; 266 | 267 | /* FOURTH SCAN */ 268 | scancircle( gray_image, line_points[0], scan_radius2_reg, lineangle(), 180 ); 269 | findLine( 1 ); 270 | 271 | // Copy I error to variable for serial transmission. 272 | I_Error=lineangle(); 273 | // Send the course errors (P_Error and I_Error over the serial port. 274 | // Uncomment to enable. 275 | //sendPositionData(); 276 | 277 | if(show_images==1) imshow( "camera image", image ); // Show camera view after editing. 278 | 279 | 280 | } while (waitKey(10) != 'q'); 281 | 282 | //cvDestroyWindow("RaspiCamTest"); 283 | raspiCamCvReleaseCapture(&capture); 284 | return 0; 285 | } 286 | 287 | void scancircle( Mat& I, Point Mp, int radius, int look_angle, int width ){ 288 | int i,j; 289 | int n = 0; 290 | Point dot_pos, dot2_pos; 291 | 292 | scanpoint = Mp; 293 | 294 | scan_radius = radius; 295 | sc_strt = Mp.x - radius; 296 | sc_end = Mp.x + radius; 297 | 298 | Point end_point_left = Point( scanpoint.x - sin(PI*(look_angle+180-width/2)/180)*scan_radius, scanpoint.y - cos(PI*(look_angle+180-width/2+180)/180)*scan_radius ); 299 | Point end_point_right = Point( scanpoint.x - sin(PI*(look_angle+180+width/2)/180)*scan_radius, scanpoint.y + cos(PI*(look_angle+180+width/2)/180)*scan_radius ); 300 | if(show_images==1) { 301 | circle( image, end_point_left, 5, Scalar( 255, 0, 0 ), -1, 8, 0 ); 302 | circle( image, end_point_right, 5, Scalar( 0, 255, 0 ), -1, 8, 0 ); 303 | } 304 | 305 | for( i=0; i(dot_pos); 311 | else scandata[n]=scandata[n-1]; 312 | n++; 313 | /*I.at(dot_pos) = 0; 314 | waitKey(10); 315 | imshow( "experiment", I );*/ 316 | 317 | for( j=1; j(dot_pos.y, dot_pos.x+j); 319 | else scandata[n] = scandata[n-1]; 320 | n++; 321 | /*I.at(dot_pos.y, dot_pos.x+j)=120 ; 322 | waitKey(10); 323 | imshow( "experiment", I );*/ 324 | } 325 | for( j=1; j(dot_pos.y, dot_pos.x-j); 327 | else scandata[n] = scandata[n-1]; 328 | n++; 329 | /*I.at(dot_pos.y, dot_pos.x-j)=120 ; 330 | waitKey(10); 331 | imshow( "experiment", I );*/ 332 | } 333 | } 334 | 335 | for( i=radius*2-1; i>=0; i-- ){ 336 | dot_pos.x = round(Mp.x+radius*sqrt(1-pow(((float)i/radius)-1,2))); 337 | dot2_pos.x = round(Mp.x+radius*sqrt(1-pow((((float)i+1)/radius)-1,2))); 338 | 339 | dot_pos.y = Mp.y-i+radius; 340 | 341 | for( j=dot2_pos.x-dot_pos.x-1; j>0; j--){ 342 | if(test_inimage( I, dot_pos.x, dot_pos.y+j)) scandata[n]=I.at(dot_pos.y, dot_pos.x+j); 343 | else scandata[n] = scandata[n-1]; 344 | n++; 345 | /*I.at(dot_pos.y, dot_pos.x+j)=120 ; 346 | waitKey(100); 347 | imshow( "experiment", I );*/ 348 | } 349 | for( j=dot_pos.x-dot2_pos.x-1; j>0; j--){ 350 | if(test_inimage( I, dot_pos.x, dot_pos.y-j)) scandata[n]=I.at(dot_pos.y, dot_pos.x-j); 351 | else scandata[n] = scandata[n-1]; 352 | n++; 353 | /*I.at(dot_pos.y, dot_pos.x-j)=120 ; 354 | waitKey(100); 355 | imshow( "experiment", I );*/ 356 | } 357 | 358 | if(test_inimage( I, dot_pos.x, dot_pos.y)) scandata[n]=I.at(dot_pos); 359 | else scandata[n]=scandata[n-1]; 360 | 361 | n++; 362 | /*I.at(dot_pos.y, dot_pos.x) = 0; 363 | waitKey(100); 364 | imshow( "experiment", I );*/ 365 | } 366 | 367 | scan_w = n-1; 368 | //cout << scan_w << endl; 369 | 370 | int look_strt = round((float)scan_w*(look_angle+180-width/2)/360); 371 | int look_end = round((float)scan_w*(look_angle+180+width/2)/360); 372 | 373 | for(i=0; i(Mp.y); 405 | 406 | for( i=0; i=I.cols) scandata[i]=p[I.cols-2]; 410 | else scandata[i] = p[i+sc_strt]; 411 | if(debug_mode==1) line( scan_image, Point(i,scan_image.rows), Point(i,scandata[i]), Scalar( 0, 0, 0 ), 1, 8 ); 412 | //cout << int(scanline[i]) << " "; 413 | //cout << i << " "; 414 | } 415 | if( show_images == 1 ){ 416 | line( image, Point(0, Mp.y), Point(image.cols, Mp.y), Scalar( 0, 0, 255 ), 1, 8 ); 417 | if(Mp.x-line_radius>=0) circle( image, Point(Mp.x-line_radius, Mp.y), 5, Scalar( 255, 0, 0 ), -1, 8, 0 ); 418 | if(Mp.x+line_radius left_side.y ){ 442 | left_side.y = der_scan[i]; 443 | left_side.x = i; 444 | } 445 | if( der_scan[i] < right_side.y ){ 446 | right_side.y = der_scan[i]; 447 | right_side.x = i; 448 | } 449 | } 450 | 451 | 452 | float line_pos = (right_side.x+left_side.x)/2; 453 | if ( scan_mode == 0 ) line_point = Point( line_pos+sc_strt, scanpoint.y ); 454 | else if ( scan_mode == 1 ){ 455 | line_pos=line_pos/scan_w; 456 | line_point = Point( scanpoint.x + cos(PI*((float)line_pos*2+0.5))*scan_radius, scanpoint.y + sin(PI*((float)line_pos*2+0.5))*scan_radius ); 457 | if(show_images==1) circle( image, scanpoint, scan_radius, Scalar( 255, 0, 0 ), 1, 8, 0 ); 458 | } 459 | 460 | if(line_point.x>=image.cols) line_point.x=image.cols-1; // Vermijden dat het gevonden punt buiten het beeld ligt. 461 | else if(line_point.x<0) line_point.x = 0; 462 | 463 | if(show_images==1) circle( image, Point(line_point.x, line_point.y), 5, Scalar( 0, 0, 255 ), -1, 8, 0 ); 464 | 465 | //circle( D_scan_image, Point( left_side.x+sc_strt, D_scan_image.rows/2-left_side.y ), 5, Scalar( 255, 0, 255 ), 2, 8, 0 ); 466 | //circle( D_scan_image, Point( right_side.x+sc_strt, D_scan_image.rows/2-right_side.y ), 5, Scalar( 0, 255, 255 ), 2, 8, 0 ); 467 | 468 | if(debug_mode==1){ 469 | imshow( "scan image", scan_image ); 470 | imshow( "derivative scan", D_scan_image ); 471 | scan_image = Mat( 256, 640, CV_8UC3, Scalar( 255, 255, 255)); 472 | D_scan_image = Mat( 256, 640, CV_8UC3, Scalar( 255, 255, 255)); 473 | } 474 | 475 | //cout << "line_error: " << line_error << endl; 476 | //cout << endl; 477 | for(i=2;i>=0;i--) line_points[i+1]=line_points[i]; // Waarden doorschuiven. 478 | line_points[0]=line_point; // Laatst gevonden punt opslaan in de lijst. 479 | 480 | return line_point; 481 | } 482 | 483 | void drawObject(int x, int y,Mat &frame, Scalar color){ 484 | /* by Kyle Hounslow */ 485 | //use some of the openCV drawing functions to draw crosshairs 486 | //on your tracked image! 487 | 488 | //UPDATE:JUNE 18TH, 2013 489 | //added 'if' and 'else' statements to prevent 490 | //memory errors from writing off the screen (ie. (-25,-25) is not within the window!) 491 | 492 | circle(frame,Point(x,y),10,color,1); 493 | if(y-15>0) 494 | line(frame,Point(x,y),Point(x,y-15),color,1); 495 | else line(frame,Point(x,y),Point(x,0),color,1); 496 | if(y+150) 500 | line(frame,Point(x,y),Point(x-15,y),color,1); 501 | else line(frame,Point(x,y),Point(0,y),color,1); 502 | if(x+15 0) { 519 | int numObjects = hierarchy.size(); 520 | //cout << " numObjects: " << numObjects << endl; 521 | if(numObjects<10) { 522 | for(int index = 0; index >=0; index = hierarchy[index][1]) { // Changed 0 to 1 to return position of closest intead of farthest object. 523 | //cout << " index: " << index << endl; 524 | Moments moment = moments((cv::Mat)contours[index]); 525 | green_area = moment.m00; 526 | cout<< " area: " << green_area << endl; 527 | 528 | if(green_area>50 && green_area>refArea){ 529 | //cout << "area: " << area << endl; 530 | *x = moment.m10/green_area; 531 | *y = moment.m01/green_area; 532 | objectFound = true; 533 | } 534 | else{ 535 | objectFound = false; 536 | *x=workImage.cols/2; 537 | *y=15; 538 | } 539 | //cout << " *x: " << *x << endl << " *y: " << *y << endl << endl; 540 | } 541 | //if(objectFound == true); 542 | //putText(image, "Object", Point(0,50), 2, 1, Scalar(0,255,0), 2); 543 | //drawObject(*x,*y,image); 544 | } 545 | else cout << "Too much noise!" << endl; 546 | } 547 | //cout << "/* End tracking */" << endl << endl; 548 | } 549 | 550 | bool test_inimage( Mat& I, short x_cor, short y_cor ){ 551 | if( x_cor >= 0 && y_cor >=0 && x_cor < I.cols && y_cor < I.rows ) return 1; 552 | else return 0; 553 | } 554 | 555 | int lineangle( void ){ 556 | return round(atan2( (line_points[0].x-scanpoint.x), -(line_points[0].y-scanpoint.y) )*180/PI); 557 | } 558 | 559 | void loadTime( float *t_load_pointer ){ 560 | time_running = (float)getTickCount()/getTickFrequency(); 561 | *t_load_pointer=time_running-time_old; 562 | time_old=time_running; 563 | } 564 | 565 | void initSerialPort( void ){ 566 | if((fd = serialOpen("/dev/ttyAMA0", 115200)) < 0) { 567 | cout << "Unable to open serial device: " << fd << endl; 568 | //return 1; 569 | } 570 | } 571 | 572 | void sendPositionData( void ){ 573 | //short new_data; 574 | /*char msg[2]; 575 | msg[0] = (line_error & 0xff00) >> 8; 576 | msg[1] = line_error & 0x00ff;*/ 577 | //cout << "msg_data: " << int(msg[0]) << " and " << int(msg[1]) << " endline"<< endl; 578 | 579 | /*new_data = msg[0]; 580 | new_data = (new_data<<8)+msg[1]; 581 | cout << "send_data: " << new_data << endl;*/ 582 | 583 | // send syntax: $RPIE,* 584 | // example: $RPIE5k* 585 | serialPrintf( fd, "$PIR" ); 586 | /*serialPutchar( fd, msg[0] ); 587 | serialPutchar( fd, msg[1] );*/ 588 | serialPutchar( fd, char(P_Error/2) ); 589 | serialPutchar( fd, char(I_Error) ); 590 | serialPutchar( fd, 0x2A ); // Send * to terminate the message. 591 | // Receive answer. 592 | if(serialDataAvail( fd ) == 1){ 593 | data_received = serialGetchar( fd ); 594 | cout << "data char: " << data_received << endl; 595 | dataDo( data_received ); // Do something with the received data. 596 | } 597 | serialFlush( fd ); // Clean the serial port. 598 | } 599 | 600 | void sendCommand( char command_1, char command_2 ){ 601 | serialPrintf( fd, "$PIC" ); 602 | serialPutchar( fd, command_1 ); 603 | serialPutchar( fd, command_2 ); 604 | serialPutchar( fd, 0x2A ); // Send * to terminate the message. 605 | } 606 | 607 | void dataDo( char rx_data ){ 608 | if( rx_data == 'R' ){ 609 | first_scanpoint.x = 160; 610 | new_scan_radius1 = 140; 611 | first_angle=0; 612 | } 613 | } 614 | -------------------------------------------------------------------------------- /RCJ_2014/README.md: -------------------------------------------------------------------------------- 1 | Project RCJ 2014 2 | ================ 3 | by Arne Baeyens 4 | 5 | This repository contains the source code I wrote for my image-processing robot for the RoboCup Junior competition in may 2014. It uses a Raspberry Pi and Pi camera for the image recognition and a Dwengo microcontroller board to control the robot. The robot recognizes a line and does object tracking at the same time and sends the line's position and angle over the serial port for further processing. [Here](http://youtu.be/AsoLF6NsqBI) is a short video of the 'thinking' of the robot but the program also gives other output like graphs and threshold images. 6 | About my robot: [raspberrypi.org/an-image-processing-robot-for-robocup-junior](http://www.raspberrypi.org/an-image-processing-robot-for-robocup-junior/). 7 | 8 | Please note that the RaspiCamCV.c file and the Makefile are made by Emil Valkov and Pierre Raufast. I only edited them slightly. 9 | 10 | ### Note (July 2019) 11 | - At the time when I developed this project, the Raspberry Pi camera and the Raspberry Pi in general were still very new. 12 | To get the camera data in OpenCV, I had to use the RaspiCamCV library. Over time, interfaces that are far easier to use where developed, in particular for Python. You might want to use those newer methods. 13 | - I chose to use C++ because Python becomes quite slow if pixels are accessed. If you choose to use OpenCV functions for all the heavy work (which is sufficient for almost all projects), the speed difference with C++ tends to be negligible. 14 | - If you are interested in this project, I highly recommend to take a look at the [the PyImageSearch site](https://www.pyimagesearch.com/). Most of its content is quite advanced, but the author has also written some great introductory articles. 15 | - The line following algorithm in this project was designed to handle simple situations quickly. If I were to redo the project with newer hardware, I would likely use the Hough transform. 16 | 17 | ## How to compile the program: 18 | 19 | 1. Install the openCV library 20 | ---------------------------------------- 21 | sudo apt-get install libopencv-dev 22 | 23 | 2. Install the raspicam library 24 | ---------------------------------------- 25 | Follow Emil Valkov's instructions in his [README](https://github.com/robidouille/robidouille/blob/master/raspicam_cv/README) but replace in step 3 before running the 'make' command the `RaspiCamCV.c` file by the one in my repository. 26 | 27 | UPDATE 27 october 2016: there seems to be a problem with the link above. 28 | Please try the [newer version](https://github.com/robidouille/robidouille/tree/master/raspicam_cv) or use an older [fork of the RaspiCamCV library](https://github.com/abaeyens/robidouille/tree/master/raspicam_cv). 29 | 30 | 3. Compile the C++ program 31 | ---------------------------------------- 32 | Copy the C++ file 'RCJ_2014' to a directory somewhere on your Pi 33 | and compile with 34 | 35 | g++ "%f" -lopencv_highgui -lopencv_core -lopencv_legacy -lopencv_video -lopencv_features2d -lopencv_calib3d -lopencv_imgproc -lpthread -lm -L/home/pi/git/raspberrypi/userland/build/lib -lmmal_core -lmmal -l mmal_util -lvcos -lbcm_host -lX11 -lXext -lrt -lstdc++ -L/home/pi/git/robidouille/raspicam_cv -lraspicamcv -I/usr/include/opencv -o "%e" -L/usr/local/lib -lwiringPi 36 | 37 | You can use the above command with the geany editor, but if compiling in a terminal you have to change "%f" to the name of the output executable and "%e" to the name of the input c++ file. 38 | For any questions, please refer to the forum thread: 39 | http://www.raspberrypi.org/forums/viewtopic.php?f=37&t=82151 40 | 41 | 4. Run the program 42 | ---------------------------------------- 43 | It is preferable to run the program in terminal as it gives verbose output during running. 44 | 45 | ./RCJ_2014 46 | 47 | To exit the program, select one of the windows and press 'q' 48 | 49 | 50 | Credits 51 | ---------------------------------------- 52 | RaspiCam library: Emil Valkov and Pierre Raufast 53 | * [robidouille.wordpress.com](http://www.robidouille.wordpress.com) 54 | * [thinkrpi.wordpress.com](http://www.thinkrpi.wordpress.com). 55 | 56 | Object tracking: Kyle Hounslow 57 | * [Tutorial: Real-Time Object Tracking Using OpenCV](https://youtube.com/watch?v=bSeFrPrqZ2A) 58 | * or the complete source code: [objectTrackingTut.cpp](https://dl.dropboxusercontent.com/u/28096936/tuts/objectTrackingTut.cpp). 59 | 60 | Other libraries: 61 | * wiringPi library: [wiringpi.com](http://wiringpi.com/) by Gordon Henderson 62 | * the openCV library: [opencv.org](http://opencv.org). 63 | 64 | More info: 65 | 66 | Forum thread: [raspberrypi.org/forums/viewtopic.php?f=37&t=82151](http://raspberrypi.org/forums/viewtopic.php?f=37&t=82151) 67 | 68 | Dwengo site: [dwengo.org/node/46](http://dwengo.org/node/46) 69 | 70 | 71 | 72 | Questions 73 | ----- 74 | If you have any questions, feel free to open an issue or to send an email to 2arne.baeyensgmail.com. 75 | -------------------------------------------------------------------------------- /RCJ_2014/RaspiCamCV.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////// 2 | // 3 | // UPDATE 2014-24-10: 4 | // I edited this program for 320 x 240 resolution instead of the normal 640 x 480. 5 | // use this file for compiling the raspicam library instead of the original file by Emil Valkov, 6 | // as 640 x 480 is a too high resolution to run smoothly on the Raspberry Pi. 7 | // Emil Valkov's github repository: https://github.com/robidouille. 8 | // 9 | // 10 | // 11 | // Many source code lines are copied from RaspiVid.c 12 | // Copyright (c) 2012, Broadcom Europe Ltd 13 | // 14 | // Lines have been added by Pierre Raufast - mai 2013 15 | // pierre.raufast@gmail.com 16 | // to work with OpenCV 2.3 17 | // visit thinkrpi.wordpress.com 18 | // Enjoy ! 19 | // This file display camera in a OpenCv window 20 | // 21 | // For a better world, read Giono's Books 22 | // 23 | ///////////////////////////////////////////////////////////// 24 | // 25 | // Emil Valkov - robidouille@valkov.com 26 | // 27 | // Converted to a library, which exposes an interface 28 | // similar to what OpenCV provides, but uses the RaspiCam 29 | // underneath - September 22 2013. 30 | // 31 | // cvCreateCameraCapture -> raspiCamCvCreateCameraCapture 32 | // cvReleaseCapture -> raspiCamCvReleaseCapture 33 | // cvSetCaptureProperty -> raspiCamCvSetCaptureProperty 34 | // cvQueryFrame -> raspiCamCvQueryFrame 35 | // 36 | ///////////////////////////////////////////////////////////// 37 | 38 | #include "RaspiCamCV.h" 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | //new 46 | #include 47 | #include 48 | #include "time.h" 49 | 50 | #include "bcm_host.h" 51 | #include "interface/vcos/vcos.h" 52 | 53 | #include "interface/mmal/mmal.h" 54 | #include "interface/mmal/mmal_logging.h" 55 | #include "interface/mmal/mmal_buffer.h" 56 | #include "interface/mmal/util/mmal_util.h" 57 | #include "interface/mmal/util/mmal_util_params.h" 58 | #include "interface/mmal/util/mmal_default_components.h" 59 | #include "interface/mmal/util/mmal_connection.h" 60 | 61 | #include "RaspiCamControl.h" 62 | 63 | #include 64 | 65 | /// Camera number to use - we only have one camera, indexed from 0. 66 | #define CAMERA_NUMBER 0 67 | 68 | // Standard port setting for the camera component 69 | #define MMAL_CAMERA_PREVIEW_PORT 0 70 | #define MMAL_CAMERA_VIDEO_PORT 1 71 | #define MMAL_CAMERA_CAPTURE_PORT 2 72 | 73 | // Video format information 74 | #define VIDEO_FRAME_RATE_NUM 30 75 | #define VIDEO_FRAME_RATE_DEN 1 76 | 77 | /// Video render needs at least 2 buffers. 78 | #define VIDEO_OUTPUT_BUFFERS_NUM 3 79 | 80 | // Max bitrate we allow for recording 81 | const int MAX_BITRATE = 30000000; // 30Mbits/s 82 | 83 | int mmal_status_to_int(MMAL_STATUS_T status); 84 | 85 | /** Structure containing all state information for the current run 86 | */ 87 | typedef struct _RASPIVID_STATE 88 | { 89 | int finished; 90 | int width; /// Requested width of image 91 | int height; /// requested height of image 92 | int bitrate; /// Requested bitrate 93 | int framerate; /// Requested frame rate (fps) 94 | int graymode; /// capture in gray only (2x faster) 95 | int immutableInput; /// Flag to specify whether encoder works in place or creates a new buffer. Result is preview can display either 96 | /// the camera output or the encoder output (with compression artifacts) 97 | RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters 98 | 99 | MMAL_COMPONENT_T *camera_component; /// Pointer to the camera component 100 | MMAL_COMPONENT_T *encoder_component; /// Pointer to the encoder component 101 | MMAL_CONNECTION_T *preview_connection; /// Pointer to the connection from camera to preview 102 | MMAL_CONNECTION_T *encoder_connection; /// Pointer to the connection from camera to encoder 103 | 104 | MMAL_POOL_T *video_pool; /// Pointer to the pool of buffers used by encoder output port 105 | 106 | IplImage *py, *pu, *pv; 107 | IplImage *pu_big, *pv_big, *yuvImage,* dstImage; 108 | 109 | VCOS_SEMAPHORE_T capture_sem; 110 | VCOS_SEMAPHORE_T capture_done_sem; 111 | 112 | } RASPIVID_STATE; 113 | 114 | // default status 115 | static void default_status(RASPIVID_STATE *state) 116 | { 117 | if (!state) 118 | { 119 | vcos_assert(0); 120 | return; 121 | } 122 | 123 | // Default everything to zero 124 | memset(state, 0, sizeof(RASPIVID_STATE)); 125 | 126 | // Now set anything non-zero 127 | state->finished = 0; 128 | state->width = 320; // use a multiple of 320 (640, 1280) 129 | state->height = 240; // use a multiple of 240 (480, 960) 130 | state->bitrate = 17000000; // This is a decent default bitrate for 1080p 131 | state->framerate = VIDEO_FRAME_RATE_NUM; 132 | state->immutableInput = 1; 133 | state->graymode = 0; // Gray (1) much faster than color (0) 134 | 135 | // Set up the camera_parameters to default 136 | raspicamcontrol_set_defaults(&state->camera_parameters); 137 | } 138 | 139 | /** 140 | * buffer header callback function for video 141 | * 142 | * @param port Pointer to port from which callback originated 143 | * @param buffer mmal buffer header pointer 144 | */ 145 | static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) 146 | { 147 | MMAL_BUFFER_HEADER_T *new_buffer; 148 | RASPIVID_STATE * state = (RASPIVID_STATE *)port->userdata; 149 | 150 | if (state) 151 | { 152 | if (state->finished) { 153 | vcos_semaphore_post(&state->capture_done_sem); 154 | return; 155 | } 156 | if (buffer->length) 157 | { 158 | mmal_buffer_header_mem_lock(buffer); 159 | 160 | // 161 | // *** PR : OPEN CV Stuff here ! 162 | // 163 | int w=state->width; // get image size 164 | int h=state->height; 165 | int h4=h/4; 166 | 167 | memcpy(state->py->imageData,buffer->data,w*h); // read Y 168 | 169 | if (state->graymode==0) 170 | { 171 | memcpy(state->pu->imageData,buffer->data+w*h,w*h4); // read U 172 | memcpy(state->pv->imageData,buffer->data+w*h+w*h4,w*h4); // read v 173 | } 174 | 175 | vcos_semaphore_post(&state->capture_done_sem); 176 | vcos_semaphore_wait(&state->capture_sem); 177 | 178 | mmal_buffer_header_mem_unlock(buffer); 179 | } 180 | else 181 | { 182 | vcos_log_error("buffer null"); 183 | } 184 | } 185 | else 186 | { 187 | vcos_log_error("Received a encoder buffer callback with no state"); 188 | } 189 | 190 | // release buffer back to the pool 191 | mmal_buffer_header_release(buffer); 192 | 193 | // and send one back to the port (if still open) 194 | if (port->is_enabled) 195 | { 196 | MMAL_STATUS_T status; 197 | 198 | new_buffer = mmal_queue_get(state->video_pool->queue); 199 | 200 | if (new_buffer) 201 | status = mmal_port_send_buffer(port, new_buffer); 202 | 203 | if (!new_buffer || status != MMAL_SUCCESS) 204 | vcos_log_error("Unable to return a buffer to the encoder port"); 205 | } 206 | } 207 | 208 | 209 | /** 210 | * Create the camera component, set up its ports 211 | * 212 | * @param state Pointer to state control struct 213 | * 214 | * @return 0 if failed, pointer to component if successful 215 | * 216 | */ 217 | static MMAL_COMPONENT_T *create_camera_component(RASPIVID_STATE *state) 218 | { 219 | MMAL_COMPONENT_T *camera = 0; 220 | MMAL_ES_FORMAT_T *format; 221 | MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL; 222 | MMAL_STATUS_T status; 223 | 224 | /* Create the component */ 225 | status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera); 226 | 227 | if (status != MMAL_SUCCESS) 228 | { 229 | vcos_log_error("Failed to create camera component"); 230 | goto error; 231 | } 232 | 233 | if (!camera->output_num) 234 | { 235 | vcos_log_error("Camera doesn't have output ports"); 236 | goto error; 237 | } 238 | 239 | video_port = camera->output[MMAL_CAMERA_VIDEO_PORT]; 240 | still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT]; 241 | 242 | // set up the camera configuration 243 | { 244 | MMAL_PARAMETER_CAMERA_CONFIG_T cam_config = 245 | { 246 | { MMAL_PARAMETER_CAMERA_CONFIG, sizeof(cam_config) }, 247 | .max_stills_w = state->width, 248 | .max_stills_h = state->height, 249 | .stills_yuv422 = 0, 250 | .one_shot_stills = 0, 251 | .max_preview_video_w = state->width, 252 | .max_preview_video_h = state->height, 253 | .num_preview_video_frames = 3, 254 | .stills_capture_circular_buffer_height = 0, 255 | .fast_preview_resume = 0, 256 | .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC 257 | }; 258 | mmal_port_parameter_set(camera->control, &cam_config.hdr); 259 | } 260 | // Set the encode format on the video port 261 | 262 | format = video_port->format; 263 | format->encoding_variant = MMAL_ENCODING_I420; 264 | format->encoding = MMAL_ENCODING_I420; 265 | format->es->video.width = state->width; 266 | format->es->video.height = state->height; 267 | format->es->video.crop.x = 0; 268 | format->es->video.crop.y = 0; 269 | format->es->video.crop.width = state->width; 270 | format->es->video.crop.height = state->height; 271 | format->es->video.frame_rate.num = state->framerate; 272 | format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN; 273 | 274 | status = mmal_port_format_commit(video_port); 275 | if (status) 276 | { 277 | vcos_log_error("camera video format couldn't be set"); 278 | goto error; 279 | } 280 | 281 | // PR : plug the callback to the video port 282 | status = mmal_port_enable(video_port, video_buffer_callback); 283 | if (status) 284 | { 285 | vcos_log_error("camera video callback2 error"); 286 | goto error; 287 | } 288 | 289 | // Ensure there are enough buffers to avoid dropping frames 290 | if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) 291 | video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; 292 | 293 | 294 | // Set the encode format on the still port 295 | format = still_port->format; 296 | format->encoding = MMAL_ENCODING_OPAQUE; 297 | format->encoding_variant = MMAL_ENCODING_I420; 298 | format->es->video.width = state->width; 299 | format->es->video.height = state->height; 300 | format->es->video.crop.x = 0; 301 | format->es->video.crop.y = 0; 302 | format->es->video.crop.width = state->width; 303 | format->es->video.crop.height = state->height; 304 | format->es->video.frame_rate.num = 1; 305 | format->es->video.frame_rate.den = 1; 306 | 307 | status = mmal_port_format_commit(still_port); 308 | if (status) 309 | { 310 | vcos_log_error("camera still format couldn't be set"); 311 | goto error; 312 | } 313 | 314 | 315 | //PR : create pool of message on video port 316 | MMAL_POOL_T *pool; 317 | video_port->buffer_size = video_port->buffer_size_recommended; 318 | video_port->buffer_num = video_port->buffer_num_recommended; 319 | pool = mmal_port_pool_create(video_port, video_port->buffer_num, video_port->buffer_size); 320 | if (!pool) 321 | { 322 | vcos_log_error("Failed to create buffer header pool for video output port"); 323 | } 324 | state->video_pool = pool; 325 | 326 | /* Ensure there are enough buffers to avoid dropping frames */ 327 | if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) 328 | still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; 329 | 330 | /* Enable component */ 331 | status = mmal_component_enable(camera); 332 | 333 | if (status) 334 | { 335 | vcos_log_error("camera component couldn't be enabled"); 336 | goto error; 337 | } 338 | 339 | raspicamcontrol_set_all_parameters(camera, &state->camera_parameters); 340 | 341 | state->camera_component = camera; 342 | 343 | return camera; 344 | 345 | error: 346 | 347 | if (camera) 348 | mmal_component_destroy(camera); 349 | 350 | return 0; 351 | } 352 | 353 | /** 354 | * Destroy the camera component 355 | * 356 | * @param state Pointer to state control struct 357 | * 358 | */ 359 | static void destroy_camera_component(RASPIVID_STATE *state) 360 | { 361 | if (state->camera_component) 362 | { 363 | mmal_component_destroy(state->camera_component); 364 | state->camera_component = NULL; 365 | } 366 | } 367 | 368 | 369 | /** 370 | * Destroy the encoder component 371 | * 372 | * @param state Pointer to state control struct 373 | * 374 | */ 375 | static void destroy_encoder_component(RASPIVID_STATE *state) 376 | { 377 | // Get rid of any port buffers first 378 | if (state->video_pool) 379 | { 380 | mmal_port_pool_destroy(state->encoder_component->output[0], state->video_pool); 381 | } 382 | 383 | if (state->encoder_component) 384 | { 385 | mmal_component_destroy(state->encoder_component); 386 | state->encoder_component = NULL; 387 | } 388 | } 389 | 390 | /** 391 | * Connect two specific ports together 392 | * 393 | * @param output_port Pointer the output port 394 | * @param input_port Pointer the input port 395 | * @param Pointer to a mmal connection pointer, reassigned if function successful 396 | * @return Returns a MMAL_STATUS_T giving result of operation 397 | * 398 | */ 399 | static MMAL_STATUS_T connect_ports(MMAL_PORT_T *output_port, MMAL_PORT_T *input_port, MMAL_CONNECTION_T **connection) 400 | { 401 | MMAL_STATUS_T status; 402 | 403 | status = mmal_connection_create(connection, output_port, input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT); 404 | 405 | if (status == MMAL_SUCCESS) 406 | { 407 | status = mmal_connection_enable(*connection); 408 | if (status != MMAL_SUCCESS) 409 | mmal_connection_destroy(*connection); 410 | } 411 | 412 | return status; 413 | } 414 | 415 | /** 416 | * Checks if specified port is valid and enabled, then disables it 417 | * 418 | * @param port Pointer the port 419 | * 420 | */ 421 | static void check_disable_port(MMAL_PORT_T *port) 422 | { 423 | if (port && port->is_enabled) 424 | mmal_port_disable(port); 425 | } 426 | 427 | RaspiCamCvCapture * raspiCamCvCreateCameraCapture(int index) 428 | { 429 | RaspiCamCvCapture * capture = (RaspiCamCvCapture*)malloc(sizeof(RaspiCamCvCapture)); 430 | // Our main data storage vessel.. 431 | RASPIVID_STATE * state = (RASPIVID_STATE*)malloc(sizeof(RASPIVID_STATE)); 432 | capture->pState = state; 433 | 434 | MMAL_STATUS_T status = -1; 435 | MMAL_PORT_T *camera_video_port = NULL; 436 | MMAL_PORT_T *camera_still_port = NULL; 437 | 438 | bcm_host_init(); 439 | 440 | // read default status 441 | default_status(state); 442 | 443 | int w = state->width; 444 | int h = state->height; 445 | state->py = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 1); // Y component of YUV I420 frame 446 | if (state->graymode==0) { 447 | state->pu = cvCreateImage(cvSize(w/2,h/2), IPL_DEPTH_8U, 1); // U component of YUV I420 frame 448 | state->pv = cvCreateImage(cvSize(w/2,h/2), IPL_DEPTH_8U, 1); // V component of YUV I420 frame 449 | } 450 | vcos_semaphore_create(&state->capture_sem, "Capture-Sem", 0); 451 | vcos_semaphore_create(&state->capture_done_sem, "Capture-Done-Sem", 0); 452 | 453 | if (state->graymode==0) { 454 | state->pu_big = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 1); 455 | state->pv_big = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 1); 456 | state->yuvImage = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 3); 457 | state->dstImage = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 3); // final picture to display 458 | } 459 | 460 | // create camera 461 | if (!create_camera_component(state)) 462 | { 463 | vcos_log_error("%s: Failed to create camera component", __func__); 464 | raspiCamCvReleaseCapture(&capture); 465 | return NULL; 466 | } 467 | 468 | camera_video_port = state->camera_component->output[MMAL_CAMERA_VIDEO_PORT]; 469 | camera_still_port = state->camera_component->output[MMAL_CAMERA_CAPTURE_PORT]; 470 | 471 | // assign data to use for callback 472 | camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *)state; 473 | 474 | // start capture 475 | if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) 476 | { 477 | vcos_log_error("%s: Failed to start capture", __func__); 478 | raspiCamCvReleaseCapture(&capture); 479 | return NULL; 480 | } 481 | 482 | // Send all the buffers to the video port 483 | 484 | int num = mmal_queue_length(state->video_pool->queue); 485 | int q; 486 | for (q = 0; q < num; q++) 487 | { 488 | MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state->video_pool->queue); 489 | 490 | if (!buffer) 491 | vcos_log_error("Unable to get a required buffer %d from pool queue", q); 492 | 493 | if (mmal_port_send_buffer(camera_video_port, buffer)!= MMAL_SUCCESS) 494 | vcos_log_error("Unable to send a buffer to encoder output port (%d)", q); 495 | } 496 | 497 | //mmal_status_to_int(status); 498 | 499 | // Disable all our ports that are not handled by connections 500 | //check_disable_port(camera_still_port); 501 | 502 | //if (status != 0) 503 | // raspicamcontrol_check_configuration(128); 504 | 505 | vcos_semaphore_wait(&state->capture_done_sem); 506 | return capture; 507 | } 508 | 509 | void raspiCamCvReleaseCapture(RaspiCamCvCapture ** capture) 510 | { 511 | RASPIVID_STATE * state = (*capture)->pState; 512 | 513 | // Unblock the the callback. 514 | state->finished = 1; 515 | vcos_semaphore_post(&state->capture_sem); 516 | vcos_semaphore_wait(&state->capture_done_sem); 517 | 518 | vcos_semaphore_delete(&state->capture_sem); 519 | vcos_semaphore_delete(&state->capture_done_sem); 520 | 521 | if (state->camera_component) 522 | mmal_component_disable(state->camera_component); 523 | 524 | destroy_camera_component(state); 525 | 526 | cvReleaseImage(&state->pu); 527 | if (state->graymode==0) { 528 | cvReleaseImage(&state->pv); 529 | cvReleaseImage(&state->py); 530 | } 531 | 532 | if (state->graymode==0) { 533 | cvReleaseImage(&state->pu_big); 534 | cvReleaseImage(&state->pv_big); 535 | cvReleaseImage(&state->yuvImage); 536 | cvReleaseImage(&state->dstImage); 537 | } 538 | 539 | free(state); 540 | free(*capture); 541 | *capture = 0; 542 | } 543 | 544 | void raspiCamCvSetCaptureProperty(RaspiCamCvCapture * capture, int property_id, double value) 545 | { 546 | } 547 | 548 | IplImage * raspiCamCvQueryFrame(RaspiCamCvCapture * capture) 549 | { 550 | RASPIVID_STATE * state = capture->pState; 551 | vcos_semaphore_post(&state->capture_sem); 552 | vcos_semaphore_wait(&state->capture_done_sem); 553 | 554 | if (state->graymode==0) 555 | { 556 | cvResize(state->pu, state->pu_big, CV_INTER_NN); 557 | cvResize(state->pv, state->pv_big, CV_INTER_NN); //CV_INTER_LINEAR looks better but it's slower 558 | cvMerge(state->py, state->pu_big, state->pv_big, NULL, state->yuvImage); 559 | 560 | cvCvtColor(state->yuvImage,state->dstImage,CV_YCrCb2RGB); // convert in RGB color space (slow) 561 | return state->dstImage; 562 | } 563 | return state->py; 564 | } 565 | -------------------------------------------------------------------------------- /RCJ_2014/wiringpi and opencv and raspicam compile command: -------------------------------------------------------------------------------- 1 | g++ "%f" -lopencv_highgui -lopencv_core -lopencv_legacy -lopencv_video -lopencv_features2d -lopencv_calib3d -lopencv_imgproc -lpthread -lm -L/home/pi/git/raspberrypi/userland/build/lib -lmmal_core -lmmal -l mmal_util -lvcos -lbcm_host -lX11 -lXext -lrt -lstdc++ -L/home/pi/git/robidouille/raspicam_cv -lraspicamcv -I/usr/include/opencv -o "%e" -L/usr/local/lib -lwiringPi 2 | 3 | How to use: 4 | - geany editor: paste it in the option box for build commands. 5 | - command line: change "%f" to the program output name and change "%e" to the C++ file name and path. 6 | For questions, please refer to the forum thread: 7 | http://www.raspberrypi.org/forums/viewtopic.php?f=37&t=82151 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | image-processing 2 | ================ 3 | 4 | soon more... 5 | --------------------------------------------------------------------------------