Add function errorTrifocalPLP without JACS

src/common_class/trifocaltensor.h

@@ -637,6 +637,52 @@ typedef std::weak_ptr<TrifocalTensor>            TrifocalTensorWPtr;
 namespace vision_utils {
 
 // TRILINEARITY PLP
+template<typename D1, class T, typename D2, typename D3>
+void errorTrifocalPLP(const Eigen::MatrixBase<D1>& _m1,
+                      const Eigen::MatrixBase<D1>& _m2,
+                      const Eigen::MatrixBase<D1>& _m3,
+                      const vision_utils::TrifocalTensorBase<T>& _tensor,
+                      const Eigen::MatrixBase<D2>& _c2Ec1,
+                      Eigen::MatrixBase<D3>& _e)
+{
+    // l2 and l3: epipolar lines of m1 in cam 2 and cam 3
+    Eigen::Matrix<T,3,1> l2 = _c2Ec1*_m1;
+
+    // p2: line in cam 2 perpendicular to epipolar
+    Eigen::Matrix<T,3,1> p2;
+    p2(0)                 =  _m2(2) * l2(1);
+    p2(1)                 = -_m2(2) * l2(0);
+    p2(2)                 =  _m2(1) * l2(0) - _m2(0) * l2(1);
+
+    // Tensor slices
+    Eigen::Matrix<T,3,3> T0, T1, T2;
+    _tensor.getLayers(T0, T1, T2);
+
+    /*
+     * We have the formula of the trilinearity
+     *   m3e = sum_i (m1_i * T_i.tr) * p2
+     *
+     * which develops as
+     *   m3e = (m1_0 * T0.tr) * p2 + (m1_1 * T1.tr) * p2 + (m1_2 * T2.tr) * p2
+     *       = m1_0 * (T0.tr * p2) + m1_1 * (T1.tr * p2) + m1_2 * (T2.tr * p2)
+     *
+     * the second form is better for partial Jacobian computations
+     */
+    Eigen::Matrix<T,3,1> T0tp2, T1tp2, T2tp2;
+    T0tp2 = T0.transpose() * p2;
+    T1tp2 = T1.transpose() * p2;
+    T2tp2 = T2.transpose() * p2;
+
+    Eigen::Matrix<T,3,1> m3e = _m1(0) * T0tp2 + _m1(1) * T1tp2 + _m1(2) * T2tp2 ;
+
+    // Go to Euclidean plane
+    Eigen::Matrix<T,2,1> u3e = vision_utils::euclidean(m3e);
+    Eigen::Matrix<T,2,1> u3  = vision_utils::euclidean(_m3);
+
+    // compute trilinearity PLP error
+    _e  = u3 - u3e;
+}
+
 template<typename D1, class T, typename D2, typename D3, typename D4, typename D5, typename D6>
 void errorTrifocalPLP(const Eigen::MatrixBase<D1>& _m1,
                       const Eigen::MatrixBase<D1>& _m2,

src/test/gtest_trifocaltensor.cpp

@@ -666,6 +666,11 @@ TEST(TrifocalTensor, errorTrifocalPLP)
     Eigen::Vector2s e;
     Eigen::Matrix<Scalar, 2, 3> J_e_m1, J_e_m2, J_e_m3;
 
+    // Without JACS
+    vision_utils::errorTrifocalPLP(c1p_can, c2p_can, c3p_can, tensor, c2Ec1, e);
+    ASSERT_MATRIX_APPROX(e, _e, 1e-6);
+
+    // With JACS
     vision_utils::errorTrifocalPLP(c1p_can, c2p_can, c3p_can, tensor, c2Ec1, e, J_e_m1, J_e_m2, J_e_m3);
 
     ASSERT_MATRIX_APPROX(e, _e, 1e-6);