Util.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "ar_track_alvar/Util.h"
00025 #include "ar_track_alvar/FileFormatUtils.h"
00026 
00027 using namespace std;
00028 
00029 namespace alvar {
00030 using namespace std;
00031 
00032 //ttesis
00033 
00034 int dot(CvPoint *A, CvPoint *B, CvPoint *C){
00035     CvPoint AB, BC; 
00036     AB.x = B->x-A->x;
00037         AB.y = B->y-A->y;
00038     BC.x = C->x-B->x;
00039     BC.y = C->y-B->y;
00040     int dot = AB.x * BC.x + AB.y * BC.y;
00041     return dot;
00042 }
00043 
00044 int cross(CvPoint *A,CvPoint *B, CvPoint *C){
00045         CvPoint AB, AC; 
00046         AB.x = B->x-A->x;
00047         AB.y = B->y-A->y;
00048         AC.x = C->x-A->x;
00049     AC.y = C->y-A->y;
00050     int cross = AB.x * AC.y - AB.y * AC.x;
00051     return cross;
00052 }
00053 
00054 double distance(CvPoint *A,CvPoint *B){
00055     double d1 = A->x - B->x;
00056         double d2 = A->y - B->y;
00057     return sqrt(d1*d1+d2*d2);
00058 }
00059 
00060 
00061 double linePointDist(CvPoint *A,CvPoint *B,CvPoint *C, bool isSegment){
00062         double dist = cross(A,B,C) / distance(A,B);
00063     if(isSegment){
00064         int dot1 = dot(A,B,C);
00065         if(dot1 > 0)return distance(B,C);
00066         int dot2 = dot(B,A,C);
00067         if(dot2 > 0)return distance(A,C);
00068     }
00069     return abs(dist);
00070 }
00071 
00072 double angle(CvPoint *A,CvPoint *B, CvPoint *C,CvPoint *D, int isDirectionDependent){
00073         double angle;
00074         double a = B->x  - A->x; 
00075         double b = B->y  - A->y;
00076         double c = D->x - C->x;
00077         double d = D->y - C->y;
00078         angle = acos( ((a * c) + (b * d) )/(sqrt(a*a + b*b) * sqrt(c*c + d*d)));
00079         if(isDirectionDependent){
00080                 return angle;
00081         }else{
00082                 if (angle > CV_PI/2){
00083                         return CV_PI - angle;
00084                 }else
00085                         return angle;
00086         }       
00087 }
00088 
00089 double polyLinePointDist(CvPoint *PointList, int nPnts,CvPoint *C, int *index, int isClosedPolygon){
00090 //      Calculate minimum distance of Point C to Polygon whose points are in list PointList
00091 //      if isClosedPolygon is true polygon is closed (segnment of the first and last point is also checked)
00092 //      index is the index of point A in pointlist, 
00093 //      where A is the starting point of the closest polygon segment
00094         *index = -1;
00095         double mindist= -1;
00096         double dist;
00097         for( int i=0; i < nPnts-1; i++){
00098                 dist=linePointDist(&PointList[i],&PointList[i+1],C,1);
00099                 if (mindist == -1 || dist<mindist){
00100                         mindist = dist;
00101                         *index = i;
00102                 }
00103         }
00104         if(isClosedPolygon){
00105                 dist=linePointDist(&PointList[nPnts-1],&PointList[0],C,1);
00106                 if (dist<mindist){
00107                         mindist = dist;
00108                         *index = nPnts-1;
00109                 }
00110         }
00111         return mindist;
00112 
00113 }
00114 
00115 //ttesis
00116 
00117 void FitCVEllipse(const vector<PointDouble> &points, CvBox2D& ellipse_box)
00118 {
00119         if(points.size() < 8) return;
00120 
00121         CvMat* vector = cvCreateMat(1, int(points.size()), CV_64FC2);
00122         for(size_t i = 0; i < points.size(); ++i)
00123         {
00124                 CV_MAT_ELEM(*vector, CvPoint2D64f, 0, i) = (CvPoint2D64f)points[i];
00125         }
00126         ellipse_box = cvFitEllipse2(vector);
00127         cvReleaseMat(&vector);
00128 }
00129 
00130 int exp_filt2(vector<double> &v, vector<double> &ret, bool clamp)
00131 {
00132         double y;
00133         int n = (int)v.size();
00134         
00135         double a = pow(0.01, 8.0/n);//0.8;
00136         double k = -log(a);
00137         
00138         // Forward
00139         vector<double> yp(n);
00140         
00141         y = 0;
00142         for(int i = 0; i < n; ++i)
00143                 y = a * y + v[i];
00144         
00145         y *= 1.0 / (1.0-pow(a,n));
00146 
00147         for(int i = 0; i < n; ++i)
00148         {
00149                 y = a * y + v[i];
00150                 yp[i] = y;
00151         }
00152 
00153         // Backward
00154         vector<double> ym(n);
00155         
00156         y = 0;
00157         for(int i = n-1; i >= 0; --i)
00158                 y = a * y + v[i];
00159         
00160         y *= 1.0 / (1.0-pow(a,n));
00161 
00162         for(int i = n-1; i >= 0; --i)
00163         {
00164                 y = a * y + v[i];
00165                 ym[i] = y;
00166         }
00167 
00168         // Filter
00169         ret.resize(n);
00170         for(int i = 0; i < n; ++i)
00171         {
00172                 ret[i] = (k/2.0) * (yp[i] + ym[i] - v[i]);
00173         }
00174         
00175         return int(ret.size());
00176 }
00177 
00178 int find_zero_crossings(const vector<double>& v, vector<int> &corners, int offs)
00179 {
00180         int ind = 0;
00181         int len = (int)v.size();
00182 
00183         int state;
00184         if(Sign(v.at(0)) == 1)
00185                 state = 1;
00186         else
00187                 state = 2;
00188         
00189         corners.clear();
00190         for(int i = 0; i < len+offs; ++i)
00191         {
00192                 if(i<len)
00193                         ind = i;
00194                 else
00195                         ind = i-len;
00196                 
00197                 int s = Sign(v.at(ind));
00198                 if(state == 1 && s == -1)
00199                         state = 2;
00200                 if(state == 2 && s == 1)
00201                 {
00202                         state = 1;
00203                         bool test = true;
00204                         for(unsigned j = 0; j < corners.size(); ++j)
00205                                 if(corners.at(j) == ind)
00206                                         test = false;
00207                                                 
00208                         if(test)        
00209                                 corners.push_back(ind);
00210                 }
00211         }
00212         
00213         return int(corners.size());
00214 }
00215 
00216 void out_matrix(const CvMat *m, const char *name) {
00217         if (m->cols == 1) {
00218                 std::cout<<name<<" = [";
00219                 for (int j=0; j<m->rows; j++) {
00220                         std::cout<<" "<<cvGet2D(m, j, 0).val[0];
00221                 }
00222                 std::cout<<"]^T"<<std::endl;
00223         } else if (m->rows == 1) {
00224                 std::cout<<name<<" = [";
00225                 for (int i=0; i<m->cols; i++) {
00226                         std::cout<<" "<<cvGet2D(m, 0, i).val[0];
00227                 }
00228                 std::cout<<"]^T"<<std::endl;
00229         } else {
00230                 std::cout<<name<<" = ["<<std::endl;
00231                 for (int j=0; j<m->rows; j++) {
00232                         for (int i=0; i<m->cols; i++) {
00233                                 std::cout<<" "<<cvGet2D(m, j, i).val[0];
00234                         }
00235                         std::cout<<std::endl;
00236                 }
00237                 std::cout<<"]"<<std::endl;
00238         }
00239 }
00240 
00241 double Limit(double val, double min_val, double max_val) {
00242         return max(min_val, min(max_val, val));
00243 }
00244 
00245 Index::Index(int a) { val.push_back(a); }
00246 
00247 Index::Index(int a, int b) {
00248         val.push_back(a); 
00249         val.push_back(b); 
00250 }
00251 
00252 Index::Index(int a, int b, int c) { 
00253         val.push_back(a); 
00254         val.push_back(b); 
00255         val.push_back(c); 
00256 }
00257 
00258 bool Index::operator<(const Index &index) const {
00259         int comp=0;
00260         size_t d=0;
00261         // Go through the dimensions (last being the most significant)
00262         while ((d < val.size()) || (d < index.val.size())) {
00263                 int v0 = (d < val.size() ? val[d] : 0);
00264                 int v1 = (d < index.val.size() ? index.val[d] : 0);
00265                 if (v0<v1) comp=-1;
00266                 else if (v1<v0) comp=1;
00267                 d++;
00268         }
00269         if (comp == -1) return true;
00270         return false;
00271 }
00272 
00273 int Histogram::DimIndex(int dim, double val) {
00274         int binsize = (dim < int(dim_binsize.size()) ? dim_binsize[dim] : 1);
00275         if (val >= 0) return int(val+(binsize/2))/binsize;
00276         return int(val-(binsize/2))/binsize;
00277 }
00278 double Histogram::DimVal(int dim, int index) {
00279         int binsize = (dim < int(dim_binsize.size()) ? dim_binsize[dim] : 1);
00280         return (index * binsize);
00281 }
00282 void Histogram::AddDimension(int binsize) {
00283         dim_binsize.push_back(binsize);
00284 }
00285 void Histogram::Clear() {
00286         bins.clear();
00287 }
00288 void Histogram::Inc(double dim0, double dim1, double dim2) {
00289         Index index(DimIndex(0,dim0), DimIndex(1,dim1), DimIndex(2,dim2));
00290         if (bins.find(index) != bins.end()) {
00291                 bins[index]++;
00292         } else {
00293                 bins[index] = 1;
00294         }
00295 }
00296 int Histogram::GetMax(double *dim0, double *dim1, double *dim2) {
00297         map<Index, int>::const_iterator iter, max_iter;
00298         int max=0;
00299         for (max_iter = iter = bins.begin(); iter != bins.end(); iter++) {
00300                 if (iter->second > max) {
00301                         max = iter->second;
00302                         max_iter = iter;
00303                 }
00304         }
00305         if (max > 0) {
00306                 *dim0 = DimVal(0, max_iter->first.val[0]);
00307                 if (dim1) *dim1 = DimVal(1, max_iter->first.val[1]);
00308                 if (dim2) *dim2 = DimVal(2, max_iter->first.val[2]);
00309         }
00310         return max;
00311 }
00312 void HistogramSubpixel::Clear() {
00313         bins.clear();
00314         acc_dim0.clear();
00315         acc_dim1.clear();
00316         acc_dim2.clear();
00317 }
00318 void HistogramSubpixel::Inc(double dim0, double dim1, double dim2) {
00319         Index index(DimIndex(0,dim0), DimIndex(1,dim1), DimIndex(2,dim2));
00320         if (bins.find(index) != bins.end()) {
00321                 bins[index]++;
00322                 acc_dim0[index] += dim0;
00323                 acc_dim1[index] += dim1;
00324                 acc_dim2[index] += dim2;
00325         } else {
00326                 bins[index] = 1;
00327                 acc_dim0[index] = dim0;
00328                 acc_dim1[index] = dim1;
00329                 acc_dim2[index] = dim2;
00330         }
00331 }
00332 int HistogramSubpixel::GetMax(double *dim0, double *dim1, double *dim2) {
00333         map<Index, int>::const_iterator iter;
00334         int max=0;
00335         int divider=0;
00336         for (iter = bins.begin(); iter != bins.end(); iter++) {
00337                 if (iter->second > max) {
00338                         divider = max = iter->second;
00339                         *dim0 = acc_dim0[iter->first];
00340                         if (dim1) *dim1 = acc_dim1[iter->first];
00341                         if (dim2) *dim2 = acc_dim2[iter->first];
00342                 } else if (iter->second == max) {
00343                         divider += iter->second;
00344                         *dim0 += acc_dim0[iter->first];
00345                         if (dim1) *dim1 += acc_dim1[iter->first];
00346                         if (dim2) *dim2 += acc_dim2[iter->first];
00347                 }
00348         }
00349         if (max > 0) {
00350                 *dim0 /= divider;
00351                 if (dim1) *dim1 /= divider;
00352                 if (dim2) *dim2 /= divider;
00353         }
00354         return max;
00355 }
00356 
00357 struct SerializationFormatterXml {
00358         TiXmlDocument document;
00359         TiXmlElement *xml_current;
00360         SerializationFormatterXml() : xml_current(0) {}
00361 };
00362 
00363 bool Serialization::Output() {
00364         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00365         if (filename.size() > 0) {
00366                 //xml->document.LinkEndChild(new TiXmlDeclaration("1.0", "UTF-8", "no"));
00367                 xml->document.InsertBeforeChild (xml->document.RootElement(), TiXmlDeclaration("1.0", "UTF-8", "no"));
00368                 xml->document.SaveFile(filename.c_str());
00369         } else {
00370                 const TiXmlNode *node = (xml->xml_current ? xml->xml_current : xml->document.RootElement());
00371                 std::basic_ostream<char> *os = dynamic_cast<std::basic_ostream<char> *>(stream);
00372                 (*os)<<(*node);
00373                 //(*stream)<<(*node);
00374         }
00375         return true;
00376 }
00377 
00378 bool Serialization::Input() {
00379         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00380         if (filename.size() > 0) {
00381                 xml->document.LoadFile(filename.c_str());
00382         } else {
00383                 // TODO: How this should be handled with nested classes?
00384                 TiXmlNode *node = (xml->xml_current ? xml->xml_current : xml->document.RootElement());
00385                 if (node == 0) {
00386                         node = (TiXmlElement*)xml->document.LinkEndChild(new TiXmlElement("root"));
00387                 }
00388                 std::basic_istream<char> *is = dynamic_cast<std::basic_istream<char> *>(stream);
00389                 (*is)>>(*node);
00390                 //(*stream)>>(*node);
00391         }
00392         return true;
00393 }
00394 
00395 bool Serialization::Descend(const char *id) {
00396         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00397         if (input) {
00398                 if (xml->xml_current == 0) {
00399                         xml->xml_current = xml->document.RootElement();
00400                         if (!xml->xml_current || (strcmp(xml->xml_current->Value(), id) != 0)) {
00401                                 return false;
00402                         }
00403                 } else {
00404                         xml->xml_current = (TiXmlElement*)xml->xml_current->FirstChild (id);
00405                         if (xml->xml_current == NULL) return false;
00406                 }
00407         } else {
00408                 if (xml->xml_current == 0) {
00409                         xml->xml_current = (TiXmlElement*)xml->document.LinkEndChild(new TiXmlElement(id));
00410                 } else {
00411                         xml->xml_current = (TiXmlElement*)xml->xml_current->LinkEndChild(new TiXmlElement(id));
00412                 }
00413         }
00414         return true;
00415 }
00416 bool Serialization::Ascend() {
00417         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00418         xml->xml_current = (TiXmlElement*)xml->xml_current->Parent();
00419         return true;
00420 }
00421 
00422 Serialization::Serialization(std::string _filename) {
00423         SerializationFormatterXml *xml = new SerializationFormatterXml();
00424         formatter_handle = xml;
00425         filename = _filename;
00426         input = false; // by default output
00427 }
00428 
00429 Serialization::Serialization(std::basic_iostream<char> &_stream)
00430 {
00431         SerializationFormatterXml *xml = new SerializationFormatterXml();
00432         formatter_handle = xml;
00433         stream = &_stream;
00434 }
00435 
00436 Serialization::Serialization(std::basic_istream<char> &_stream) {
00437         SerializationFormatterXml *xml = new SerializationFormatterXml();
00438         formatter_handle = xml;
00439         stream = &_stream;
00440 }
00441 
00442 Serialization::Serialization(std::basic_ostream<char> &_stream) {
00443         SerializationFormatterXml *xml = new SerializationFormatterXml();
00444         formatter_handle = xml;
00445         stream = &_stream;
00446 }
00447 
00448 Serialization::~Serialization() {
00449         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00450         delete xml;
00451 }
00452 
00453 bool Serialization::Serialize(int &data, const std::string &name) {
00454         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00455         if (!xml || !xml->xml_current) return false;
00456         bool ret = true;
00457         if (input) ret = (xml->xml_current->QueryIntAttribute(name, &data) == TIXML_SUCCESS);
00458         else xml->xml_current->SetAttribute(name, data);
00459         return ret;
00460 }
00461 
00462 bool Serialization::Serialize(unsigned short &data, const std::string &name) {
00463         int i = data;
00464         bool ret = Serialize(i, name);
00465         data = i;
00466         return ret;
00467 }
00468 
00469 bool Serialization::Serialize(unsigned long &data, const std::string &name) {
00470         // TODO: Possible overflow here...
00471         int i = data;
00472         bool ret = Serialize(i, name);
00473         data = i;
00474         return ret;
00475 }
00476 
00477 bool Serialization::Serialize(double &data, const std::string &name) {
00478         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00479         bool ret = true;
00480         if (input) ret = (xml->xml_current->QueryDoubleAttribute(name, &data) == TIXML_SUCCESS);
00481         else xml->xml_current->SetDoubleAttribute(name.c_str(), data);
00482         return ret;
00483 }
00484 
00485 bool Serialization::Serialize(std::string &data, const std::string &name) {
00486         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00487         bool ret = true;
00488         if (input) {
00489                 const char *tmp = xml->xml_current->Attribute(name.c_str());
00490                 if (tmp == NULL) ret=false;
00491                 else data = tmp;
00492         }
00493         else xml->xml_current->SetAttribute(name.c_str(), data.c_str());
00494         return ret;
00495 }
00496 
00497 bool Serialization::Serialize(CvMat &data, const std::string &name) {
00498         SerializationFormatterXml *xml = (SerializationFormatterXml *)formatter_handle;
00499         bool ret = true;
00500         if (input) {
00501                 TiXmlElement *xml_matrix = (TiXmlElement*)xml->xml_current->FirstChild(name);
00502                 if (xml_matrix == NULL) return false;
00503                 if (!FileFormatUtils::parseXMLMatrix(xml_matrix, &data)) return false;
00504         }
00505         else {
00506                 xml->xml_current->LinkEndChild(FileFormatUtils::createXMLMatrix(name.c_str(), &data));
00507         }
00508         return ret;
00509 }
00510 
00511 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02