From 98696ff270ca176430ad6f114f01120dc3f8e7fc Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Mon, 22 Jan 2018 15:58:14 +0100
Subject: [PATCH] wip gtest trifocal from projection matrices

---
 src/CMakeLists.txt                  |   3 +-
 src/common_class/rotations.h        | 501 ++++++++++++++++++++++++++++
 src/common_class/trifocaltensor.cpp |  13 +
 src/common_class/trifocaltensor.h   |   2 +
 src/test/gtest_trifocaltensor.cpp   | 100 ++++++
 src/vision_utils.cpp                |   1 -
 src/vision_utils.h                  |  87 ++++-
 7 files changed, 704 insertions(+), 3 deletions(-)
 create mode 100644 src/common_class/rotations.h

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e564d33..6d93eae 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -103,7 +103,8 @@ SET(headers_main
 SET(headers_com
 	common_class/buffer.h
 	common_class/frame.h
-	common_class/trifocaltensor.h)
+	common_class/trifocaltensor.h
+	common_class/rotations.h)
 SET(headers_sen	
 	sensors/sensor_factory.h 
 	sensors/sensor_base.h)
diff --git a/src/common_class/rotations.h b/src/common_class/rotations.h
new file mode 100644
index 0000000..d22a678
--- /dev/null
+++ b/src/common_class/rotations.h
@@ -0,0 +1,501 @@
+#ifndef ROTATIONS_H_
+#define ROTATIONS_H_
+
+#include "vision_utils.h"
+
+#include "iostream"
+
+namespace vision_utils
+{
+
+//////////////////////////////////////////////////////////////
+
+/** \brief Return angle between -pi and pi
+ *
+ * @param angle
+ * @return formatted angle
+ */
+template<typename T>
+inline T pi2pi(const T angle)
+{
+    using std::fmod;
+
+    return (angle > (T)0 ?
+            fmod(angle + (T)M_PI, (T)(2*M_PI)) - (T)M_PI :
+            fmod(angle - (T)M_PI, (T)(2*M_PI)) + (T)M_PI);
+}
+
+/** \brief Conversion to radians
+ *
+ * @param deg angle in degrees
+ * @return angle in radians
+ */
+template<typename T>
+inline T toRad(const T deg)
+{
+    return (T)M_TORAD * deg;
+}
+
+/** \brief Conversion to degrees
+ *
+ * @param rad angle in radians
+ * @return angle in degrees
+ */
+template<typename T>
+inline T toDeg(const T rad)
+{
+    return (T)M_TODEG * rad;
+}
+
+//////////////////////////////////////////////////////////////////
+// Operators skew and vee
+
+/** \brief Skew symmetric matrix
+ *
+ * @param _v a 3D vector
+ * @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBase<Derived>& _v)
+{
+    MatrixSizeCheck<3,1>::check(_v);
+
+    typedef typename Derived::Scalar T;
+
+    Eigen::Matrix<T, 3, 3> sk;
+
+    sk << (T)0.0 , -_v(2), +_v(1),
+           +_v(2), (T)0.0, -_v(0),
+           -_v(1), +_v(0), (T)0.0;
+
+    return sk;
+}
+
+/** \brief Inverse of skew symmetric matrix
+ *
+ * @param _m A skew-symmetric matrix
+ * @return a 3-vector v such that skew(v) = _m
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase<Derived>& _m)
+{
+    MatrixSizeCheck<3,3>::check(_m);
+
+    typedef typename Derived::Scalar T;
+
+    return (Eigen::Matrix<T, 3, 1>() << _m(2, 1), _m(0, 2), _m(1, 0)).finished();
+}
+
+///////////////////////////////////////////////////////////////
+// Rotation conversions - exp and log maps
+
+/** \brief Quaternion exponential map
+ *
+ * @param _v a rotation vector with _v.norm() the rotated angle and _v.normalized() the rotation axis.
+ * @return the right-handed unit quaternion performing the rotation encoded by _v
+ */
+template<typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase<Derived>& _v)
+{
+    using std::sqrt;
+    using std::cos;
+    using std::sin;
+
+    MatrixSizeCheck<3,1>::check(_v);
+
+    typedef typename Derived::Scalar T;
+
+    T angle_squared = _v.squaredNorm();
+    T angle         = sqrt(angle_squared);
+    T angle_half    = angle / (T)2.0;
+
+    Eigen::Quaternion<T> q;
+    if (angle > (T)(EPS))
+    {
+        q.w()   = cos(angle_half);
+        q.vec() = sin(angle_half) * _v.normalized();// / angle;
+    }
+    else
+    {
+        q.w()   = (T)1.0 - angle_squared/(T)2; // Taylor expansion of cos(x) = 1 - x^2/2!;
+        q.vec() = _v * ((T)2.0 - angle_squared / (T)48.0); // Taylor series of sinc(x) ~ 1 - x^2/3!, and have q.vec = v/2 * sinc(angle_half)
+    }
+    return q.normalized();
+}
+
+/** \brief Quaternion logarithmic map
+ *
+ * @param _q a unit right-handed quaternion
+ * @return a rotation vector v such that _q = exp_q(v)
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_q(const Eigen::QuaternionBase<Derived>& _q)
+{
+    using std::sqrt;
+
+    typedef typename Derived::Scalar T;
+
+    Eigen::Matrix<T, 3, 1> vec = _q.vec();
+    T vecnorm_squared = vec.squaredNorm();
+    T vecnorm = sqrt(vecnorm_squared); // vec.norm();
+    if (vecnorm > (T)EPS_SMALL)
+    { // regular angle-axis conversion
+        T angle = (T)2.0 * atan2(vecnorm, _q.w());
+        return vec * angle / vecnorm;
+    }
+    else
+    { // small-angle approximation using truncated Taylor series
+        T r2 = vecnorm_squared / (_q.w() *_q.w());
+        return vec * ( (T)2.0 -  r2 / (T)1.5 ) / _q.w(); // log = 2 * vec * ( 1 - norm(vec)^2 / 3*w^2 ) / w.
+    }
+}
+
+/** \brief Rotation matrix exponential map
+ *
+ * @param _v a rotation vector
+ * @return the rotation matrix performing the rotation encoded by _v
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBase<Derived>& _v)
+{
+    using std::sqrt;
+
+    MatrixSizeCheck<3, 1>::check(_v);
+
+    typedef typename Derived::Scalar T;
+
+    Eigen::Matrix<typename Derived::Scalar, 3, 3> R;
+
+    T angle_squared = _v.squaredNorm();
+    T angle = sqrt(angle_squared);
+
+    if (angle > EPS)
+        R = Eigen::AngleAxis<T>(angle, _v.normalized()).toRotationMatrix();
+    else
+        R = Eigen::Matrix<T, 3, 3>::Identity() + skew(_v);
+
+    return R;
+}
+
+/** \brief Rotation matrix logarithmic map
+ *
+ * @param _R a 3D rotation matrix
+ * @return the rotation vector v such that _R = exp_R(v)
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBase<Derived>& _R)
+{
+    MatrixSizeCheck<3, 3>::check(_R);
+
+    typedef typename Derived::Scalar T;
+
+    Eigen::AngleAxis<T> aa = Eigen::AngleAxis<T>(_R);
+    return aa.axis() * aa.angle();
+}
+
+/** \brief Rotation vector to quaternion conversion
+ *
+ * @param _v a rotation vector
+ * @return the equivalent right-handed unit quaternion
+ */
+template<typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v)
+{
+    return exp_q(_v);
+}
+
+/** \brief Quaternion to rotation vector conversion
+ *
+ * @param _q a right-handed unit quaternion
+ * @return the equivalent rotation vector
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> q2v(const Eigen::QuaternionBase<Derived>& _q)
+{
+    return log_q(_q);
+}
+
+/** \brief Rotation vector to rotation matrix conversion
+ *
+ * @param _v a rotation vector
+ * @return the equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> v2R(const Eigen::MatrixBase<Derived>& _v)
+{
+    return exp_R(_v);
+}
+
+/** \brief Rotation matrix to rotation vector conversion
+ *
+ * @param _R a rotation matrix
+ * @return the equivalent rotation vector
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> R2v(const Eigen::MatrixBase<Derived>& _R)
+{
+    return log_R(_R);
+}
+
+/** \brief quaternion to rotation matrix conversion
+ *
+ * @param _q a right-handed unit quaternion
+ * @return the equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::QuaternionBase<Derived>& _q)
+{
+    return _q.matrix();
+}
+
+/** \brief quaternion to rotation matrix conversion
+ *
+ * @param _q a right-handed unit quaternion
+ * @return the equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase<Derived>& _q)
+{
+    MatrixSizeCheck<4,1>::check(_q);
+    Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
+    return q2R( q );
+}
+
+/** \brief rotation matrix to quaternion conversion
+ *
+ * @param _R a rotation matrix
+ * @return the equivalent right-handed unit quaternion
+ */
+template<typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R)
+{
+    MatrixSizeCheck<3,3>::check(_R);
+
+    return Eigen::Quaternion<typename Derived::Scalar>(_R);
+}
+
+/////////////////////////////////////////////////////////////////
+// Jacobians of SO(3)
+
+/** \brief Compute Jr (Right Jacobian)
+ * Right Jacobian for exp map in SO(3) - equation (10.86) and following equations in
+ *  G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ *
+ *      expmap( theta + d_theta ) \approx expmap(theta) * expmap(Jr * d_theta)
+ *
+ *  where Jr = jac_SO3_right(theta);
+ *
+ *  This maps a perturbation in the tangent space (d_theta) to a perturbation on the manifold (expmap(Jr * d_theta))
+ *  so that:
+ *
+ *      exp(theta+d_theta) = exp(theta)*exp(Jr(theta)*d_theta)
+ */
+
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
+{
+    using std::sqrt;
+    using std::cos;
+    using std::sin;
+
+    MatrixSizeCheck<3, 1>::check(_theta);
+
+    typedef typename Derived::Scalar T;
+
+    T theta2 = _theta.squaredNorm();
+    Eigen::Matrix<T, 3, 3> W(skew(_theta));
+    if (theta2 <= EPS_SMALL)
+        return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
+    T theta = sqrt(theta2);  // rotation angle
+    Eigen::Matrix<T, 3, 3> M1, M2;
+    M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
+    M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
+    return Eigen::Matrix<T, 3, 3>::Identity() - M1 + M2;
+}
+
+/** \brief Compute Jrinv (inverse of Right Jacobian which corresponds to the jacobian of log)
+ *  Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
+ *  G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ *
+ *      logmap( R * expmap(d_theta) ) \approx logmap( R ) + Jrinv * d_theta
+ *      logmap( q * expmap(d_theta) ) \approx logmap( q ) + Jrinv * d_theta
+ *
+ *  where Jrinv = jac_SO3_right_inv(theta);
+ *
+ *  This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so that
+ *
+ *      log( exp(theta) * exp(d_theta) ) = theta + Jrinv(theta) * d_theta
+ *
+ *  or, having R = exp(theta),
+ *
+ *      log( R * exp(d_theta) ) = log(R) + Jrinv(theta) * d_theta
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
+{
+    using std::sqrt;
+    using std::cos;
+    using std::sin;
+
+    MatrixSizeCheck<3, 1>::check(_theta);
+
+    typedef typename Derived::Scalar T;
+
+    T theta2 = _theta.squaredNorm();
+    Eigen::Matrix<T, 3, 3> W(skew(_theta));
+    if (theta2 <= EPS_SMALL)
+        return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
+    T theta = sqrt(theta2);  // rotation angle
+    Eigen::Matrix<T, 3, 3> M;
+    M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
+    return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; //is this really more optimized?
+}
+
+/** \brief Compute Jl (Left Jacobian)
+ * Left Jacobian for exp map in SO(3) - equation (10.86) and following equations in
+ *  G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ *
+ *      expmap( theta + d_theta ) \approx expmap(Jl * d_theta) * expmap(theta)
+ *
+ *  where Jl = jac_SO3_left(theta);
+ *
+ *  This maps a perturbation in the tangent space (d_theta) to a perturbation on the manifold (expmap(Jl * d_theta))
+ *  so that:
+ *
+ *      exp(theta+d_theta) = exp(Jr(theta)*d_theta)*exp(theta)
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
+{
+    using std::sqrt;
+    using std::cos;
+    using std::sin;
+
+    MatrixSizeCheck<3, 1>::check(_theta);
+
+    typedef typename Derived::Scalar T;
+
+    T theta2 = _theta.squaredNorm();
+    Eigen::Matrix<T, 3, 3> W(skew(_theta));
+    if (theta2 <= EPS_SMALL)
+        return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
+    T theta = sqrt(theta2);  // rotation angle
+    Eigen::Matrix<T, 3, 3> M1, M2;
+    M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
+    M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
+    return Eigen::Matrix<T, 3, 3>::Identity() + M1 + M2;
+}
+
+/** \brief Compute Jl_inv (inverse of Left Jacobian which corresponds to the jacobian of log)
+ *  Left Jacobian for Log map in SO(3) - equation (10.86) and following equations in
+ *  G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ *
+ *      logmap( expmap(d_theta) * R ) \approx logmap( R ) + Jlinv * d_theta
+ *
+ *  where Jlinv = jac_SO3_left_inv(theta);
+ *
+ *  This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so that
+ *
+ *      log( exp(d_theta) * exp(theta) ) = theta + Jlinv(theta) * d_theta
+ *
+ *  or, having R = exp(theta),
+ *
+ *      log( exp(d_theta) * R ) = log(R) + Jlinv(theta) * d_theta
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
+{
+    using std::sqrt;
+    using std::cos;
+    using std::sin;
+
+    MatrixSizeCheck<3, 1>::check(_theta);
+
+    typedef typename Derived::Scalar T;
+
+    T theta2 = _theta.squaredNorm();
+    Eigen::Matrix<T, 3, 3> W(skew(_theta));
+    if (theta2 <= EPS_SMALL)
+        return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
+    T theta = sqrt(theta2);  // rotation angle
+    Eigen::Matrix<T, 3, 3> M;
+    M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
+    return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized?
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const Eigen::QuaternionBase<D1>& _q1,
+                    const Eigen::QuaternionBase<D2>& _q2,
+                    Eigen::QuaternionBase<D3>& _q_comp,
+                    Eigen::MatrixBase<D4>& _J_comp_q1,
+                    Eigen::MatrixBase<D5>& _J_comp_q2)
+{
+    MatrixSizeCheck<3, 3>::check(_J_comp_q1);
+    MatrixSizeCheck<3, 3>::check(_J_comp_q2);
+
+    _q_comp = _q1 * _q2;
+
+    _J_comp_q1 = q2R(_q2.conjugate()); //  R2.tr
+    _J_comp_q2 . setIdentity();
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void between(const Eigen::QuaternionBase<D1>& _q1,
+                    const Eigen::QuaternionBase<D2>& _q2,
+                    Eigen::QuaternionBase<D3>& _q_between,
+                    Eigen::MatrixBase<D4>& _J_between_q1,
+                    Eigen::MatrixBase<D5>& _J_between_q2)
+{
+    MatrixSizeCheck<3, 3>::check(_J_between_q1);
+    MatrixSizeCheck<3, 3>::check(_J_between_q2);
+
+    _q_between = _q1.conjugate() * _q2;
+
+    _J_between_q1 = -q2R(_q2.conjugate()*_q1); // - R2.tr * R1
+    _J_between_q2 . setIdentity();
+}
+
+template<typename D1, typename D2>
+inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
+{
+    MatrixSizeCheck<3,1>::check(v);
+    return q * exp_q(v);
+}
+
+template<typename D1, typename D2>
+inline  Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+{
+    return log_q(q1.conjugate() * q2);
+}
+
+template<typename D1, typename D2>
+inline  Eigen::Matrix<typename D2::Scalar, 3, 1> diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+{
+    return minus(q1, q2);
+}
+
+template<typename T>
+inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
+                                                 const T pitch,
+                                                 const T yaw)
+{
+    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll,  Eigen::Matrix<T, 3, 1>::UnitX());
+    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
+    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw,   Eigen::Matrix<T, 3, 1>::UnitZ());
+
+    return (az * ay * ax).toRotationMatrix().matrix();
+}
+
+template <typename Derived>
+inline typename Eigen::MatrixBase<Derived>::Scalar
+getYaw(const Eigen::MatrixBase<Derived>& R)
+{
+    MatrixSizeCheck<3, 3>::check(R);
+
+    using std::atan2;
+    return atan2( R(1, 0), R(0, 0) );
+}
+
+} // namespace wolf
+
+#endif /* ROTATIONS_H_ */
diff --git a/src/common_class/trifocaltensor.cpp b/src/common_class/trifocaltensor.cpp
index 12c4cc3..ba39fe2 100644
--- a/src/common_class/trifocaltensor.cpp
+++ b/src/common_class/trifocaltensor.cpp
@@ -81,6 +81,19 @@ void TrifocalTensor::fillTensor(const Eigen::VectorXd& tensorVector)
 	}
 }
 
+void TrifocalTensor::getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2, Eigen::Matrix3d& T3)
+{
+	for(size_t ii = 0; ii<3; ++ii)
+	{
+		for(size_t jj = 0; jj<3; ++jj)
+		{
+			T1(ii,jj) = tensor_[ii][jj][0];
+			T2(ii,jj) = tensor_[ii][jj][1];
+			T3(ii,jj) = tensor_[ii][jj][2];
+		}
+	}
+}
+
 std::vector<std::vector<int> > TrifocalTensor::getAllComb(const int& nelements)
 {
 	std::vector<std::vector<int> > list;
diff --git a/src/common_class/trifocaltensor.h b/src/common_class/trifocaltensor.h
index 4d4097f..7100d22 100644
--- a/src/common_class/trifocaltensor.h
+++ b/src/common_class/trifocaltensor.h
@@ -42,6 +42,8 @@ public:
 	bool computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
 	bool computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
 
+	void getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2, Eigen::Matrix3d& T3);
+
 	/**
 	 * \brief Compute the coordinates of the point p3 on third img corresponding to p1 and p2 of others images
 	 */
diff --git a/src/test/gtest_trifocaltensor.cpp b/src/test/gtest_trifocaltensor.cpp
index 1d2d7e2..8ef5330 100644
--- a/src/test/gtest_trifocaltensor.cpp
+++ b/src/test/gtest_trifocaltensor.cpp
@@ -6,6 +6,7 @@
 #include "../descriptors.h"
 #include "../matchers.h"
 #include "../common_class/trifocaltensor.h"
+#include "../common_class/rotations.h"
 
 cv::Mat image1, image2, image3;
 std::string vu_root = _VU_ROOT_DIR;
@@ -525,6 +526,105 @@ TEST(TrifocalTensor, ComputeTensorReal)
 }
 
 
+TEST(TrifocalTensor, computeTensorFromProjectionMat)
+{
+	// Point in three images
+	Eigen::Vector3d p1c1, p1c2, p1c3;
+	p1c1 << 0.0,0.0,1.0;
+	p1c2 << 0.1045, 0.0, 1.0;
+	p1c3 << -0.2,0.0,1.0;
+
+	VU_DEBUG("");
+
+	//Extrisinsics
+	Eigen::Vector3d wtc1, wtc2, wtc3;
+	wtc1 << 0,0,5;
+	wtc2 << 5,0,5;
+	wtc3 << 10,0,5;
+
+	VU_DEBUG("");
+
+	Eigen::Vector3d angc1, angc2, angc3;
+	angc1 << -90.0,45.0,0.0;
+	angc1 = angc1*M_PI/180.0;
+
+//	angc2 << -90.0,-20.0,0.0;
+//	angc2 = angc2*M_PI/180.0;
+//
+//	angc3 << -90.0,-45.0,0.0;
+//	angc3 = angc3*M_PI/180.0;
+
+	VU_DEBUG("");
+
+	Eigen::Matrix3d wRc1 = vision_utils::matrixRollPitchYaw(angc1(0),angc1(1),angc1(2));
+//	Eigen::Matrix3d wRc2 = vision_utils::matrixRollPitchYaw(angc2(0),angc2(1),angc2(2));
+//	Eigen::Matrix3d wRc3 = vision_utils::matrixRollPitchYaw(angc3(0),angc3(1),angc3(2));
+
+	std::cout << " wRc1 " << std::endl << wRc1.transpose() << std::endl;
+	//	std::cout << " wRc2 " << wRc2 << std::endl;
+//	std::cout << " wRc3 " << wRc3 << std::endl;
+
+//
+//	VU_DEBUG("");
+//
+//	// Extrinsics between cameras
+//	Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2;
+//	Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3;
+//	Eigen::Vector3d c1tc2 = -wRc1.transpose()*(wtc2-wtc1);
+//	Eigen::Vector3d c1tc3 = -wRc1.transpose()*(wtc3-wtc1);
+//
+//	VU_DEBUG("");
+//
+//	// Projection matrices (without K intrinsics)
+//	Eigen::MatrixXd P1(3,4), P2(3,4), P3(3,4);
+//	P1.block(0,0,3,3) = wRc1;
+//	P1.block(0,3,3,1) = wtc1;
+//	P2.block(0,0,3,3) = wRc2;
+//	P2.block(0,3,3,1) = wtc2;
+//	P3.block(0,0,3,3) = wRc3;
+//	P3.block(0,3,3,1) = wtc3;
+//
+//	VU_DEBUG("");
+//
+//	// Compute tensor
+//	vision_utils::TrifocalTensor tensor(P1,P2,P3);
+//
+//	VU_DEBUG("");
+//
+//	// Fundamental Matrices
+//	Eigen::Matrix3d F12 = c1Rc2.transpose()*vision_utils::skew(c1tc2);
+//	Eigen::Matrix3d F13 = c1Rc3.transpose()*vision_utils::skew(c1tc3);
+//
+//	VU_DEBUG("");
+//
+//	// Epipolar lines c1->c2 and c2->c3 in normalized C2 and C3 3D spaces
+//	Eigen::Vector3d epLinec2_norm = F12*p1c1;
+//	Eigen::Vector3d	epLinec3_norm = F13*p1c1;
+//
+//	VU_DEBUG("");
+//
+//	// Create 2D lines (perpendicular to epipolar lines)
+//	Eigen::Vector3d l1, l2, l3;
+//	l1 = p1c1;
+//	l2 << epLinec2_norm(1), -epLinec2_norm(0), epLinec2_norm(0)*p1c2(1)-epLinec2_norm(1)*p1c2(0);
+//	l3 << epLinec3_norm(1), -epLinec3_norm(0), epLinec3_norm(0)*p1c3(1)-epLinec3_norm(1)*p1c3(0);
+//
+//	VU_DEBUG("");
+//
+//	// Element computed using the tensor
+//	Eigen::Matrix3d T1, T2, T3;
+//	tensor.getLayers(T1,T2,T3);
+//	Eigen::Matrix3d xTi(3,3);
+//	xTi = p1c1(0,0)*T1;
+//	xTi = xTi + p1c1(1,0)*T2;
+//	xTi = xTi + p1c1(2,0)*T3;
+//
+//	std::cout << xTi << std::endl;
+
+
+
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
index c628fad..ef0a0fc 100644
--- a/src/vision_utils.cpp
+++ b/src/vision_utils.cpp
@@ -368,6 +368,5 @@ Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& l_in, Eigen::Matri
 	return T;
 }
 
-
 } /* namespace vision_utils */
 
diff --git a/src/vision_utils.h b/src/vision_utils.h
index e412e43..57dec8d 100644
--- a/src/vision_utils.h
+++ b/src/vision_utils.h
@@ -42,7 +42,17 @@ typedef std::vector<cv::DMatch> DMatchVector;
 
 namespace vision_utils {
 
-const double EPS = 1e-8;
+//typedef float Scalar;         // Use this for float, 32 bit precision
+typedef double Scalar;        // Use this for double, 64 bit precision
+//typedef long double Scalar;   // Use this for long double, 128 bit pre
+
+typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE Size;
+
+#define M_TORAD 0.017453292519943295769236907684886127134  // pi / 180
+#define M_TODEG 57.295779513082320876798154814105170332    // 180 / pi
+
+const Scalar EPS = 1e-8; // Standard tolerance
+const Scalar EPS_SMALL = 1e-16; // Small tolerance
 
 /////////////////////////////////////////////////////////////////////////
 //      TYPEDEFS FOR POINTERS, LISTS AND ITERATORS
@@ -244,6 +254,81 @@ bool next_combination(const Iterator first, Iterator k, const Iterator last)
 
 Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& list1, Eigen::MatrixXd& l1);
 
+//////////////////////////////////////////////////////////
+/** Check Eigen Matrix sizes, both statically and dynamically
+ *
+ * Help:
+ *
+ * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
+ * (Static size, Dynamic size, Map, Matrix expression).
+ *
+ * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
+ *
+ *      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
+ *      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
+ *      EIGEN_STATIC_ASSERT_VECTOR_ONLY
+ *
+ * but they do not work if the evaluated types are of dynamic size.
+ *
+ * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
+ *
+ * template<typename Derived>
+ * inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
+ *
+ *     MatrixSizeCheck<3,1>::check(_v); /// We check here the size of the input parameter
+ *
+ *     typedef typename Derived::Scalar T;
+ *
+ *     ... code ...
+ *
+ *     return M;
+ *     }
+ *
+ * The function :  MatrixSizeCheck <Rows, Cols>::check(M)   checks that the Matrix M is of size ( Rows x Cols ).
+ * This check is performed statically or dynamically, depending on the type of argument provided.
+ */
+template<int Size, int DesiredSize>
+struct StaticSizeCheck
+{
+        template<typename T>
+        StaticSizeCheck(const T&)
+        {
+            static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
+        }
+};
+//
+template<int DesiredSize>
+struct StaticSizeCheck<Eigen::Dynamic, DesiredSize>
+{
+        template<typename T>
+        StaticSizeCheck(const T& t)
+        {
+            assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
+        }
+};
+//
+template<int DesiredR, int DesiredC>
+struct MatrixSizeCheck
+{
+        /** \brief Assert matrix size dynamically or statically
+         *
+         * @param t the variable for which we wish to assert the size.
+         *
+         * Usage: to assert that t is size (Rows x Cols)
+         *
+         *  MatrixSizeCheck<Rows, Cols>::check(t);
+         */
+        template<typename T>
+        static void check(const T& t)
+        {
+            StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
+            StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
+        }
+};
+//
+// End of check matrix sizes /////////////////////////////////////////////////
+
+
 } /* namespace vision_utils */
 
 #endif
-- 
GitLab