fixed vector size mismatch between matched and status vectors

camera_models/include/camodocal/gpl/EigenQuaternionParameterization.h

@@ -0,0 +1,40 @@
+#ifndef EIGENQUATERNIONPARAMETERIZATION_H
+#define EIGENQUATERNIONPARAMETERIZATION_H
+
+#include "ceres/local_parameterization.h"
+
+namespace camodocal
+{
+
+class EigenQuaternionParameterization : public ceres::LocalParameterization
+{
+public:
+    virtual ~EigenQuaternionParameterization() {}
+    virtual bool Plus(const double* x,
+                      const double* delta,
+                      double* x_plus_delta) const;
+    virtual bool ComputeJacobian(const double* x,
+                                 double* jacobian) const;
+    virtual int GlobalSize() const { return 4; }
+    virtual int LocalSize() const { return 3; }
+
+private:
+    template<typename T>
+    void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;
+};
+
+
+template<typename T>
+void
+EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const
+{
+    zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];
+    zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];
+    zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];
+    zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];
+}
+
+}
+
+#endif
+