00001 /** 00002 * @file AutoShutter.cpp 00003 * 00004 * Implementation of class AutoShutter 00005 * 00006 * @author <A href=mailto:jochen@kerdels.de>Jochen Kerdels</A> 00007 */ 00008 00009 #include "AutoShutter.h" 00010 #include "Representations/Perception/CameraParameters.h" 00011 #include "Representations/Perception/CameraInfo.h" 00012 00013 00014 00015 AutoShutter::AutoShutter(const SpecialVisionInterfaces& interfaces) 00016 : SpecialVision(interfaces) 00017 { 00018 reset(); 00019 } 00020 00021 void AutoShutter::reset() 00022 { 00023 state = 0; 00024 for (int i = 0; i < 256; ++i) 00025 for (int j = 0; j < 9; ++j) 00026 histograms[i][j] = 0.0; 00027 } 00028 00029 void AutoShutter::execute() 00030 { 00031 switch (state) { 00032 case 0: { 00033 CameraParameters tmp; 00034 tmp.setGain( CameraParameters::gain_low); 00035 tmp.setShutterSpeed(CameraParameters::shutter_slow); 00036 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00037 sensors.setCameraParameters(tmp); 00038 } break; 00039 case 2: { 00040 CameraParameters tmp; 00041 tmp.setGain( CameraParameters::gain_mid); 00042 tmp.setShutterSpeed(CameraParameters::shutter_slow); 00043 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00044 sensors.setCameraParameters(tmp); 00045 } break; 00046 case 4: { 00047 CameraParameters tmp; 00048 tmp.setGain( CameraParameters::gain_high); 00049 tmp.setShutterSpeed(CameraParameters::shutter_slow); 00050 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00051 sensors.setCameraParameters(tmp); 00052 } break; 00053 case 6: { 00054 CameraParameters tmp; 00055 tmp.setGain( CameraParameters::gain_low); 00056 tmp.setShutterSpeed(CameraParameters::shutter_mid); 00057 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00058 sensors.setCameraParameters(tmp); 00059 } break; 00060 case 8: { 00061 CameraParameters tmp; 00062 tmp.setGain( CameraParameters::gain_mid); 00063 tmp.setShutterSpeed(CameraParameters::shutter_mid); 00064 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00065 sensors.setCameraParameters(tmp); 00066 } break; 00067 case 10: { 00068 CameraParameters tmp; 00069 tmp.setGain( CameraParameters::gain_high); 00070 tmp.setShutterSpeed(CameraParameters::shutter_mid); 00071 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00072 sensors.setCameraParameters(tmp); 00073 } break; 00074 case 12: { 00075 CameraParameters tmp; 00076 tmp.setGain( CameraParameters::gain_low); 00077 tmp.setShutterSpeed(CameraParameters::shutter_fast); 00078 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00079 sensors.setCameraParameters(tmp); 00080 } break; 00081 case 14: { 00082 CameraParameters tmp; 00083 tmp.setGain( CameraParameters::gain_mid); 00084 tmp.setShutterSpeed(CameraParameters::shutter_fast); 00085 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00086 sensors.setCameraParameters(tmp); 00087 } break; 00088 case 16: { 00089 CameraParameters tmp; 00090 tmp.setGain( CameraParameters::gain_high); 00091 tmp.setShutterSpeed(CameraParameters::shutter_fast); 00092 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00093 sensors.setCameraParameters(tmp); 00094 } break; 00095 case 1: 00096 case 3: 00097 case 5: 00098 case 7: 00099 case 9: 00100 case 11: 00101 case 13: 00102 case 15: 00103 case 17: makeHistogram(state / 2); break; 00104 case 18: selectBest(); break; 00105 } 00106 if (state < 18) 00107 { 00108 state++; 00109 specialPercept.type = SpecialPercept::autoShutter; 00110 specialPercept.shutterSelected = false; 00111 } 00112 else 00113 { 00114 specialPercept.type = SpecialPercept::autoShutter; 00115 specialPercept.shutterSelected = true; 00116 reset(); 00117 } 00118 } 00119 00120 void AutoShutter::makeHistogram(int which) 00121 { 00122 const double sampleCount = 2500; 00123 int i; 00124 for (i = 0; i < sampleCount; ++i) 00125 ++histograms[image.image[rand()%cameraResolutionHeight_ERS7][0][rand()%cameraResolutionWidth_ERS7]][which]; 00126 histograms[0][which] /= sampleCount; 00127 for (i = 1; i < 256; ++i) 00128 histograms[i][which] = histograms[i-1][which] + (histograms[i][which] / sampleCount); 00129 } 00130 00131 void AutoShutter::selectBest() 00132 { 00133 double a; 00134 double min = 256; 00135 int minid = -1; 00136 for (int i = 0; i < 9; ++i) { 00137 a = 0; 00138 for (int j = 0; j < 256; ++j) { 00139 a += fabs(histograms[j][i] - (double(j) / 255.0)); 00140 } 00141 if (a < min) 00142 { 00143 min = a; 00144 minid = i; 00145 } 00146 } 00147 00148 if (minid >= 0) 00149 switch (minid) 00150 { 00151 case 0: { 00152 CameraParameters tmp; 00153 tmp.setGain( CameraParameters::gain_low); 00154 tmp.setShutterSpeed(CameraParameters::shutter_slow); 00155 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00156 sensors.setCameraParameters(tmp); 00157 } break; 00158 case 1: { 00159 CameraParameters tmp; 00160 tmp.setGain( CameraParameters::gain_mid); 00161 tmp.setShutterSpeed(CameraParameters::shutter_slow); 00162 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00163 sensors.setCameraParameters(tmp); 00164 } break; 00165 case 2: { 00166 CameraParameters tmp; 00167 tmp.setGain( CameraParameters::gain_high); 00168 tmp.setShutterSpeed(CameraParameters::shutter_slow); 00169 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00170 sensors.setCameraParameters(tmp); 00171 } break; 00172 case 3: { 00173 CameraParameters tmp; 00174 tmp.setGain( CameraParameters::gain_low); 00175 tmp.setShutterSpeed(CameraParameters::shutter_mid); 00176 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00177 sensors.setCameraParameters(tmp); 00178 } break; 00179 case 4: { 00180 CameraParameters tmp; 00181 tmp.setGain( CameraParameters::gain_mid); 00182 tmp.setShutterSpeed(CameraParameters::shutter_mid); 00183 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00184 sensors.setCameraParameters(tmp); 00185 } break; 00186 case 5: { 00187 CameraParameters tmp; 00188 tmp.setGain( CameraParameters::gain_high); 00189 tmp.setShutterSpeed(CameraParameters::shutter_mid); 00190 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00191 sensors.setCameraParameters(tmp); 00192 } break; 00193 case 6: { 00194 CameraParameters tmp; 00195 tmp.setGain( CameraParameters::gain_low); 00196 tmp.setShutterSpeed(CameraParameters::shutter_fast); 00197 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00198 sensors.setCameraParameters(tmp); 00199 } break; 00200 case 7: { 00201 CameraParameters tmp; 00202 tmp.setGain( CameraParameters::gain_mid); 00203 tmp.setShutterSpeed(CameraParameters::shutter_fast); 00204 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00205 sensors.setCameraParameters(tmp); 00206 } break; 00207 case 8: { 00208 CameraParameters tmp; 00209 tmp.setGain( CameraParameters::gain_high); 00210 tmp.setShutterSpeed(CameraParameters::shutter_fast); 00211 tmp.setWhiteBalance(CameraParameters::wb_indoor_mode); 00212 sensors.setCameraParameters(tmp); 00213 } break; 00214 } 00215 }
1.3.6