text: /*
  May 2015
  
            Authors:
  Ankit Dhall     Yash Chandak
  
  
  
  TO DO:
  -set capture frame size in VideoCapture object
  -take lines only closer to the region of the vanishing points
      - plug the prevRes in the eqns for new frame and consider lines only with error < |epsilon|
  -cluster more if more than 2 vanishing points are present (advanced)
  -refactor the CODE
  -error calculation, remove so many divisions!
  
  */
  #include <iostream>
  #include <armadillo>
  #include <string>
  #include "opencv2/core/core.hpp"
  #include "opencv2/core/utility.hpp"
  #include "opencv2/imgproc/imgproc.hpp"
  #include "opencv2/imgcodecs.hpp"
  #include "opencv2/highgui/highgui.hpp"
  #include <vector>
  #include <fstream>
  #include <math.h>
  #include <ctime>
  
  //add OpenCV and Armadillo namespaces
  using namespace cv;
  using namespace std;
  using namespace arma;
  
  class vanishingPt
  {
      public:
      cv::Mat image, img, gray;
      cv::Mat frame;
      vector< vector<int> > points;
      mat A,b, prevRes;
      mat Atemp, btemp, res, aug, error, soln;
      //ofstream out1, out2;
      float epsilon;
  
      //store slope (m) and y-intercept (c) of each lines
      float m,c;
  
      //store minimum length for lines to be considered while estimating vanishing point
      int minlength;
  
      //temporary vector for intermediate storage
      vector<int> temp;
  
      //store (x1, y1) and (x2, y2) endpoints for each line segment
      vector<cv::Vec4i> lines_std;
  
      //video capture object from OpenCV
      cv::VideoCapture cap;
  
      //to store intermediate errors
      double temperr;
  
      //constructor to set video/webcam and find vanishing point
      vanishingPt()
      {
          cv::namedWindow("win", 2);
          cv::namedWindow("Lines", 2);
  
          // to calculate fps
          clock_t begin, end;
  
          //read from video file on disk
          //to read from webcam initialize as: cap = VideoCapture(int device_id);
          cap = VideoCapture(0);
          //cap = VideoCapture("road.m4v");
  
          if( cap.isOpened() )//check if camera/ video stream is available
          {
              //get first frame to intialize the values
              cap.read(frame);
              image= cv::Mat(cv::Size(frame.rows,frame.cols), CV_8UC1, 0.0);
          }
  
          // define minimum length requirement for any line
          minlength = image.cols * image.cols * 0.001 ;
          minlength /= 10. ; //added for debugging ...must be removed later
          int flag=0;
          while( cap.isOpened() )//check if camera/ video stream is available
          {
              if ( ! cap.grab() )
                  continue;
  
              if(!cap.retrieve(img))
                  continue;
  
              //it's advised not to modify image stored in the buffer structure of the opencv.
              frame  = img.clone();
  
              //to calculate fps
              begin = clock();
  
              cv::cvtColor(frame,image , cv::COLOR_BGR2GRAY);
  
              //resize frame to 480x320
              cv::resize(image, image, cv::Size(480,320));
  
              //equalize histogram
              cv::equalizeHist(image, image);
  
              //initialize the line segment matrix in format y = m*x + c
              init(image, prevRes);
  
              //draw lines on image and display
              makeLines(flag);
  
              //approximate vanishing point
              eval();
  
              //to calculate fps
              end = clock();
  
              //hit 'esc' to exit program
              int k = cv::waitKey(1);
              cout<<"fps: "<<1/(double(end-begin)/CLOCKS_PER_SEC)<<";k="<<k<<endl;
  
              if ( k==27 )
                  break;
          }
  
      }
      void init(cv::Mat image, mat prevRes)
      {
          //create OpenCV object for line segment detection
          cv::Ptr<cv::LineSegmentDetector> ls = cv::createLineSegmentDetector(cv::LSD_REFINE_STD);
  
          //initialize
          lines_std.clear();
  
          //detect lines in image and store in linse_std
          //store (x1, y1) and (x2, y2) endpoints for each line segment
          ls->detect(image, lines_std);
  
          // Show found lines
          cv::Mat drawnLines (image);
  
          for(int i=0; i<(int)lines_std.size(); i++)
          {
  
              //ignore if almost vertical
              if ( abs(lines_std[i][0]-lines_std[i][2]) < 10 || abs(lines_std[i][1]-lines_std[i][3]) < 10) //check if almost vertical
                  continue;
              //ignore shorter lines (x1-x2)^2 + (y2-y1)^2 < minlength
              if( ((lines_std[i][0]-lines_std[i][2])*(lines_std[i][0]-lines_std[i][2]) +(lines_std[i][1]-lines_std[i][3])*(lines_std[i][1]-lines_std[i][3])) < minlength)
                  continue;
  
              //store valid lines' endpoints for calculations
              for(int j=0; j<4; j++)
              {
                  temp.push_back(lines_std[i][j]);
              }
  
              points.push_back(temp);
              temp.clear();
          }
          ls->drawSegments(drawnLines, lines_std);
          cv::imshow("Lines", drawnLines);
          cout<<"Detected:"<<lines_std.size()<<endl;
          cout<<"Filtered:"<<points.size()<<endl;
      }
      void makeLines(int flag)
      {
          // to solve Ax = b for x
          int points_size = (int) points.size();
          A = zeros<mat>(points_size, 2);
          b = zeros<mat>(points_size, 1);
  
          //convert given end-points of line segment into a*x + b*y = c format for calculations
          //do for each line segment detected
          for(int i=0; i<points_size; i++)
          {
  
              A(i,0)=-(points[i][3]-points[i][1]);            //-(y2-y1)
              A(i,1)=(points[i][2]-points[i][0]);             //x2-x1
              b(i,0)=A(i,0)*points[i][0]+A(i,1)*points[i][1]; //-(y2-y1)*x1 + (x2-x1)*y1
          }
      }
  
      //estimate the vanishing point
      void eval()
      {
          //stores the estimated co-ordinates of the vanishing point with respect to the image
          soln= zeros<mat>(2,1);
  
          //initialize error
          double err = 9999999999;
  
          //calculate point of intersection of every pair of lines and
          //find the sum of distance from all other lines
          //select the point which has the minimum sum of distance
          int points_size = (int) points.size();
          for(int i=0; i<points_size; i++)
          {
              for(int j=0; j<points_size; j++)
              {
                  if(i >= j)
                  continue;
  
                  //armadillo vector
                  uvec indices;
  
                  //store indices of lines to be used for calculation
                  indices << i << j;
  
                  //extract the rows with indices specified in uvec indices
                  //stores the ith and jth row of matrices A and b into Atemp and btemp respectively
                  //hence creating a 2x2 matrix for calculating point of intersection
                  Atemp = A.rows(indices);
                  btemp = b.rows(indices);
  
                  //if lines are parallel then skip
                  if(arma::rank(Atemp) != 2)
                      continue;
  
                  //solves for 'x' in A*x = b
                  res = calc(Atemp, btemp);
  
  
                  if(res.n_rows == 0 || res.n_cols == 0)
                      continue;
  
                  // calculate error assuming perfect intersection is
                  error = A*res - b;
  
                  //reduce size of error
                  error = error/1000;
  
                  // to store intermediate error values
                  temperr = 0;
                  //summation of errors
                  for(int i=0; i<(int)error.n_rows ; i++)
                      temperr+=(error(i,0)*error(i,0))/1000;
  
                  //scale errors to prevent any overflows
                  temperr/=1000000;
  
                  //if current error is smaller than previous min error then update the solution (point)
                  if(err > temperr)
                  {
                      soln = res;
                      err = temperr;
                  }
              }
          }
  
          //cout<<"\n\nResult:\n"<<soln(0,0)<<","<<soln(1,0)<<"\nError:"<<err<<"\n\n";
  
          // draw a circle to visualize the approximate vanishing point
          if(soln(0,0) > 0 && soln(0,0) < image.cols && soln(1,0) > 0 && soln(1,0) < image.rows)
              cv::circle(image, Point(soln(0,0), soln(1,0)), 25, cv::Scalar(0,0,255), 10);
          cv::imshow("win", image);
  
          //flush the vector
          points.clear();
  
          //toDo: use previous frame's result to reduce calculations and stabilize the region of vanishing point
          prevRes = soln;
      }
  
      //function to calculate and return the intersection point
      mat calc(mat A, mat b)
      {
          mat x = zeros<mat>(2,1);
          solve(x,A,b);
          return x;
      }
  };
  
  
  int main()
  {
      // make object
      vanishingPt obj;
      cv::destroyAllWindows();
      return 0;
  }
  