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 <cmath>
00027 #include <sys/time.h>
00028 #include <models/shape/ht_lines.h>
00029 #include <utils/math/angle.h>
00030
00031 using namespace std;
00032 using namespace fawkes;
00033
00034 namespace firevision {
00035 #if 0
00036 }
00037 #endif
00038
00039 #define TEST_IF_IS_A_PIXEL(x) ((x)>230)
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 HtLinesModel::HtLinesModel(unsigned int nr_candidates, float angle_from, float angle_range, int r_scale, float min_votes_ratio, int min_votes)
00054 {
00055 RHT_NR_CANDIDATES = nr_candidates;
00056
00057 RHT_R_SCALE = r_scale;
00058
00059 RHT_MIN_VOTES = min_votes;
00060 RHT_MIN_VOTES_RATIO = min_votes_ratio;
00061
00062 RHT_ANGLE_FROM = angle_from - (floor(angle_from / (2 * M_PI )) * (2 * M_PI));
00063 RHT_ANGLE_RANGE = angle_range - (floor(angle_range / (2 * M_PI )) * (2 * M_PI));
00064 RHT_ANGLE_INCREMENT = RHT_ANGLE_RANGE / RHT_NR_CANDIDATES;
00065 }
00066
00067
00068
00069 HtLinesModel::~HtLinesModel(void)
00070 {
00071 m_Lines.clear();
00072 }
00073
00074 int
00075 HtLinesModel::parseImage( unsigned char *buf,
00076 ROI *roi )
00077 {
00078 unsigned char *buffer = roi->get_roi_buffer_start(buf);
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 for (y = 0; y < roi->height; ++y) {
00093 for (x = 0; x < roi->width; ++x) {
00094 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00095 point_t pt={x, y};
00096 pixels.push_back(pt);
00097 }
00098
00099 ++buffer;
00100 }
00101 line_start += roi->line_step;
00102 buffer = line_start;
00103 }
00104
00105
00106 point_t p;
00107 float r, phi;
00108 vector< point_t >::iterator pos;
00109 if (pixels.size() == 0) {
00110
00111 return 0;
00112 }
00113
00114
00115 while (pixels.size() > 0) {
00116 p = pixels.back();
00117 pixels.pop_back();
00118
00119 for (unsigned int i = 0; i < RHT_NR_CANDIDATES; ++i) {
00120 phi = RHT_ANGLE_FROM + i * RHT_ANGLE_INCREMENT;
00121 r = p.x * cos( phi ) + p.y * sin( phi );
00122
00123 int angle = (int)round(fawkes::rad2deg( phi ));
00124
00125 accumulator.accumulate( (int)round(r / RHT_R_SCALE),
00126 angle,
00127 0 );
00128 }
00129 }
00130
00131
00132
00133 int max, r_max, phi_max, any_max;
00134 max = accumulator.getMax(r_max, phi_max, any_max);
00135
00136 roi_width = roi->width;
00137 roi_height = roi->height;
00138
00139 LineShape l(roi->width, roi->height);
00140 l.r = r_max * RHT_R_SCALE;
00141 l.phi = phi_max;
00142 l.count = max;
00143 m_Lines.push_back( l );
00144
00145 return 1;
00146 }
00147
00148
00149 int
00150 HtLinesModel::getShapeCount(void) const
00151 {
00152 return m_Lines.size();
00153 }
00154
00155 LineShape *
00156 HtLinesModel::getShape(int id) const
00157 {
00158 if (id < 0 || (unsigned int)id >= m_Lines.size()) {
00159 return NULL;
00160 } else {
00161 return const_cast<LineShape*>(&m_Lines[id]);
00162 }
00163 }
00164
00165
00166 LineShape *
00167 HtLinesModel::getMostLikelyShape(void) const
00168 {
00169 if (m_Lines.size() == 0) {
00170 return NULL;
00171 } else if (m_Lines.size() == 1) {
00172 return const_cast<LineShape*>(&m_Lines[0]);
00173 } else {
00174 int cur=0;
00175 for (unsigned int i=1; i < m_Lines.size(); ++i) {
00176 if (m_Lines[i].count > m_Lines[cur].count) {
00177 cur = i;
00178 }
00179 }
00180 return const_cast<LineShape*>(&m_Lines[cur]);
00181 }
00182 }
00183
00184
00185
00186
00187
00188 vector< LineShape > *
00189 HtLinesModel::getShapes()
00190 {
00191 int votes = (int)(accumulator.getNumVotes() * (float)RHT_MIN_VOTES_RATIO);
00192
00193 if ( RHT_MIN_VOTES > votes ) {
00194 votes = RHT_MIN_VOTES;
00195 }
00196
00197 vector< LineShape > * rv = new vector< LineShape >();
00198
00199 vector< vector< int > > *rht_nodes = accumulator.getNodes( votes );
00200 vector< vector< int > >::iterator node_it;
00201
00202 LineShape l(roi_width, roi_height);
00203
00204 for (node_it = rht_nodes->begin(); node_it != rht_nodes->end(); ++node_it) {
00205 l.r = node_it->at(0) * RHT_R_SCALE;
00206 l.phi = node_it->at(1);
00207
00208 l.count = node_it->at(3);
00209 l.calcPoints();
00210 rv->push_back( l );
00211 }
00212
00213 return rv;
00214 }
00215
00216 }