fixed vector size mismatch between matched and status vectors

camera_models/include/camodocal/gpl/gpl.h

@@ -0,0 +1,111 @@
+#ifndef GPL_H
+#define GPL_H
+
+#include <algorithm>
+#include <cmath>
+#include <opencv2/core/core.hpp>
+
+namespace camodocal
+{
+
+template<class T>
+const T clamp(const T& v, const T& a, const T& b)
+{
+	return std::min(b, std::max(a, v));
+}
+
+double hypot3(double x, double y, double z);
+float hypot3f(float x, float y, float z);
+
+template<class T>
+const T normalizeTheta(const T& theta)
+{
+	T normTheta = theta;
+
+	while (normTheta < - M_PI)
+	{
+		normTheta += 2.0 * M_PI;
+	}
+	while (normTheta > M_PI)
+	{
+		normTheta -= 2.0 * M_PI;
+	}
+
+	return normTheta;
+}
+
+double d2r(double deg);
+float d2r(float deg);
+double r2d(double rad);
+float r2d(float rad);
+
+double sinc(double theta);
+
+template<class T>
+const T square(const T& x)
+{
+	return x * x;
+}
+
+template<class T>
+const T cube(const T& x)
+{
+	return x * x * x;
+}
+
+template<class T>
+const T random(const T& a, const T& b)
+{
+	return static_cast<double>(rand()) / RAND_MAX * (b - a) + a;
+}
+
+template<class T>
+const T randomNormal(const T& sigma)
+{
+    T x1, x2, w;
+
+    do
+    {
+        x1 = 2.0 * random(0.0, 1.0) - 1.0;
+        x2 = 2.0 * random(0.0, 1.0) - 1.0;
+        w = x1 * x1 + x2 * x2;
+    }
+    while (w >= 1.0 || w == 0.0);
+
+    w = sqrt((-2.0 * log(w)) / w);
+
+    return x1 * w * sigma;
+}
+
+unsigned long long timeInMicroseconds(void);
+
+double timeInSeconds(void);
+
+void colorDepthImage(cv::Mat& imgDepth,
+                     cv::Mat& imgColoredDepth,
+                     float minRange, float maxRange);
+
+bool colormap(const std::string& name, unsigned char idx,
+              float& r, float& g, float& b);
+
+std::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1);
+std::vector<cv::Point2i> bresCircle(int x0, int y0, int r);
+
+void fitCircle(const std::vector<cv::Point2d>& points,
+               double& centerX, double& centerY, double& radius);
+
+std::vector<cv::Point2d> intersectCircles(double x1, double y1, double r1,
+                                          double x2, double y2, double r2);
+
+void LLtoUTM(double latitude, double longitude,
+             double& utmNorthing, double& utmEasting,
+             std::string& utmZone);
+void UTMtoLL(double utmNorthing, double utmEasting,
+             const std::string& utmZone,
+             double& latitude, double& longitude);
+
+long int timestampDiff(uint64_t t1, uint64_t t2);
+
+}
+
+#endif