fixed vector size mismatch between matched and status vectors

camera_models/include/camodocal/chessboard/Chessboard.h

@@ -0,0 +1,86 @@
+#ifndef CHESSBOARD_H
+#define CHESSBOARD_H
+
+#include <boost/shared_ptr.hpp>
+#include <opencv2/core/core.hpp>
+
+namespace camodocal
+{
+
+// forward declarations
+class ChessboardCorner;
+typedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;
+class ChessboardQuad;
+typedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;
+
+class Chessboard
+{
+public:
+    Chessboard(cv::Size boardSize, cv::Mat& image);
+
+    void findCorners(bool useOpenCV = false);
+    const std::vector<cv::Point2f>& getCorners(void) const;
+    bool cornersFound(void) const;
+
+    const cv::Mat& getImage(void) const;
+    const cv::Mat& getSketch(void) const;
+
+private:
+    bool findChessboardCorners(const cv::Mat& image,
+                               const cv::Size& patternSize,
+                               std::vector<cv::Point2f>& corners,
+                               int flags, bool useOpenCV);
+
+    bool findChessboardCornersImproved(const cv::Mat& image,
+                                       const cv::Size& patternSize,
+                                       std::vector<cv::Point2f>& corners,
+                                       int flags);
+
+    void cleanFoundConnectedQuads(std::vector<ChessboardQuadPtr>& quadGroup, cv::Size patternSize);
+
+    void findConnectedQuads(std::vector<ChessboardQuadPtr>& quads,
+                            std::vector<ChessboardQuadPtr>& group,
+                            int group_idx, int dilation);
+
+//    int checkQuadGroup(std::vector<ChessboardQuadPtr>& quadGroup,
+//                       std::vector<ChessboardCornerPtr>& outCorners,
+//                       cv::Size patternSize);
+
+    void labelQuadGroup(std::vector<ChessboardQuadPtr>& quad_group,
+                        cv::Size patternSize, bool firstRun);
+
+    void findQuadNeighbors(std::vector<ChessboardQuadPtr>& quads, int dilation);
+
+    int augmentBestRun(std::vector<ChessboardQuadPtr>& candidateQuads, int candidateDilation,
+                       std::vector<ChessboardQuadPtr>& existingQuads, int existingDilation);
+
+    void generateQuads(std::vector<ChessboardQuadPtr>& quads,
+                       cv::Mat& image, int flags,
+                       int dilation, bool firstRun);
+
+    bool checkQuadGroup(std::vector<ChessboardQuadPtr>& quads,
+                        std::vector<ChessboardCornerPtr>& corners,
+                        cv::Size patternSize);
+
+    void getQuadrangleHypotheses(const std::vector< std::vector<cv::Point> >& contours,
+                                 std::vector< std::pair<float, int> >& quads,
+                                 int classId) const;
+
+    bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const;
+
+    bool checkBoardMonotony(std::vector<ChessboardCornerPtr>& corners,
+                            cv::Size patternSize);
+
+    bool matchCorners(ChessboardQuadPtr& quad1, int corner1,
+                      ChessboardQuadPtr& quad2, int corner2) const;
+
+    cv::Mat mImage;
+    cv::Mat mSketch;
+    std::vector<cv::Point2f> mCorners;
+    cv::Size mBoardSize;
+    bool mCornersFound;
+};
+
+}
+
+#endif

camera_models/include/camodocal/chessboard/ChessboardCorner.h

@@ -0,0 +1,45 @@
+#ifndef CHESSBOARDCORNER_H
+#define CHESSBOARDCORNER_H
+
+#include <boost/shared_ptr.hpp>
+#include <opencv2/core/core.hpp>
+
+namespace camodocal
+{
+
+class ChessboardCorner;
+typedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;
+
+class ChessboardCorner
+{
+public:
+    ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}
+
+    float meanDist(int &n) const
+    {
+        float sum = 0;
+        n = 0;
+        for (int i = 0; i < 4; ++i)
+        {
+            if (neighbors[i].get())
+            {
+                float dx = neighbors[i]->pt.x - pt.x;
+                float dy = neighbors[i]->pt.y - pt.y;
+                sum += sqrt(dx*dx + dy*dy);
+                n++;
+            }
+        }
+        return sum / std::max(n, 1);
+    }
+
+    cv::Point2f pt;                     // X and y coordinates
+    int row;                            // Row and column of the corner
+    int column;                         // in the found pattern
+    bool needsNeighbor;                 // Does the corner require a neighbor?
+    int count;                          // number of corner neighbors
+    ChessboardCornerPtr neighbors[4];   // pointer to all corner neighbors
+};
+
+}
+
+#endif

camera_models/include/camodocal/chessboard/ChessboardQuad.h

@@ -0,0 +1,29 @@
+#ifndef CHESSBOARDQUAD_H
+#define CHESSBOARDQUAD_H
+
+#include <boost/shared_ptr.hpp>
+
+#include "camodocal/chessboard/ChessboardCorner.h"
+
+namespace camodocal
+{
+
+class ChessboardQuad;
+typedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;
+
+class ChessboardQuad
+{
+public:
+    ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}
+
+    int count;                         // Number of quad neighbors
+    int group_idx;                     // Quad group ID
+    float edge_len;                    // Smallest side length^2
+    ChessboardCornerPtr corners[4];    // Coordinates of quad corners
+    ChessboardQuadPtr neighbors[4];    // Pointers of quad neighbors
+    bool labeled;                      // Has this corner been labeled?
+};
+
+}
+
+#endif

camera_models/include/camodocal/chessboard/Spline.h

@@ -0,0 +1,319 @@
+/*  dynamo:- Event driven molecular dynamics simulator
+    http://www.marcusbannerman.co.uk/dynamo
+    Copyright (C) 2011  Marcus N Campbell Bannerman <m.bannerman@gmail.com>
+
+    This program is free software: you can redistribute it and/or
+    modify it under the terms of the GNU General Public License
+    version 3 as published by the Free Software Foundation.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/vector_proxy.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <boost/numeric/ublas/triangular.hpp>
+#include <boost/numeric/ublas/lu.hpp>
+#include <exception>
+
+namespace ublas = boost::numeric::ublas;
+
+class Spline : private std::vector<std::pair<double, double> >
+{
+public:
+  //The boundary conditions available
+  enum BC_type {
+	FIXED_1ST_DERIV_BC,
+	FIXED_2ND_DERIV_BC,
+	PARABOLIC_RUNOUT_BC
+  };
+
+  enum Spline_type {
+	LINEAR,
+	CUBIC
+  };
+
+  //Constructor takes the boundary conditions as arguments, this
+  //sets the first derivative (gradient) at the lower and upper
+  //end points
+  Spline():
+	_valid(false),
+	_BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC),
+	_BCLowVal(0), _BCHighVal(0),
+	_type(CUBIC)
+  {}
+
+  typedef std::vector<std::pair<double, double> > base;
+  typedef base::const_iterator const_iterator;
+
+  //Standard STL read-only container stuff
+  const_iterator begin() const { return base::begin(); }
+  const_iterator end() const { return base::end(); }
+  void clear() { _valid = false; base::clear(); _data.clear(); }
+  size_t size() const { return base::size(); }
+  size_t max_size() const { return base::max_size(); }
+  size_t capacity() const { return base::capacity(); }
+  bool empty() const { return base::empty(); }
+
+  //Add a point to the spline, and invalidate it so its
+  //recalculated on the next access
+  inline void addPoint(double x, double y)
+  {
+	_valid = false;
+	base::push_back(std::pair<double, double>(x,y));
+  }
+
+  //Reset the boundary conditions
+  inline void setLowBC(BC_type BC, double val = 0)
+  { _BCLow = BC; _BCLowVal = val; _valid = false; }
+
+  inline void setHighBC(BC_type BC, double val = 0)
+  { _BCHigh = BC; _BCHighVal = val; _valid = false; }
+
+  void setType(Spline_type type) { _type = type; _valid = false; }
+
+  //Check if the spline has been calculated, then generate the
+  //spline interpolated value
+  double operator()(double xval)
+  {
+	if (!_valid) generate();
+
+	//Special cases when we're outside the range of the spline points
+	if (xval <= x(0)) return lowCalc(xval);
+	if (xval >= x(size()-1)) return highCalc(xval);
+
+	//Check all intervals except the last one
+	for (std::vector<SplineData>::const_iterator iPtr = _data.begin();
+		 iPtr != _data.end()-1; ++iPtr)
+		if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x))
+		  return splineCalc(iPtr, xval);
+
+	return splineCalc(_data.end() - 1, xval);
+  }
+
+private:
+
+  ///////PRIVATE DATA MEMBERS
+  struct SplineData { double x,a,b,c,d; };
+  //vector of calculated spline data
+  std::vector<SplineData> _data;
+  //Second derivative at each point
+  ublas::vector<double> _ddy;
+  //Tracks whether the spline parameters have been calculated for
+  //the current set of points
+  bool _valid;
+  //The boundary conditions
+  BC_type _BCLow, _BCHigh;
+  //The values of the boundary conditions
+  double _BCLowVal, _BCHighVal;
+
+  Spline_type _type;
+
+  ///////PRIVATE FUNCTIONS
+  //Function to calculate the value of a given spline at a point xval
+  inline double splineCalc(std::vector<SplineData>::const_iterator i, double xval)
+  {
+	const double lx = xval - i->x;
+	return ((i->a * lx + i->b) * lx + i->c) * lx + i->d;
+  }
+
+  inline double lowCalc(double xval)
+  {
+	const double lx = xval - x(0);
+
+	if (_type == LINEAR)
+	  return lx * _BCHighVal + y(0);
+
+	const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;
+
+	switch(_BCLow)
+	  {
+	  case FIXED_1ST_DERIV_BC:
+		return lx * _BCLowVal + y(0);
+	  case FIXED_2ND_DERIV_BC:
+		  return lx * lx * _BCLowVal + firstDeriv * lx + y(0);
+	  case PARABOLIC_RUNOUT_BC:
+		return lx * lx * _ddy[0] + lx * firstDeriv  + y(0);
+	  }
+	throw std::runtime_error("Unknown BC");
+  }
+
+  inline double highCalc(double xval)
+  {
+	const double lx = xval - x(size() - 1);
+
+	if (_type == LINEAR)
+	  return lx * _BCHighVal + y(size() - 1);
+
+	const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2);
+
+	switch(_BCHigh)
+	  {
+	  case FIXED_1ST_DERIV_BC:
+		return lx * _BCHighVal + y(size() - 1);
+	  case FIXED_2ND_DERIV_BC:
+		return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);
+	  case PARABOLIC_RUNOUT_BC:
+		return lx * lx * _ddy[size()-1] + lx * firstDeriv  + y(size() - 1);
+	  }
+	throw std::runtime_error("Unknown BC");
+  }
+
+  //These just provide access to the point data in a clean way
+  inline double x(size_t i) const { return operator[](i).first; }
+  inline double y(size_t i) const { return operator[](i).second; }
+  inline double h(size_t i) const { return x(i+1) - x(i); }
+
+  //Invert a arbitrary matrix using the boost ublas library
+  template<class T>
+  bool InvertMatrix(ublas::matrix<T> A,
+		ublas::matrix<T>& inverse)
+  {
+	using namespace ublas;
+
+	// create a permutation matrix for the LU-factorization
+	permutation_matrix<std::size_t> pm(A.size1());
+
+	// perform LU-factorization
+	int res = lu_factorize(A,pm);
+		if( res != 0 ) return false;
+
+	// create identity matrix of "inverse"
+	inverse.assign(ublas::identity_matrix<T>(A.size1()));
+
+	// backsubstitute to get the inverse
+	lu_substitute(A, pm, inverse);
+
+	return true;
+  }
+
+  //This function will recalculate the spline parameters and store
+  //them in _data, ready for spline interpolation
+  void generate()
+  {
+	if (size() < 2)
+	  throw std::runtime_error("Spline requires at least 2 points");
+
+	//If any spline points are at the same x location, we have to
+	//just slightly seperate them
+	{
+	  bool testPassed(false);
+	  while (!testPassed)
+		{
+		  testPassed = true;
+		  std::sort(base::begin(), base::end());
+
+		  for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr)
+		if (iPtr->first == (iPtr+1)->first)
+		  {
+			if ((iPtr+1)->first != 0)
+			  (iPtr+1)->first += (iPtr+1)->first
+			* std::numeric_limits<double>::epsilon() * 10;
+			else
+			  (iPtr+1)->first = std::numeric_limits<double>::epsilon() * 10;
+			testPassed = false;
+			break;
+		  }
+		}
+	}
+
+	const size_t e = size() - 1;
+
+	switch (_type)
+	  {
+	  case LINEAR:
+		{
+		  _data.resize(e);
+		  for (size_t i(0); i < e; ++i)
+		{
+		  _data[i].x = x(i);
+		  _data[i].a = 0;
+		  _data[i].b = 0;
+		  _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i));
+		  _data[i].d = y(i);
+		}
+		  break;
+		}
+	  case CUBIC:
+		{
+		  ublas::matrix<double> A(size(), size());
+		  for (size_t yv(0); yv <= e; ++yv)
+		for (size_t xv(0); xv <= e; ++xv)
+		  A(xv,yv) = 0;
+
+		  for (size_t i(1); i < e; ++i)
+		{
+		  A(i-1,i) = h(i-1);
+		  A(i,i) = 2 * (h(i-1) + h(i));
+		  A(i+1,i) = h(i);
+		}
+
+		  ublas::vector<double> C(size());
+		  for (size_t xv(0); xv <= e; ++xv)
+		C(xv) = 0;
+
+		  for (size_t i(1); i < e; ++i)
+		C(i) = 6 *
+		  ((y(i+1) - y(i)) / h(i)
+		   - (y(i) - y(i-1)) / h(i-1));
+
+		  //Boundary conditions
+		  switch(_BCLow)
+		{
+		case FIXED_1ST_DERIV_BC:
+		  C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);
+		  A(0,0) = 2 * h(0);
+		  A(1,0) = h(0);
+		  break;
+		case FIXED_2ND_DERIV_BC:
+		  C(0) = _BCLowVal;
+		  A(0,0) = 1;
+		  break;
+		case PARABOLIC_RUNOUT_BC:
+		  C(0) = 0; A(0,0) = 1; A(1,0) = -1;
+		  break;
+		}
+
+		  switch(_BCHigh)
+		{
+		case FIXED_1ST_DERIV_BC:
+		  C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1));
+		  A(e,e) = 2 * h(e - 1);
+		  A(e-1,e) = h(e - 1);
+		  break;
+		case FIXED_2ND_DERIV_BC:
+		  C(e) = _BCHighVal;
+		  A(e,e) = 1;
+		  break;
+		case PARABOLIC_RUNOUT_BC:
+		  C(e) = 0; A(e,e) = 1; A(e-1,e) = -1;
+		  break;
+		}
+
+		  ublas::matrix<double> AInv(size(), size());
+		  InvertMatrix(A,AInv);
+
+		  _ddy = ublas::prod(C, AInv);
+
+		  _data.resize(size()-1);
+		  for (size_t i(0); i < e; ++i)
+		{
+		  _data[i].x = x(i);
+		  _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i));
+		  _data[i].b = _ddy(i) / 2;
+		  _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3;
+		  _data[i].d = y(i);
+		}
+		}
+	  }
+	_valid = true;
+  }
+};