WIP: Vision utils

MR open while the Gitflow transition is active.

It can stay open until big merge in the future

src/constraints/constraint_autodiff_trifocal.h

@@ -114,6 +114,13 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT
         SensorCameraPtr camera_ptr_;        // To look for intrinsics
         Vector3s pixel_canonical_prev_, pixel_canonical_origin_, pixel_canonical_last_;
         Matrix3s sqrt_information_upper;
+
+        //Print function specialized for doubles (avoid jets)
+        template <class T, int ROWS, int COLS>
+        void print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat);
+
+        template <int ROWS, int COLS>
+        void print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat);
 };
 
 } // namespace wolf
@@ -311,12 +318,12 @@ inline void ConstraintAutodiffTrifocal::expectation(const MatrixBase<D1>&     _w
          *   c1Ec2 = [T]x * R        <--- Essential matrix
          *
          * or, if we prefer the constraint P2' * c2Ec1 * P1 = 0,
-         *   c2Ec1 = c1Ec2' = R' * [T]x (we obviate the sign change)
+         *   c2Ec1 = c1Ec2' = - R' * [T]x
          */
         Matrix<T,3,3> Rtr = c1Hc2.matrix().block(0,0,3,3).transpose();
         Matrix<T,3,1> t   = c1Hc2.matrix().block(0,3,3,1);
-        _c2Ec1 = Rtr * skew(t);
-//        _c2Ec1 =  c1Hc2.rotation().transpose() * skew(c1Hc2.translation()) ;
+        _c2Ec1            = - Rtr * skew(t);
+//        _c2Ec1 = - c1Hc2.rotation().transpose() * skew(c1Hc2.translation()) ;
 }
 
 template<typename T, typename D1>
@@ -397,6 +404,16 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_
 
 }
 
+// Print function
+template<class T, int ROWS, int COLS>
+void ConstraintAutodiffTrifocal::print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat)
+{}
+
+template<int ROWS, int COLS>
+void ConstraintAutodiffTrifocal::print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat)
+{
+    std::cout << _mat << std::endl;
+}
 
 }    // namespace wolf