srba/impl/jacobians.h

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/math/jacobians.h>
#include <mrpt/poses/SE_traits.h>
#include <srba/landmark_jacob_families.h>

namespace srba {

#ifndef SRBA_USE_NUMERIC_JACOBIANS
#  define SRBA_USE_NUMERIC_JACOBIANS             0
#endif
#ifndef SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS
#  define SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS  0
#endif

#define SRBA_COMPUTE_NUMERIC_JACOBIANS   (SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS || SRBA_USE_NUMERIC_JACOBIANS)
#define SRBA_COMPUTE_ANALYTIC_JACOBIANS  (SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS || !SRBA_USE_NUMERIC_JACOBIANS)

#define DEBUG_JACOBIANS_SUPER_VERBOSE  0
#define DEBUG_NOT_UPDATED_ENTRIES      0   // Extremely slow, just for debug during development! This checks that all the expected Jacobians are actually updated


namespace internal {
    template <landmark_jacob_family_t LM_JACOB_FAMILY>
    struct recompute_all_Jacobians_dh_df;

    // Specialization for "normal SLAM" (SLAM with real landmarks)
    template <> struct recompute_all_Jacobians_dh_df<jacob_point_landmark> {
        template <class RBAENGINE,class LSTJACOBCOLS,class LSTPOSES>
        static size_t eval(
            RBAENGINE &rba,
            LSTJACOBCOLS  &lst_JacobCols_df,  // std::vector<typename RBAENGINE::TSparseBlocksJacobians_dh_df::col_t*>
            LSTPOSES * out_list_of_required_num_poses ) // std::vector<const typename RBAENGINE::kf2kf_pose_traits<RBAENGINE::kf2kf_pose_t>::pose_flag_t*>
        {
            const size_t nUnknowns_k2f = lst_JacobCols_df.size();
            size_t nJacobs = 0;
            for (size_t i=0;i<nUnknowns_k2f;i++)
            {
                // For each column, process each nonzero block:
                typename RBAENGINE::TSparseBlocksJacobians_dh_df::col_t *col = lst_JacobCols_df[i];

                for (typename RBAENGINE::TSparseBlocksJacobians_dh_df::col_t::iterator it=col->begin();it!=col->end();++it)
                {
                    const size_t obs_idx = it->first;
                    typename RBAENGINE::TSparseBlocksJacobians_dh_df::TEntry & jacob_entry = it->second;
                    rba.compute_jacobian_dh_df(
                        jacob_entry,
                        rba.get_rba_state().all_observations[obs_idx],
                        out_list_of_required_num_poses );
                    nJacobs++;
                }
            }
            return nJacobs;
        }

    };

    // Specialization for relative graph-SLAM (no real landmarks)
    template <> struct recompute_all_Jacobians_dh_df<jacob_relpose_landmark> {
        template <class RBAENGINE,class LSTJACOBCOLS,class LSTPOSES>
        static size_t eval(
            RBAENGINE &rba,
            LSTJACOBCOLS  &lst_JacobCols_df,  // std::vector<typename RBAENGINE::TSparseBlocksJacobians_dh_df::col_t*>
            LSTPOSES * out_list_of_required_num_poses ) // std::vector<const typename RBAENGINE::kf2kf_pose_traits<RBAENGINE::kf2kf_pose_t>::pose_flag_t*>
        {
            MRPT_UNUSED_PARAM(rba); MRPT_UNUSED_PARAM(lst_JacobCols_df);
            MRPT_UNUSED_PARAM(out_list_of_required_num_poses);
            // Nothing to do: this will never be actually called.
            return 0;
        }
    };
} // end "internal" ns


template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::numeric_dh_dAp(const array_pose_t &x, const TNumeric_dh_dAp_params& params, array_obs_t &y)
{
    pose_t incr(mrpt::poses::UNINITIALIZED_POSE);
    mrpt::poses::SE_traits<pose_t::rotation_dimensions>::pseudo_exp(x,incr);

    pose_t base_from_obs(mrpt::poses::UNINITIALIZED_POSE);
    if (!params.is_inverse_dir)
    {
        if (params.pose_d1_wrt_obs) // "A" in papers
                base_from_obs.composeFrom(*params.pose_d1_wrt_obs,incr);
        else    base_from_obs = incr;
        base_from_obs.composeFrom(base_from_obs,params.pose_base_wrt_d1);
    }
    else
    {
        // Changes due to the inverse pose:
        // D becomes D' = p_d^{d+1} (+) D
        ASSERT_(params.k2k_edge_id<params.k2k_edges.size())
        const pose_t & p_d_d1 = params.k2k_edges[params.k2k_edge_id].inv_pose;

        pose_t pose_base_wrt_d_prime(mrpt::poses::UNINITIALIZED_POSE);  // D' in papers
        pose_base_wrt_d_prime.composeFrom( p_d_d1, params.pose_base_wrt_d1 );

        pose_t p_d_d1_mod(mrpt::poses::UNINITIALIZED_POSE);
        p_d_d1_mod.composeFrom(incr, p_d_d1);
        p_d_d1_mod.inverse();

        // total pose: base from obs =  pose_d1_wrt_obs (+) inv(p_d_d1_mod) (+) D'
        if (params.pose_d1_wrt_obs) // "A" in papers
                base_from_obs = *params.pose_d1_wrt_obs + p_d_d1_mod + pose_base_wrt_d_prime;
        else    base_from_obs =  p_d_d1_mod + pose_base_wrt_d_prime;
    }

    // pose_robot2sensor(): pose wrt sensor = pose_wrt_robot (-) sensor_pose_on_the_robot
    typename options::internal::resulting_pose_t<typename RBA_OPTIONS::sensor_pose_on_robot_t,REL_POSE_DIMS>::pose_t base_pose_wrt_sensor(mrpt::poses::UNINITIALIZED_POSE);
    RBA_OPTIONS::sensor_pose_on_robot_t::pose_robot2sensor( base_from_obs, base_pose_wrt_sensor, params.sensor_pose );

    // Generate observation:
    array_obs_t z_zero;
    z_zero.setZero();

    sensor_model_t::observe_error(y,z_zero,base_pose_wrt_sensor,params.xji_i, params.sensor_params);
    y=-y; // because the method above evals: "z_zero - h(x)"
}

#if DEBUG_NOT_UPDATED_ENTRIES

struct TNumSTData
{
    TKeyFrameID from, to;
};

TNumSTData check_num_st_entry_exists(
    const pose_flag_t * entry,
    const TRBA_Problem_state::TSpanningTree & st)
{
    for (TRelativePosesForEachTarget::const_iterator it_tree=st.num.begin();it_tree!=st.num.end();++it_tree)
    {
        const TKeyFrameID id_from = it_tree->first;
        const frameid2pose_map_t & tree = it_tree->second;

        for (frameid2pose_map_t::const_iterator it=tree.begin();it!=tree.end();++it)
        {
            const TKeyFrameID id_to = it->first;
            const pose_flag_t & e = it->second;
            if (&e == entry)
            {
                TNumSTData d;
                d.from = id_from;
                d.to = id_to;
                return d;
            }
        }
    }
    ASSERT_(false)
}

#endif

template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::numeric_dh_df(const array_landmark_t &x, const TNumeric_dh_df_params& params, array_obs_t &y)
{
    static const pose_t my_aux_null_pose;

    const array_landmark_t x_local = params.xji_i + x;
    const pose_t * pos_cam = params.pose_base_wrt_obs!=NULL ? params.pose_base_wrt_obs : &my_aux_null_pose;

    // pose_robot2sensor(): pose wrt sensor = pose_wrt_robot (-) sensor_pose_on_the_robot
    typename options::internal::resulting_pose_t<typename RBA_OPTIONS::sensor_pose_on_robot_t,REL_POSE_DIMS>::pose_t  base_pose_wrt_sensor(mrpt::poses::UNINITIALIZED_POSE);
    RBA_OPTIONS::sensor_pose_on_robot_t::pose_robot2sensor( *pos_cam, base_pose_wrt_sensor, params.sensor_pose );

    // Generate observation:
    array_obs_t z_zero;
    z_zero.setZero();
    sensor_model_t::observe_error(y,z_zero,base_pose_wrt_sensor,x_local, params.sensor_params);
    y=-y; // because the method above evals: "z_zero - h(x)"
}


template <landmark_jacob_family_t JACOB_FAMILY, size_t POINT_DIMS, size_t POSE_DIMS, class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps;

// ====================================================================
//                       j,i                    lm_id,base_id
//           \partial  h            \partial  h
//                       l                      obs_frame_id
// dh_dp = ------------------ = ---------------------------------
//                       d+1                    cur_id
//           \partial  p            \partial  p
//                       d                      stp.next_node
//
//  See tech report:
//   "A tutorial on SE(3) transformation parameterizations and
//    on-manifold optimization", Jose-Luis Blanco.
// ====================================================================
template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::compute_jacobian_dh_dp(
    typename TSparseBlocksJacobians_dh_dAp::TEntry  &jacob,
    const k2f_edge_t & observation,
    const k2k_edges_deque_t  &k2k_edges,
    std::vector<const pose_flag_t*>    *out_list_of_required_num_poses) const
{
    ASSERT_(observation.obs.kf_id!=jacob.sym.kf_base)

    if (! *jacob.sym.is_valid )
        return; // Another block of the same Jacobian row said this observation was invalid for some reason.

    // And x^{j,i}_l = pose_of_i_wrt_l (+) x^{j,i}_i

    // Handle the special case when d==obs, so rel_pose_d1_from_obs==NULL, and its pose is the origin:
    const pose_flag_t * pose_d1_wrt_obs  =  jacob.sym.rel_pose_d1_from_obs; // "A" in papers
    const pose_flag_t & pose_base_wrt_d1 = *jacob.sym.rel_pose_base_from_d1;
    const bool is_inverse_edge_jacobian = !jacob.sym.edge_normal_dir;  // If edge points in the opposite direction than as assumed in mathematical derivation.

    if (out_list_of_required_num_poses)
    {
        if (jacob.sym.rel_pose_d1_from_obs)
            out_list_of_required_num_poses->push_back(jacob.sym.rel_pose_d1_from_obs);
        out_list_of_required_num_poses->push_back(jacob.sym.rel_pose_base_from_d1);
    }

    // make sure the numeric spanning tree is working and updating all that we need:
#if DEBUG_NOT_UPDATED_ENTRIES
    TNumSTData d1 = check_num_st_entry_exists(&pose_base_wrt_d1, rba_state.spanning_tree);
#endif

    if (!pose_base_wrt_d1.updated)
    {
        std::cerr << " kf_d+1: " << jacob.sym.kf_d << ", base_id: "<< jacob.sym.kf_base << std::endl;
        rba_state.spanning_tree.save_as_dot_file("_debug_jacob_error_all_STs.dot");
        ASSERT_(pose_base_wrt_d1.updated)
    }

    if (pose_d1_wrt_obs)
    {
#if DEBUG_NOT_UPDATED_ENTRIES
        TNumSTData d2 = check_num_st_entry_exists(pose_d1_wrt_obs, rba_state.spanning_tree);
#endif
        if (!pose_d1_wrt_obs->updated)
        {
            std::cerr << " kf_d+1: " << jacob.sym.kf_d << ", obs_frame_id: "<< observation.obs.kf_id << std::endl;
            rba_state.spanning_tree.save_as_dot_file("_debug_jacob_error_all_STs.dot");
        }
        ASSERT_(pose_d1_wrt_obs->updated)
    }


    // i<-l = d <- obs/l  (+)  base/i <- d
    pose_t pose_i_wrt_l(mrpt::poses::UNINITIALIZED_POSE);
    if (pose_d1_wrt_obs!=NULL)
            pose_i_wrt_l.composeFrom( pose_d1_wrt_obs->pose, pose_base_wrt_d1.pose);
    else    pose_i_wrt_l = pose_base_wrt_d1.pose;

    // First, we need x^{j,i}_i:
    // LM parameters in: jacob.sym.feat_rel_pos->pos[0:N-1]
    const array_landmark_t &xji_i = jacob.sym.feat_rel_pos->pos;

    // xji_l = pose_i_wrt_l (+) xji_i
    array_landmark_t xji_l = xji_i; //
    landmark_t::composePosePoint(xji_l, pose_i_wrt_l);

#if DEBUG_JACOBIANS_SUPER_VERBOSE  // Debug:
    {
        const TKeyFrameID obs_frame_id = observation.obs.kf_id;
        const TKeyFrameID base_id      = jacob.sym.kf_base;
        const TKeyFrameID d_plus_1_id  = jacob.sym.kf_d;
        cout << "compute_jacobian_dh_dp: "
            << " obs_frame_id: " << obs_frame_id
            << " base_id: " << base_id
            << " kf_d+1: " << d_plus_1_id
            << " k2k_edge_id: " << jacob.sym.k2k_edge_id
            << " is_inverse: " << is_inverse_edge_jacobian << endl
            << " pose_base_wrt_d1 (D): "<<pose_base_wrt_d1.pose << endl;
        if (pose_d1_wrt_obs) cout << " pose_d1_wrt_obs (A): "<< pose_d1_wrt_obs->pose<< endl;
        // Observations from the "base_id" of a fixed,known landmark are NOT considered observations for optimization:
        //mrpt::system::pause();
    }
#endif

#if SRBA_COMPUTE_NUMERIC_JACOBIANS
    // Numeric jacobians
    typename TSparseBlocksJacobians_dh_dAp::matrix_t  num_jacob;

    array_pose_t x;
    x.setZero(); // Evaluate Jacobian at manifold incr around origin
    array_pose_t x_incrs;
    x_incrs.setConstant(1e-4);

    const TNumeric_dh_dAp_params num_params(jacob.sym.k2k_edge_id,&pose_d1_wrt_obs->pose, pose_base_wrt_d1.pose,jacob.sym.feat_rel_pos->pos, is_inverse_edge_jacobian,k2k_edges,this->parameters.sensor,this->parameters.sensor_pose);

    mrpt::math::jacobians::jacob_numeric_estimate(x,&numeric_dh_dAp,x_incrs,num_params,num_jacob);

#endif // SRBA_COMPUTE_NUMERIC_JACOBIANS


#if SRBA_COMPUTE_ANALYTIC_JACOBIANS
    //  d h(x^{j,i}_l)    d h(x')       d x^{j,i}_l
    // --------------- = --------- * ----------------
    //  d p^{d+1}_d        d x'         d epsilon
    //
    //                  With: x' = x^{j,i}_l

    // First jacobian: (uses xji_l)
    // -----------------------------
    Eigen::Matrix<double,OBS_DIMS,LM_DIMS>  dh_dx;

    // Converts a point relative to the robot coordinate frame (P) into a point relative to the sensor (RES = P \ominus POSE_IN_ROBOT )
    RBA_OPTIONS::sensor_pose_on_robot_t::template point_robot2sensor<landmark_t,array_landmark_t>(xji_l,xji_l,this->parameters.sensor_pose );

    // Invoke sensor model:
    if (!sensor_model_t::eval_jacob_dh_dx(dh_dx,xji_l, this->parameters.sensor))
    {
        // Invalid Jacobian:
        *jacob.sym.is_valid = 0;
        jacob.num.setZero();
        return;
    }

    // take into account the possible displacement of the sensor wrt the keyframe:
    RBA_OPTIONS::sensor_pose_on_robot_t::jacob_dh_dx_rotate( dh_dx, this->parameters.sensor_pose );

    // Second Jacobian: (uses xji_i)
    // ------------------------------
    compute_jacobian_dAepsDx_deps<landmark_t::jacob_family,LM_DIMS,REL_POSE_DIMS,rba_engine_t>::eval(jacob.num,dh_dx,is_inverse_edge_jacobian,xji_i, pose_d1_wrt_obs, pose_base_wrt_d1,jacob.sym,k2k_edges,rba_state.all_observations);

#endif // SRBA_COMPUTE_ANALYTIC_JACOBIANS


#if SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS
    // Check jacob.num vs. num_jacob
    const double MAX_REL_ERROR = 0.1;
    if ((jacob.num-num_jacob).array().abs().maxCoeff()>MAX_REL_ERROR*num_jacob.array().maxCoeff())
    {
        std::cerr << "NUMERIC VS. ANALYTIC JACOBIAN dh_dAp FAILED:"
            << "\njacob.num:\n" << jacob.num
            << "\nnum_jacob:\n" << num_jacob
            << "\nDiff:\n" << jacob.num-num_jacob << endl << endl;
    }
#endif

#if SRBA_USE_NUMERIC_JACOBIANS
    jacob.num = num_jacob;
#endif
}

// ====================================================================
//  Auxiliary sub-Jacobian used in compute_jacobian_dh_dp()
// These are specializations of the template, for each of the cases:
//template <size_t , size_t , class MATRIX, class POINT>
// ====================================================================

// Case: 3D point, SE(3) poses:
template <class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps<jacob_point_landmark /* Jacobian family: this LM is a point */, 3 /*POINT_DIMS*/,6 /*POSE_DIMS*/,RBA_ENGINE_T>
{
    template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
    static void eval(
        MATRIX      & jacob,
        const MATRIX_DH_DX  & dh_dx,
        const bool is_inverse_edge_jacobian,
        const POINT & xji_i,
        const pose_flag_t * pose_d1_wrt_obs,  // "A" in handwritten notes
        const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
        const JACOB_SYM_T & jacob_sym,
        const K2K_EDGES_T & k2k_edges,
        const OBS_VECTOR  & all_obs
        )
    {
        MRPT_UNUSED_PARAM(all_obs);
        // See section 10.3.7 of technical report on SE(3) poses [http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf]
        if (!is_inverse_edge_jacobian)
        {   // Normal formulation: unknown is pose "d+1 -> d"

            // This is "D" in my handwritten notes:
            // pose_i_wrt_dplus1  -> pose_base_wrt_d1

            const mrpt::math::CMatrixDouble33 & ROTD = pose_base_wrt_d1.pose.getRotationMatrix();

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0,0,0,0):
            Eigen::Matrix<double,RBA_ENGINE_T::OBS_DIMS,3> H_ROTA;
            if (pose_d1_wrt_obs!=NULL)
            {
                // pose_d_plus_1_wrt_l  -> pose_d1_wrt_obs

                // 3x3 term: H*R(A)
                H_ROTA = dh_dx * pose_d1_wrt_obs->pose.getRotationMatrix();
            }
            else
            {
                // 3x3 term: H*R(A)
                H_ROTA = dh_dx;
            }

            // First 2x3 block:
            jacob.block(0,0,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA;

            // Second 2x3 block:
            // compute aux vector "v":
            Eigen::Matrix<double,3,1> v;
            v[0] =  -pose_base_wrt_d1.pose.x()  - xji_i[0]*ROTD.coeff(0,0) - xji_i[1]*ROTD.coeff(0,1) - xji_i[2]*ROTD.coeff(0,2);
            v[1] =  -pose_base_wrt_d1.pose.y()  - xji_i[0]*ROTD.coeff(1,0) - xji_i[1]*ROTD.coeff(1,1) - xji_i[2]*ROTD.coeff(1,2);
            v[2] =  -pose_base_wrt_d1.pose.z()  - xji_i[0]*ROTD.coeff(2,0) - xji_i[1]*ROTD.coeff(2,1) - xji_i[2]*ROTD.coeff(2,2);

            Eigen::Matrix<double,3,3> aux;

            aux.coeffRef(0,0)=0;
            aux.coeffRef(1,1)=0;
            aux.coeffRef(2,2)=0;

            aux.coeffRef(1,2)=-v[0];
            aux.coeffRef(2,1)= v[0];
            aux.coeffRef(2,0)=-v[1];
            aux.coeffRef(0,2)= v[1];
            aux.coeffRef(0,1)=-v[2];
            aux.coeffRef(1,0)= v[2];

            jacob.block(0,3,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA * aux;
        }
        else
        {   // Inverse formulation: unknown is pose "d -> d+1"

            // Changes due to the inverse pose:
            // D becomes D' = p_d^{d+1} (+) D

            ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
            const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id].inv_pose;

            typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
            pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );

            const mrpt::math::CMatrixDouble33 & ROTD = pose_base_wrt_d1_prime.getRotationMatrix();

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0,0,0,0):
            Eigen::Matrix<double,RBA_ENGINE_T::OBS_DIMS,3> H_ROTA;
            if (pose_d1_wrt_obs!=NULL)
            {
                // pose_d_plus_1_wrt_l  -> pose_d1_wrt_obs

                // In inverse edges, A (which is "pose_d1_wrt_obs") becomes A * (p_d_d1)^-1 =>
                //   So: ROT_A' = ROT_A * ROT_d_d1^t

                // 3x3 term: H*R(A')
                H_ROTA = dh_dx * pose_d1_wrt_obs->pose.getRotationMatrix() * p_d_d1.getRotationMatrix().transpose();
            }
            else
            {
                // Was in the normal edge: H_ROTA = dh_dx;

                // 3x3 term: H*R(A')
                H_ROTA = dh_dx * p_d_d1.getRotationMatrix().transpose();
            }

            // First 2x3 block:
            jacob.block(0,0,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA;

            // Second 2x3 block:
            // compute aux vector "v":
            Eigen::Matrix<double,3,1> v;
            v[0] =  -pose_base_wrt_d1_prime.x()  - xji_i[0]*ROTD.coeff(0,0) - xji_i[1]*ROTD.coeff(0,1) - xji_i[2]*ROTD.coeff(0,2);
            v[1] =  -pose_base_wrt_d1_prime.y()  - xji_i[0]*ROTD.coeff(1,0) - xji_i[1]*ROTD.coeff(1,1) - xji_i[2]*ROTD.coeff(1,2);
            v[2] =  -pose_base_wrt_d1_prime.z()  - xji_i[0]*ROTD.coeff(2,0) - xji_i[1]*ROTD.coeff(2,1) - xji_i[2]*ROTD.coeff(2,2);

            Eigen::Matrix<double,3,3> aux;

            aux.coeffRef(0,0)=0;
            aux.coeffRef(1,1)=0;
            aux.coeffRef(2,2)=0;

            aux.coeffRef(1,2)=-v[0];
            aux.coeffRef(2,1)= v[0];
            aux.coeffRef(2,0)=-v[1];
            aux.coeffRef(0,2)= v[1];
            aux.coeffRef(0,1)=-v[2];
            aux.coeffRef(1,0)= v[2];

            jacob.block(0,3,RBA_ENGINE_T::OBS_DIMS,3).noalias() = H_ROTA * aux;

            // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
            jacob = -jacob;

        } // end inverse edge case

    }
}; // end of specialization of "compute_jacobian_dAepsDx_deps"


template <size_t POINT_DIMS, class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps_SE2
{
    template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
    static void eval(
        MATRIX      & jacob,
        const MATRIX_DH_DX  & dh_dx,
        const bool is_inverse_edge_jacobian,
        const POINT & xji_i,
        const pose_flag_t * pose_d1_wrt_obs,  // "A" in handwritten notes
        const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
        const JACOB_SYM_T & jacob_sym,
        const K2K_EDGES_T & k2k_edges,
        const OBS_VECTOR  & all_obs
        )
    {
        MRPT_UNUSED_PARAM(all_obs);
        MRPT_COMPILE_TIME_ASSERT(POINT_DIMS==2 || POINT_DIMS==3)

        if (!is_inverse_edge_jacobian)
        {   // Normal formulation: unknown is pose "d+1 -> d"

            const double Xd=pose_base_wrt_d1.pose.x();
            const double Yd=pose_base_wrt_d1.pose.y();

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            double PHIa=0; // Xa, Ya: Are not really needed, since they don't appear in the Jacobian.
            mrpt::poses::CPose2D AD(mrpt::poses::UNINITIALIZED_POSE); // AD = A(+)D
            if (pose_d1_wrt_obs!=NULL)
            {
                // pose_d_plus_1_wrt_l  -> pose_d1_wrt_obs
                //Xa = pose_d1_wrt_obs->pose.x(); Ya = pose_d1_wrt_obs->pose.y(); // Not needed
                PHIa = pose_d1_wrt_obs->pose.phi();
                AD.composeFrom(pose_d1_wrt_obs->pose,pose_base_wrt_d1.pose);
            }
            else
            {
                AD = pose_base_wrt_d1.pose;  // A=0 -> A(+)D is simply D
            }

            // d(P (+) x) / dP, P = A*D
            Eigen::Matrix<double,POINT_DIMS/* 2 or 3*/,3 /* x,y,phi*/> dPx_P;
            const double ccos_ad = cos(AD.phi());
            const double ssin_ad = sin(AD.phi());
            dPx_P(0,0) = 1;  dPx_P(0,1) = 0; dPx_P(0,2) = -xji_i[0]*ssin_ad - xji_i[1]*ccos_ad;
            dPx_P(1,0) = 0;  dPx_P(1,1) = 1; dPx_P(1,2) =  xji_i[0]*ccos_ad - xji_i[1]*ssin_ad;
            if (POINT_DIMS==3) {
                dPx_P(2,0) = 0;  dPx_P(2,1) = 0; dPx_P(2,2) =  1;
            }

            // d(A*exp(eps)*D) / deps
            Eigen::Matrix<double,3,3> dAD_deps;
            const double ccos_a = cos(PHIa);
            const double ssin_a = sin(PHIa);
            dAD_deps(0,0) = ccos_a; dAD_deps(0,1) = -ssin_a;
            dAD_deps(1,0) = ssin_a; dAD_deps(1,1) =  ccos_a;
            dAD_deps(2,0) = 0;    dAD_deps(2,1) =  0;

            dAD_deps(0,2) = -ssin_a*Xd - ccos_a*Yd;
            dAD_deps(1,2) =  ccos_a*Xd - ssin_a*Yd;
            dAD_deps(2,2) = 1;

            // Chain rule:
            jacob.noalias() = dh_dx * dPx_P * dAD_deps;
        }
        else
        {   // Inverse formulation: unknown is pose "d -> d+1"

            // Changes due to the inverse pose:
            // D becomes D' = p_d^{d+1} (+) D
            // and A (which is "pose_d1_wrt_obs") becomes A' = A (+) (p_d_d1)^-1

            ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
            const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id].inv_pose;

            typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
            pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            const typename RBA_ENGINE_T::pose_t p_d_d1_inv = -p_d_d1;

            typename RBA_ENGINE_T::pose_t A_prime = (pose_d1_wrt_obs!=NULL) ?
                (pose_d1_wrt_obs->pose + p_d_d1_inv)
                :
                p_d_d1_inv;

            mrpt::poses::CPose2D AD(mrpt::poses::UNINITIALIZED_POSE); // AD = A(+)D
            AD.composeFrom(A_prime,pose_base_wrt_d1_prime);

            const double Xd=pose_base_wrt_d1_prime.x();
            const double Yd=pose_base_wrt_d1_prime.y();

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            const double PHIa=A_prime.phi();


            // d(P (+) x) / dP, P = A*D
            Eigen::Matrix<double,POINT_DIMS/* 2 or 3*/,3 /* x,y,phi*/> dPx_P;
            const double ccos_ad = cos(AD.phi());
            const double ssin_ad = sin(AD.phi());
            dPx_P(0,0) = 1;  dPx_P(0,1) = 0; dPx_P(0,2) = -xji_i[0]*ssin_ad - xji_i[1]*ccos_ad;
            dPx_P(1,0) = 0;  dPx_P(1,1) = 1; dPx_P(1,2) =  xji_i[0]*ccos_ad - xji_i[1]*ssin_ad;
            if (POINT_DIMS==3) {
                dPx_P(2,0) = 0;  dPx_P(2,1) = 0; dPx_P(2,2) =  1;
            }

            // d(A*exp(eps)*D) / deps
            Eigen::Matrix<double,3,3> dAD_deps;
            const double ccos_a = cos(PHIa);
            const double ssin_a = sin(PHIa);
            dAD_deps(0,0) = ccos_a; dAD_deps(0,1) = -ssin_a;
            dAD_deps(1,0) = ssin_a; dAD_deps(1,1) =  ccos_a;
            dAD_deps(2,0) = 0;    dAD_deps(2,1) =  0;

            dAD_deps(0,2) = -ssin_a*Xd - ccos_a*Yd;
            dAD_deps(1,2) =  ccos_a*Xd - ssin_a*Yd;
            dAD_deps(2,2) = 1;

            // Chain rule:
            jacob.noalias() = dh_dx * dPx_P * dAD_deps;

            // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
            jacob = -jacob;

        } // end inverse edge case

    }
}; // end of "compute_jacobian_dAepsDx_deps_SE2"

// Case: 2D point, SE(2) poses: (derived from generic SE2 implementation above)
template <class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps<jacob_point_landmark /* Jacobian family: this LM is a point */, 2 /*POINT_DIMS*/,3 /*POSE_DIMS*/,RBA_ENGINE_T>
    : public compute_jacobian_dAepsDx_deps_SE2<2 /*POINT_DIMS*/,RBA_ENGINE_T>
{
};

// Case: 3D point, SE(2) poses: (derived from generic SE2 implementation above)
template <class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps<jacob_point_landmark /* Jacobian family: this LM is a point */, 3 /*POINT_DIMS*/,3 /*POSE_DIMS*/,RBA_ENGINE_T>
    : public compute_jacobian_dAepsDx_deps_SE2<3 /*POINT_DIMS*/,RBA_ENGINE_T>
{
};

// Case: SE(2) relative-poses, SE(2) poses:
template <class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps<jacob_relpose_landmark /* Jacobian family: this LM is a relative pose (graph-slam) */, 3 /*POINT_DIMS*/,3 /*POSE_DIMS*/,RBA_ENGINE_T>
{
    template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
    static void eval(
        MATRIX      & jacob,
        const MATRIX_DH_DX  & dh_dx,
        const bool is_inverse_edge_jacobian,
        const POINT & xji_i,
        const pose_flag_t * pose_d1_wrt_obs,  // "A" in handwritten notes
        const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
        const JACOB_SYM_T & jacob_sym,
        const K2K_EDGES_T & k2k_edges,
        const OBS_VECTOR  & all_obs
        )
    {
        MRPT_UNUSED_PARAM(xji_i);
        MRPT_UNUSED_PARAM(all_obs);
        double Xd,Yd,PHIa;
        mrpt::poses::CPose2D  base_wrt_obs(mrpt::poses::UNINITIALIZED_POSE); // A(+)D

        if (!is_inverse_edge_jacobian)
        {   // Normal formulation: unknown is pose "d+1 -> d"

            Xd=pose_base_wrt_d1.pose.x();
            Yd=pose_base_wrt_d1.pose.y();

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            PHIa=0; // Xa, Ya: Are not really needed, since they don't appear in the Jacobian.
            if (pose_d1_wrt_obs!=NULL)
            {
                // pose_d_plus_1_wrt_l  -> pose_d1_wrt_obs
                PHIa = pose_d1_wrt_obs->pose.phi();
                base_wrt_obs.composeFrom(pose_d1_wrt_obs->pose, pose_base_wrt_d1.pose);  // A (+) D
            }
            else
            {
                base_wrt_obs = pose_base_wrt_d1.pose;  // A (+) D
            }
        }
        else
        {   // Inverse formulation: unknown is pose "d -> d+1"

            // Changes due to the inverse pose:
            // D becomes D' = p_d^{d+1} (+) D
            // and A (which is "pose_d1_wrt_obs") becomes A' = A (+) (p_d_d1)^-1

            ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
            const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id].inv_pose;

            typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
            pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            const typename RBA_ENGINE_T::pose_t p_d_d1_inv = -p_d_d1;

            typename RBA_ENGINE_T::pose_t A_prime = (pose_d1_wrt_obs!=NULL) ?
                (pose_d1_wrt_obs->pose + p_d_d1_inv)
                :
                p_d_d1_inv;

            Xd=pose_base_wrt_d1_prime.x();
            Yd=pose_base_wrt_d1_prime.y();

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            PHIa=A_prime.phi();

            base_wrt_obs.composeFrom(A_prime, pose_base_wrt_d1_prime);  // A (+) D
        }

        //const mrpt::poses::CPose2D base_wrt_obs_inv = -base_wrt_obs;
        const double PHIad = base_wrt_obs.phi();

        const double ccos_ad = cos(PHIad), ssin_ad=sin(PHIad);
        const double ccos_a = cos(PHIa), ssin_a=sin(PHIa);

        Eigen::Matrix<double,3,3> J0; // -d(\ominus p)_dp, with p=A*D
        J0(0,0)= ccos_ad; J0(0,1)= ssin_ad; J0(0,2)= 0;
        J0(1,0)=-ssin_ad; J0(1,1)= ccos_ad; J0(1,2)= 0;
        J0(2,0)=0;        J0(2,1)=0;        J0(2,2)= 1;

        Eigen::Matrix<double,3,3> J1; // dAD_dA;
        J1(0,0)=1; J1(0,1)=0; J1(0,2)= -Xd*ssin_a-Yd*ccos_a;
        J1(1,0)=0; J1(1,1)=1; J1(1,2)=  Xd*ccos_a-Yd*ssin_a;
        J1(2,0)=0; J1(2,1)=0; J1(2,2)= 1;

        Eigen::Matrix<double,3,3> J2; // dAe_de
        J2(0,0)=ccos_a;  J2(0,1)=-ssin_a;  J2(0,2)=0;
        J2(1,0)=ssin_a;  J2(1,1)= ccos_a;  J2(1,2)=0;
        J2(2,0)=0;       J2(2,1)=0;        J2(2,2)=1;

        // Chain rule:
        jacob.noalias() = dh_dx * J0* J1 * J2;

        if (is_inverse_edge_jacobian)
        {
            // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
            jacob = -jacob;
        }
    }
}; // end of "compute_jacobian_dAepsDx_deps", Case: SE(2) relative-poses, SE(2) poses:

// Case: SE(3) relative-poses, SE(3) poses:
template <class RBA_ENGINE_T>
struct compute_jacobian_dAepsDx_deps<jacob_relpose_landmark /* Jacobian family: this LM is a relative pose (graph-slam) */, 6 /*POINT_DIMS*/,6 /*POSE_DIMS*/,RBA_ENGINE_T>
{
    template <class MATRIX, class MATRIX_DH_DX,class POINT,class pose_flag_t,class JACOB_SYM_T,class K2K_EDGES_T,class OBS_VECTOR>
    static void eval(
        MATRIX      & jacob,
        const MATRIX_DH_DX  & dh_dx,
        const bool is_inverse_edge_jacobian,
        const POINT & xji_i,
        const pose_flag_t * pose_d1_wrt_obs,  // "A" in handwritten notes
        const pose_flag_t & pose_base_wrt_d1, // "D" in handwritten notes
        const JACOB_SYM_T & jacob_sym,
        const K2K_EDGES_T & k2k_edges,
        const OBS_VECTOR  & all_obs
        )
    {
        MRPT_UNUSED_PARAM(xji_i);
        MRPT_UNUSED_PARAM(all_obs);
        //
        //  d ps-log(p^obs_base)      d ps-log(p)       d A*e^eps*D
        // --------------------  =  --------------- * -----------------
        //    d eps^d+1_d                d p             d eps
        //                   6x6                 6x12                12x6
        //
        //                              ^: (1)              ^: (2)
        // See section 10.3.7 of technical report on SE(3) poses [http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf]
        mrpt::math::CMatrixDouble33 ROTA;  // A.rotationMatrix
        typename RBA_ENGINE_T::pose_t  D(mrpt::poses::UNINITIALIZED_POSE);
        typename RBA_ENGINE_T::pose_t  base_wrt_obs(mrpt::poses::UNINITIALIZED_POSE); // A(+)D

        if (!is_inverse_edge_jacobian)
        {   // Normal formulation: unknown is pose "d+1 -> d"
            D = pose_base_wrt_d1.pose;

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            if (pose_d1_wrt_obs!=NULL)
            {
                // pose_d_plus_1_wrt_l  -> pose_d1_wrt_obs
                base_wrt_obs.composeFrom(pose_d1_wrt_obs->pose, D);  // A (+) D
                ROTA = pose_d1_wrt_obs->pose.getRotationMatrix();
            }
            else
            {
                base_wrt_obs = D;  // A (+) D
                ROTA.setIdentity();
            }
        }
        else
        {   // Inverse formulation: unknown is pose "d -> d+1"

            // Changes due to the inverse pose:
            // D becomes D' = p_d^{d+1} (+) D
            // and A (which is "pose_d1_wrt_obs") becomes A' = A (+) (p_d_d1)^-1

            ASSERT_(jacob_sym.k2k_edge_id<k2k_edges.size())
            const typename RBA_ENGINE_T::pose_t & p_d_d1 = k2k_edges[jacob_sym.k2k_edge_id].inv_pose;

            typename RBA_ENGINE_T::pose_t pose_base_wrt_d1_prime(mrpt::poses::UNINITIALIZED_POSE);
            pose_base_wrt_d1_prime.composeFrom( p_d_d1 , pose_base_wrt_d1.pose );

            // We need to handle the special case where "d+1"=="l", so A=Pose(0,0,0):
            const typename RBA_ENGINE_T::pose_t p_d_d1_inv = -p_d_d1;

            typename RBA_ENGINE_T::pose_t A_prime = (pose_d1_wrt_obs!=NULL) ?
                (pose_d1_wrt_obs->pose + p_d_d1_inv)
                :
                p_d_d1_inv;

            ROTA=A_prime.getRotationMatrix();

            D=pose_base_wrt_d1_prime;

            base_wrt_obs.composeFrom(A_prime, pose_base_wrt_d1_prime);  // A (+) D
        }

        const mrpt::math::CMatrixDouble44 & HM_D = D.getHomogeneousMatrixVal(); // [ROT(D) | Trans(D) ]


        // (1): Common part: d_Ln(R)_dR:
        //
        const mrpt::math::CMatrixDouble33 & ROT_AplusB = base_wrt_obs.getRotationMatrix(); // The rotation matrix.
        Eigen::Matrix<double,6,12>  dLnRelPose_deps;
        dLnRelPose_deps.setZero();
        dLnRelPose_deps.block<3,3>(0,9).setIdentity();
        {
            mrpt::math::CMatrixFixedNumeric<double,3,9> dLnRot_dRot(mrpt::math::UNINITIALIZED_MATRIX);
            mrpt::poses::CPose3D::ln_rot_jacob( ROT_AplusB, dLnRot_dRot);
            dLnRelPose_deps.block<3,9>(3,0) = dLnRot_dRot;
        }

        // (2): d A*e^eps*D / d eps =
        //
        //  [ 0_{3x3}  -R(A)*[dc1]_x ]
        //  [ 0_{3x3}  -R(A)*[dc2]_x ]
        //  [ 0_{3x3}  -R(A)*[dc3]_x ]
        //  [ R(A)     -R(A)*[dt]_x  ]
        //
        Eigen::Matrix<double,12,6>  dAeD_de;
        // Blocks: (1-3,1)
        dAeD_de.block<9,3>(0,0).setZero();
        // Block (4,1)
        dAeD_de.block<3,3>(9,0) = (ROTA * HM_D.block<3,3>(0,0)).transpose();

        //cout << "========= ROTA ========= :\n" << ROTA << endl;
        //cout << "========= HM_D ========= :\n" << HM_D << endl;

        // Blocks (1-4,2)
        for (int i=0;i<4;i++)
        {
            EIGEN_ALIGN16 const double aux_vals[] = {
                        .0, -HM_D(2,i),  HM_D(1,i),
                 HM_D(2,i),         .0, -HM_D(0,i),
                -HM_D(1,i),  HM_D(0,i),         .0  };
            dAeD_de.block<3,3>(i*3,3) = -ROTA * mrpt::math::CMatrixDouble33(aux_vals);
        }

        // (3): Apply chain rule:
        //
        jacob.noalias() = dLnRelPose_deps * dAeD_de;

        if (is_inverse_edge_jacobian)
        {
            // And this comes from: d exp(-epsilon)/d epsilon = - d exp(epsilon)/d epsilon
            jacob = -jacob;
        }
    }
}; // end of "compute_jacobian_dAepsDx_deps", Case: SE(3) relative-poses, SE(3) poses:



// ====================================================================
//                       j,i                    lm_id,base_id
//           \partial  h            \partial  h
//                       l                      obs_frame_id
// dh_df = ------------------ = ---------------------------------
//
//           \partial  f            \partial  f
//
//   Note: f=relative position of landmark with respect to its base kf
// ====================================================================
template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::compute_jacobian_dh_df(
    typename TSparseBlocksJacobians_dh_df::TEntry  &jacob,
    const k2f_edge_t & observation,
    std::vector<const pose_flag_t*> *out_list_of_required_num_poses) const
{
    MRPT_UNUSED_PARAM(observation);
    if (! *jacob.sym.is_valid )
        return; // Another block of the same Jacobian row said this observation was invalid for some reason.

    // Handle the special case when obs==base, for which rel_pose_base_from_obs==NULL
    const pose_flag_t * rel_pose_base_from_obs = jacob.sym.rel_pose_base_from_obs;

    if (out_list_of_required_num_poses && rel_pose_base_from_obs)
        out_list_of_required_num_poses->push_back(jacob.sym.rel_pose_base_from_obs);

    // make sure the numeric spanning tree is working and updating all that we need:
    if (rel_pose_base_from_obs)
    {
#if DEBUG_NOT_UPDATED_ENTRIES
        TNumSTData d1 = check_num_st_entry_exists(rel_pose_base_from_obs, rba_state.spanning_tree);
#endif
        if (!rel_pose_base_from_obs->updated)
        {
#if DEBUG_NOT_UPDATED_ENTRIES
            cout << "not updated ST entry for: from=" << d1.from << ", to=" << d1.to << endl;
#endif
            rba_state.spanning_tree.save_as_dot_file("_debug_jacob_error_all_STs.dot");
            ASSERT_(rel_pose_base_from_obs->updated)
        }
    }

    // First, we need x^{j,i}_i:
    //const mrpt::math::TPoint3D & xji_i = jacob.sym.feat_rel_pos->pos;
    const array_landmark_t &xji_i = jacob.sym.feat_rel_pos->pos;

    array_landmark_t xji_l = xji_i; //

    if (rel_pose_base_from_obs!=NULL)
    {
#if 0
        cout << "dh_df(ft_id="<< observation.obs.obs.feat_id << ", obs_kf="<< observation.obs.kf_id << "): o2b=" << *rel_pose_base_from_obs << endl;
#endif
        // xji_l = rel_pose_base_from_obs (+) xji_i
        //rel_pose_base_from_obs->pose.composePoint(xji_i, xji_l);
        landmark_t::composePosePoint(xji_l, rel_pose_base_from_obs->pose);
    }
    else
    {
        // I'm observing from the same base key-frame: xji_l = xji_i (already done above)
    }


#if SRBA_COMPUTE_NUMERIC_JACOBIANS
    // Numeric jacobians
    typename TSparseBlocksJacobians_dh_df::matrix_t  num_jacob;

    array_landmark_t x;
    x.setZero(); // Evaluate Jacobian at incr around origin
    array_landmark_t x_incrs;
    x_incrs.setConstant(1e-3);

    const TNumeric_dh_df_params num_params(&rel_pose_base_from_obs->pose,jacob.sym.feat_rel_pos->pos,this->parameters.sensor,this->parameters.sensor_pose);

    mrpt::math::jacobians::jacob_numeric_estimate(x,&numeric_dh_df,x_incrs,num_params,num_jacob);

#endif // SRBA_COMPUTE_NUMERIC_JACOBIANS


#if SRBA_COMPUTE_ANALYTIC_JACOBIANS
    //  d h(x^{j,i}_l)    d h(x')       d x'
    // --------------- = --------- * ----------
    //  d f                d x'         d x
    //
    //                  With: x' = x^{j,i}_l

    // First jacobian: (uses xji_l)
    // -----------------------------
    Eigen::Matrix<double,OBS_DIMS,LM_DIMS>  dh_dx;

    // Converts a point relative to the robot coordinate frame (P) into a point relative to the sensor (RES = P \ominus POSE_IN_ROBOT )
    RBA_OPTIONS::sensor_pose_on_robot_t::template point_robot2sensor<landmark_t,array_landmark_t>(xji_l,xji_l,this->parameters.sensor_pose );

    // Invoke sensor model:
    if (!sensor_model_t::eval_jacob_dh_dx(dh_dx,xji_l, this->parameters.sensor))
    {
        // Invalid Jacobian:
        *jacob.sym.is_valid = 0;
        jacob.num.setZero();
        return;
    }

    // take into account the possible displacement of the sensor wrt the keyframe:
    RBA_OPTIONS::sensor_pose_on_robot_t::jacob_dh_dx_rotate( dh_dx, this->parameters.sensor_pose );

    // Second Jacobian: Simply the 2x2 or 3x3 rotation matrix of base wrt observing
    // ------------------------------
    if (rel_pose_base_from_obs!=NULL)
    {
        mrpt::math::CMatrixFixedNumeric<double,LM_DIMS,LM_DIMS> R(mrpt::math::UNINITIALIZED_MATRIX);
        rel_pose_base_from_obs->pose.getRotationMatrix(R);
        jacob.num.noalias() = dh_dx * R;
    }
    else
    {
        // if observing from the same base kf, we're done:
        jacob.num.noalias() = dh_dx;
    }
#endif // SRBA_COMPUTE_ANALYTIC_JACOBIANS


#if SRBA_VERIFY_AGAINST_NUMERIC_JACOBIANS
    // Check jacob.num vs. num_jacob
    const double MAX_REL_ERROR = 0.1;
    if ((jacob.num-num_jacob).array().abs().maxCoeff()>MAX_REL_ERROR*num_jacob.array().maxCoeff())
    {
        std::cerr << "NUMERIC VS. ANALYTIC JACOBIAN dh_df FAILED:"
            << "\njacob.num:\n" << jacob.num
            << "\nnum_jacob:\n" << num_jacob
            << "\nDiff:\n" << jacob.num-num_jacob << endl << endl;
    }
#endif

#if SRBA_USE_NUMERIC_JACOBIANS
    jacob.num = num_jacob;
#endif
}


// ------------------------------------------------------------------------
//   prepare_Jacobians_required_tree_roots()
// ------------------------------------------------------------------------
template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::prepare_Jacobians_required_tree_roots(
    std::set<TKeyFrameID>  & lst,
    const std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> &lst_JacobCols_dAp,
    const std::vector<typename TSparseBlocksJacobians_dh_df::col_t*>  &lst_JacobCols_df )
{
    lst.clear();

    const size_t nUnknowns_k2k = lst_JacobCols_dAp.size();
    const size_t nUnknowns_k2f = lst_JacobCols_df.size();

    // k2k edges ------------------------------------------------------
    for (size_t i=0;i<nUnknowns_k2k;i++)
    {
        // For each column, process each nonzero block:
        const typename TSparseBlocksJacobians_dh_dAp::col_t *col = lst_JacobCols_dAp[i];

        for (typename TSparseBlocksJacobians_dh_dAp::col_t::const_iterator it=col->begin();it!=col->end();++it)
        {
            // For each dh_dAp block we need:
            //  *  (d+1) -> obs
            //  *  (d+1) -> base

            const size_t obs_idx = it->first;
            const typename TSparseBlocksJacobians_dh_dAp::TEntry & jacob_entry = it->second;

            const TKeyFrameID obs_id  = rba_state.all_observations[obs_idx].obs.kf_id;
            const TKeyFrameID d1_id   = jacob_entry.sym.kf_d;
            const TKeyFrameID base_id = jacob_entry.sym.kf_base;

            add_edge_ij_to_list_needed_roots(lst, d1_id  , obs_id );
            add_edge_ij_to_list_needed_roots(lst, base_id, d1_id );
        }
    }

    // k2f edges ------------------------------------------------------
    for (size_t i=0;i<nUnknowns_k2f;i++)
    {
        // For each column, process each nonzero block:
        const typename TSparseBlocksJacobians_dh_df::col_t *col = lst_JacobCols_df[i];

        for (typename TSparseBlocksJacobians_dh_df::col_t::const_iterator it=col->begin();it!=col->end();++it)
        {
            // For each dh_df block we need:
            //  *  obs -> base
            const size_t obs_idx = it->first;
            const k2f_edge_t &k2f =
            rba_state.all_observations[obs_idx];

            ASSERT_(k2f.feat_rel_pos)

            const TKeyFrameID obs_id  = k2f.obs.kf_id;
            const TKeyFrameID base_id = k2f.feat_rel_pos->id_frame_base;

            add_edge_ij_to_list_needed_roots(lst, base_id, obs_id );
        }
    }
}


// ------------------------------------------------------------------------
//   recompute_all_Jacobians(): Re-evaluate all Jacobians numerically
// ------------------------------------------------------------------------
template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
size_t RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::recompute_all_Jacobians(
    std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> &lst_JacobCols_dAp,
    std::vector<typename TSparseBlocksJacobians_dh_df::col_t*>  &lst_JacobCols_df,
    std::vector<const typename kf2kf_pose_traits<kf2kf_pose_t>::pose_flag_t*>    * out_list_of_required_num_poses )
{
    size_t nJacobs=0;
    if (out_list_of_required_num_poses) out_list_of_required_num_poses->clear();

    const size_t nUnknowns_k2k = lst_JacobCols_dAp.size();

    // k2k edges ------------------------------------------------------
    for (size_t i=0;i<nUnknowns_k2k;i++)
    {
        // For each column, process each nonzero block:
        typename TSparseBlocksJacobians_dh_dAp::col_t *col = lst_JacobCols_dAp[i];

        for (typename TSparseBlocksJacobians_dh_dAp::col_t::iterator it=col->begin();it!=col->end();++it)
        {
            const size_t obs_idx = it->first;
            typename TSparseBlocksJacobians_dh_dAp::TEntry & jacob_entry = it->second;
            compute_jacobian_dh_dp(
                jacob_entry,
                rba_state.all_observations[obs_idx],
                rba_state.k2k_edges,
                out_list_of_required_num_poses );
            nJacobs++;
        }
    }

    // k2f edges ------------------------------------------------------
    // Only if we are in landmarks-based SLAM, not in graph-SLAM:
    nJacobs += internal::recompute_all_Jacobians_dh_df<landmark_t::jacob_family>::eval(*this, lst_JacobCols_df,out_list_of_required_num_poses);

    return nJacobs;
} // end of recompute_all_Jacobians()


} // end of namespace