00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <utils/math/angle.h>
00027 #include <sys/time.h>
00028 #include <models/shape/rht_lines.h>
00029
00030 using namespace std;
00031 using namespace fawkes;
00032
00033 #define TEST_IF_IS_A_PIXEL(x) ((x)>230)
00034
00035 namespace firevision {
00036 #if 0
00037 }
00038 #endif
00039
00040
00041
00042
00043
00044
00045 RhtLinesModel::RhtLinesModel(float max_time, int max_iter, unsigned int nr_candidates, float angle_from, float angle_range, int r_scale, float min_votes_ratio, int min_votes)
00046 {
00047 RHT_MAX_TIME = max_time;
00048 RHT_MAX_ITER = max_iter;
00049
00050 RHT_NR_CANDIDATES = nr_candidates;
00051
00052 RHT_R_SCALE = r_scale;
00053
00054 RHT_MIN_VOTES = min_votes;
00055 RHT_MIN_VOTES_RATIO = min_votes_ratio;
00056
00057 RHT_ANGLE_FROM = angle_from - (floor(angle_from / (2 * M_PI )) * (2 * M_PI));
00058 RHT_ANGLE_RANGE = angle_range - (floor(angle_range / (2 * M_PI )) * (2 * M_PI));
00059 RHT_ANGLE_INCREMENT = RHT_ANGLE_RANGE / RHT_NR_CANDIDATES;
00060 }
00061
00062
00063
00064 RhtLinesModel::~RhtLinesModel(void)
00065 {
00066 m_Lines.clear();
00067 }
00068
00069
00070
00071
00072 int
00073 RhtLinesModel::parseImage( unsigned char *buf,
00074 ROI *roi )
00075 {
00076 unsigned char *buffer = roi->get_roi_buffer_start(buf);
00077
00078 struct timeval start, now;
00079
00080
00081 accumulator.reset();
00082
00083
00084 m_Lines.clear();
00085
00086
00087
00088 unsigned char *line_start = buffer;
00089 unsigned int x, y;
00090 vector<point_t> pixels;
00091
00092 gettimeofday(&start, NULL);
00093
00094 for (y = 0; y < roi->height; ++y) {
00095 for (x = 0; x < roi->width; ++x) {
00096 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00097 point_t pt={x, y};
00098 pixels.push_back(pt);
00099 }
00100
00101 ++buffer;
00102 }
00103 line_start += roi->line_step;
00104 buffer = line_start;
00105 }
00106
00107
00108 point_t p;
00109 float r, phi;
00110 vector< point_t >::iterator pos;
00111 int num_iter = 0;
00112 if (pixels.size() == 0) {
00113
00114 return 0;
00115 }
00116
00117
00118
00119 do {
00120
00121 if (pixels.size() > 0) {
00122 int ri = rand() % pixels.size();
00123 pos = pixels.begin() + ri;
00124 p = *pos;
00125 pixels.erase(pos);
00126
00127 for (unsigned int i = 0; i < RHT_NR_CANDIDATES; ++i) {
00128 phi = RHT_ANGLE_FROM + i * RHT_ANGLE_INCREMENT;
00129 r = p.x * cos( phi ) + p.y * sin( phi );
00130
00131 int angle = (int)round(fawkes::rad2deg( phi ));
00132
00133 accumulator.accumulate( (int)round(r / RHT_R_SCALE),
00134 angle,
00135 0 );
00136 }
00137
00138 gettimeofday(&now, NULL);
00139
00140 diff_sec = now.tv_sec - start.tv_sec;
00141 diff_usec = now.tv_usec - start.tv_usec;
00142 if (diff_usec < 0) {
00143 diff_sec -= 1;
00144 diff_usec += 1000000;
00145 }
00146
00147 f_diff_sec = diff_sec + diff_usec / 1000000.f;
00148
00149 }
00150 } while( ( ++num_iter < RHT_MAX_ITER) &&
00151 ( f_diff_sec < RHT_MAX_TIME) );
00152
00153
00154 int max, r_max, phi_max, any_max;
00155 max = accumulator.getMax(r_max, phi_max, any_max);
00156
00157 roi_width = roi->width;
00158 roi_height = roi->height;
00159
00160 LineShape l(roi->width, roi->height);
00161 l.r = r_max * RHT_R_SCALE;
00162 l.phi = phi_max;
00163 l.count = max;
00164 m_Lines.push_back( l );
00165
00166 return 1;
00167 }
00168
00169
00170 int
00171 RhtLinesModel::getShapeCount(void) const
00172 {
00173 return m_Lines.size();
00174 }
00175
00176 LineShape *
00177 RhtLinesModel::getShape(int id) const
00178 {
00179 if (id < 0 || (unsigned int)id >= m_Lines.size()) {
00180 return NULL;
00181 } else {
00182 return const_cast<LineShape*>(&m_Lines[id]);
00183 }
00184 }
00185
00186
00187 LineShape *
00188 RhtLinesModel::getMostLikelyShape(void) const
00189 {
00190 if (m_Lines.size() == 0) {
00191 return NULL;
00192 } else if (m_Lines.size() == 1) {
00193 return const_cast<LineShape*>(&m_Lines[0]);
00194 } else {
00195 int cur=0;
00196 for (unsigned int i=1; i < m_Lines.size(); ++i) {
00197 if (m_Lines[i].count > m_Lines[cur].count) {
00198 cur = i;
00199 }
00200 }
00201 return const_cast<LineShape*>(&m_Lines[cur]);
00202 }
00203 }
00204
00205
00206
00207
00208
00209 vector< LineShape > *
00210 RhtLinesModel::getShapes()
00211 {
00212 int votes = (int)(accumulator.getNumVotes() * (float)RHT_MIN_VOTES_RATIO);
00213
00214 if ( RHT_MIN_VOTES > votes ) {
00215 votes = RHT_MIN_VOTES;
00216 }
00217
00218 vector< LineShape > * rv = new vector< LineShape >();
00219
00220 vector< vector< int > > *rht_nodes = accumulator.getNodes( votes );
00221 vector< vector< int > >::iterator node_it;
00222
00223 LineShape l(roi_width, roi_height);
00224
00225 for (node_it = rht_nodes->begin(); node_it != rht_nodes->end(); ++node_it) {
00226 l.r = node_it->at(0) * RHT_R_SCALE;
00227 l.phi = node_it->at(1);
00228
00229 l.count = node_it->at(3);
00230 l.calcPoints();
00231 rv->push_back( l );
00232 }
00233
00234 return rv;
00235 }
00236
00237 }