Line.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/Line.h"
00025 
00026 using namespace std;
00027 
00028 namespace alvar {
00029 using namespace std;
00030 
00031 Line::Line(float params[4])
00032 {
00033         c.x = params[2];
00034         c.y = params[3];
00035         s.x = params[0];
00036         s.y = params[1];
00037 }
00038 
00039 void FitLines(vector<Line>& lines)
00040 {
00041 
00042 }
00043 
00044 int FitLines(vector<Line> &lines,
00045                         const vector<int>& corners,
00046                         const vector<PointInt >& edge,
00047                         IplImage *grey /*=0*/) // grey image for future sub pixel accuracy
00048 {
00049         lines.clear();
00050         for(unsigned j = 0; j < corners.size(); ++j)
00051         {
00052                 int start, end, first;
00053                 int size = (int)edge.size();
00054 
00055                 first = corners[0];
00056                 start = corners[j];
00057 
00058                 if(j < corners.size()-1)
00059                         end  = corners[j+1];
00060                 else
00061                         end = first;
00062 
00063                 int len = 0;
00064 
00065                 if(start < end)
00066                         len = end-start+1;
00067                 else
00068                         len = size-start+end+1;
00069 
00070                 int ind;
00071                 double* data = new double[2*len];
00072 
00073                 // OpenCV routine... 
00074                 CvMat* line_data = cvCreateMat(1, len, CV_32FC2);
00075                 for(int i = 0; i < len; ++i)
00076                 {
00077                         ind = i + start;
00078                         if(ind >= size)
00079                         ind = ind-size;
00080 
00081                         double px = double(edge[ind].x);
00082                         double py = double(edge[ind].y);
00083                         CV_MAT_ELEM(*line_data, CvPoint2D32f, 0, i) = cvPoint2D32f(px, py);
00084                 }
00085 
00086                 float params[4] = {0};
00087                 cvFitLine(line_data, CV_DIST_L2, 0, 0.01, 0.01, params);
00088                 lines.push_back(Line(params));
00089 
00090                 delete [] data;
00091                 cvReleaseMat(&line_data);
00092 
00093         }
00094 
00095         return lines.size();
00096 }
00097 
00098 PointDouble Intersection(const Line& l1, const Line& l2)
00099 {
00100 
00101         double vx = l1.s.x;
00102         double vy = l1.s.y;
00103         double ux = l2.s.x;
00104         double uy = l2.s.y;
00105         double wx = l2.c.x-l1.c.x;
00106         double wy = l2.c.y-l1.c.y;
00107 
00108         double s, px, py;
00109         double tmp = vx*uy-vy*ux;
00110         if(tmp==0) tmp = 1;
00111 
00112         //if(/*tmp <= 1.f && tmp >= -1.f && */tmp != 0.f && ang > 0.1)
00113         {
00114                 s = (vy*wx-vx*wy) / (tmp);
00115                 px = l2.c.x+s*ux;
00116                 py = l2.c.y+s*uy;
00117         }
00118 
00119         return PointDouble(px, py);
00120 }
00121 
00122 } // namespace alvar


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