diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e564d333c679de85d2746d51720b986ab6731ffe..6d93eae66bb27879d4a06c212b542b69f99112c5 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 0000000000000000000000000000000000000000..d22a678d89aec07dc6cb670593c1c97982574626
--- /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 12c4cc3aa382e92b9127a7834e6fe5e7628b9d3a..ba39fe21db986d952b6ddbee1cbe2268bc74af26 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 4d4097fd70a0e620ded48ab847a19c58fa79022b..7100d226e44c4389c1f3be3ccd29f054c0fd4626 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 1d2d7e2cdf915d5ef87137f5aa4751eac70751df..8ef5330041476a38191fcf68fd18fa4cf342d5e9 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 c628fad9d3fd5f1a6bb5c74898b3247cb2bc4ce3..ef0a0fc57108677ac8aa8203bb12e9815159b021 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 e412e43c00b8c08fef6ce4a12f088ce7d612f68e..57dec8db9eb8ec807c850d35e1c002c859a46c96 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