add-observations.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     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

namespace mrpt { namespace srba {

#define OBS_SUPER_VERBOSE   0

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>::add_observation(
        const TKeyFrameID            observing_kf_id,
        const typename observation_traits<OBS_TYPE>::observation_t     & new_obs,
        const array_landmark_t * fixed_relative_position,
        const array_landmark_t * unknown_relative_position_init_val
        )
{
        m_profiler.enter("add_observation");

        ASSERT_( !( fixed_relative_position!=NULL && unknown_relative_position_init_val!=NULL) ) // Both can't be !=NULL at once.

        const bool is_1st_time_seen = ( new_obs.feat_id>=rba_state.all_lms.size() || rba_state.all_lms[new_obs.feat_id].rfp==NULL );

        const bool is_fixed =
                // This is the first observation of a fixed landmark:
                (fixed_relative_position!=NULL)
                || // or if it was observed before, get its feature type from stored structure:
                (!is_1st_time_seen && rba_state.all_lms[new_obs.feat_id].has_known_pos );

        // Append all new observation to raw vector:
        // ---------------------------------------------------------------------
        const size_t new_obs_idx = rba_state.all_observations.size();    // O(1)

#ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
        rba_state.all_observations.push_back(stlplus::smart_ptr<k2f_edge_t>(new k2f_edge_t)); // Create new k2f_edge -- O(1)
#else
        rba_state.all_observations.push_back(k2f_edge_t()); // Create new k2f_edge -- O(1)
#endif

        rba_state.all_observations_Jacob_validity.push_back(1);  // Also grow this vector (its content now are irrelevant, they'll be updated in optimization)
        char * const jacob_valid_bit = &(*rba_state.all_observations_Jacob_validity.rbegin());

        // Get a ref. to observation info, filled in below:
#ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG
        k2f_edge_t & new_k2f_edge = *(*rba_state.all_observations.rbegin());
#else
        k2f_edge_t & new_k2f_edge = *rba_state.all_observations.rbegin();
#endif

        // New landmark? Update LMs structures if this is the 1st time we see this landmark:
        // -----------------------------------------------------------------------
        if (is_1st_time_seen)
        {
                if (is_fixed)
                {       // LM with KNOWN relative position.
                        // ----------------------------------
                        TRelativeLandmarkPos new_rfp;
                        new_rfp.id_frame_base = observing_kf_id;  // The current KF at creation time becomes the relative coordinate base.
                        new_rfp.pos = *fixed_relative_position;

                        // O(1) insertion if the obs.feat_id is the largest until now:
                        typename TRelativeLandmarkPosMap::iterator it_new = rba_state.known_lms.insert(
                                rba_state.known_lms.end(),
                                typename TRelativeLandmarkPosMap::value_type(new_obs.feat_id, new_rfp )
                                );

                        // Add to list of all LMs:   Amortized O(1)
                        if (new_obs.feat_id >= rba_state.all_lms.size()) rba_state.all_lms.resize(new_obs.feat_id+1);
                        rba_state.all_lms[new_obs.feat_id] = typename landmark_traits<LM_TYPE>::TLandmarkEntry(true /*known pos.*/, &it_new->second);
                }
                else
                {       // LM with UNKNOWN relative position.
                        // ----------------------------------
                        // O(1) insertion if the feat_ID is the largest until now:

                        TRelativeLandmarkPos new_rfp;
                        new_rfp.id_frame_base = observing_kf_id;  // The current KF at creation time becomes the relative coordinate base.
                        if (unknown_relative_position_init_val)
                             new_rfp.pos =  *unknown_relative_position_init_val;
                        else {
                                // Default landmark position: Invoke sensor's inverse model.
                                sensor_model_t::inverse_sensor_model(new_rfp.pos,new_obs.obs_data,this->parameters.sensor);

                                // Take into account the sensor pose wrt the KF:
                                RBA_OPTIONS::sensor_pose_on_robot_t::template sensor2robot_point<LM_TYPE>(new_rfp.pos, this->parameters.sensor_pose );
                        }

                        typename TRelativeLandmarkPosMap::iterator it_new = rba_state.unknown_lms.insert(
                                rba_state.unknown_lms.end(),
                                typename TRelativeLandmarkPosMap::value_type( new_obs.feat_id, new_rfp )
                                );

                        // Add to list of all LMs:
                        if (new_obs.feat_id >= rba_state.all_lms.size()) rba_state.all_lms.resize(new_obs.feat_id+1);
                        rba_state.all_lms[new_obs.feat_id] = typename landmark_traits<LM_TYPE>::TLandmarkEntry(false /*unknown pos.*/, &it_new->second);

                        // Expand Jacobian dh_df to accomodate a new column for a new unknown:
                        // ("Remap indices" in dh_df for each column are the feature IDs of those feature with unknown positions)
                        rba_state.lin_system.dh_df.appendCol( new_obs.feat_id );
                }
        }

        // Maintain a pointer to the relative position wrt its base keyframe:
        TRelativeLandmarkPos *lm_rel_pos = rba_state.all_lms[new_obs.feat_id].rfp;
        // and get the ID of the base keyframe for the observed landmark:
        const TKeyFrameID base_id = lm_rel_pos->id_frame_base;

        // Fill in kf-to-feature edge data:
        // ------------------------------------
        new_k2f_edge.obs.kf_id = observing_kf_id;
        new_k2f_edge.obs.obs   = new_obs;
        new_k2f_edge.obs.obs.obs_data.getAsArray(new_k2f_edge.obs.obs_arr);  // Save the observation data as an array now, only once, and reuse it from now on in optimizations, etc.
        new_k2f_edge.is_first_obs_of_unknown = is_1st_time_seen && !is_fixed;
        new_k2f_edge.feat_has_known_rel_pos  = is_fixed;
        new_k2f_edge.feat_rel_pos = lm_rel_pos;


        // Update keyframe incident edge list:
        // ---------------------------------------------------------------------
        ASSERTDEB_( observing_kf_id < rba_state.keyframes.size() )

        keyframe_info &kf_info = rba_state.keyframes[ observing_kf_id ];   // O(1)

        // Incident edges:
        kf_info.adjacent_k2f_edges.push_back( &new_k2f_edge );   // Amortized O(1)


        // Update linear system:

        //  If the observed feat has a known rel. pos., only dh_dAp; otherwise, both dh_dAp and dh_df
        // ---------------------------------------------------------------------
        // Add a new (block) row for this new observation (row index = "new_obs_idx" defined above)
        // We must create a block for each edge in between the observing and the ref. base id.
        // Note: no error checking here in find's for efficiency...
        m_profiler.enter("add_observation.jacobs.sym");

        // ===========================
        // Jacob 1/2: dh_dAp
        // ===========================
        bool graph_says_ignore_this_obs = false;

        if (base_id!=new_k2f_edge.obs.kf_id) // If this is a feat with unknown rel.pos. and it's its first observation, the dh_dAp part of the Jacobian is empty.
        {
#if OBS_SUPER_VERBOSE
                std::cout << "Jacobian dh_dAp for obs #"<<new_obs_idx << " has these edges [obs_kf="<<observing_kf_id<< " base_kf="<< base_id<<"]:\n";
#endif

                // Since "all_edges" is symmetric, only the (i,j), i>j entries are stored:
                const TKeyFrameID from = std::max(observing_kf_id, base_id);
                const TKeyFrameID to   = std::min(observing_kf_id, base_id);

                const bool all_edges_inverse = (from!=observing_kf_id);

                typename rba_problem_state_t::TSpanningTree::all_edges_maps_t::const_iterator it_map = rba_state.spanning_tree.sym.all_edges.find(from);  // Was: observing
                ASSERTMSG_(it_map != rba_state.spanning_tree.sym.all_edges.end(), mrpt::format("No ST.all_edges found for observing_id=%u, base_id=%u", static_cast<unsigned int>(observing_kf_id), static_cast<unsigned int>(base_id) ) )

                typename std::map<TKeyFrameID, typename rba_problem_state_t::k2k_edge_vector_t >::const_iterator it_obs_ed = it_map->second.find(to);
                //ASSERTMSG_(it_obs_ed != it_map->second.end(), mrpt::format("No spanning-tree found from KF #%u to KF #%u, base of observation of landmark #%u", static_cast<unsigned int>(observing_kf_id),static_cast<unsigned int>(base_id),static_cast<unsigned int>(new_obs.feat_id) ))

                if (it_obs_ed != it_map->second.end())
                {
                        const typename rba_problem_state_t::k2k_edge_vector_t & obs_edges = it_obs_ed->second;
                        ASSERT_(!obs_edges.empty())

                        TKeyFrameID curKF = observing_kf_id; // The backward running index towards "base_id"
                        for (size_t j=0;j<obs_edges.size();j++)
                        {
                                const size_t i = all_edges_inverse ?  (obs_edges.size()-j-1) : j;

                                ASSERT_(curKF!=to)

                                const size_t edge_id = obs_edges[i]->id;

                                // Normal direction: from -> to, so "to"=="curKF" (we go backwards)
                                const bool normal_dir = (obs_edges[i]->to==curKF);

#if OBS_SUPER_VERBOSE
                                std::cout << " * edge #"<<edge_id<< ": "<<obs_edges[i]->from <<" => "<<obs_edges[i]->to << " (inverse: " << (normal_dir ? "no":"yes") << ")\n";
#endif

                                // Get sparse block column:
                                typename TSparseBlocksJacobians_dh_dAp::col_t & col = rba_state.lin_system.dh_dAp.getCol(edge_id);

                                // Create new entry: O(1) optimized insert if obs_idx is the largest index (as it will normally be):
                                typename TSparseBlocksJacobians_dh_dAp::col_t::iterator it_new_entry = col.insert(
                                        col.end(),
                                        typename TSparseBlocksJacobians_dh_dAp::col_t::value_type( new_obs_idx, typename TSparseBlocksJacobians_dh_dAp::TEntry() )
                                        );

                                typename TSparseBlocksJacobians_dh_dAp::TEntry & entry = it_new_entry->second;

                                entry.sym.obs_idx          = new_obs_idx;
                                entry.sym.is_valid         = jacob_valid_bit;
                                entry.sym.edge_normal_dir  = normal_dir;
                                entry.sym.kf_d             = curKF;
                                entry.sym.kf_base          = base_id;
                                entry.sym.feat_rel_pos     = lm_rel_pos;
                                entry.sym.k2k_edge_id      = edge_id;
                                //entry.sym.rel_poses_path   = &obs_edges;  // Path OBSERVING_KF -> BASE_KF

                                // Pointers to placeholders of future numeric results of the spanning tree:
                                entry.sym.rel_pose_base_from_d1 = & rba_state.spanning_tree.num[curKF][base_id];
                                entry.sym.rel_pose_d1_from_obs  =
                                        (curKF==observing_kf_id) ?
                                                NULL // Use special value "NULL" when the CPose is fixed to the origin.
                                                :
                                                & rba_state.spanning_tree.num[observing_kf_id][curKF];

                                // next node after this edge is:
                                curKF = normal_dir ? obs_edges[i]->from : obs_edges[i]->to;
                        }
                }
                else
                {
                        // Ignore this obs...
                        graph_says_ignore_this_obs=true;
                }
        }

        // ===========================
        // Jacob 2/2: dh_df
        // ===========================
        if (!is_fixed && !graph_says_ignore_this_obs) // Only for features with unknown rel.pos.
        {
                // "Remap indices" in dh_df for each column are the feature IDs of those feature with unknown positions.
                const size_t remapIdx = new_k2f_edge.obs.obs.feat_id;

                const mrpt::utils::map_as_vector<size_t,size_t> &dh_df_remap = rba_state.lin_system.dh_df.getColInverseRemappedIndices();
                const mrpt::utils::map_as_vector<size_t,size_t>::const_iterator it_idx = dh_df_remap.find(remapIdx);  // O(1) in mrpt::utils::map_as_vector()
                ASSERT_(it_idx!=dh_df_remap.end())

                const size_t col_idx = it_idx->second;

#if OBS_SUPER_VERBOSE
                cout << "dh_df: col_idx=" << col_idx << " feat_id=" << remapIdx << " obs_idx=" << new_obs_idx <<  endl;
#endif
                // Get sparse block column:
                typename TSparseBlocksJacobians_dh_df::col_t & col = rba_state.lin_system.dh_df.getCol(col_idx);

                // Create new entry: O(1) optimized insert if obs_idx is the largest index (as it will normally be):
                typename TSparseBlocksJacobians_dh_df::col_t::iterator it_new_entry = col.insert(
                        col.end(),
                        typename TSparseBlocksJacobians_dh_df::col_t::value_type( new_obs_idx, typename TSparseBlocksJacobians_dh_df::TEntry() )
                        );

                typename TSparseBlocksJacobians_dh_df::TEntry & entry = it_new_entry->second;

                entry.sym.obs_idx   = new_obs_idx;
                entry.sym.is_valid  = jacob_valid_bit;

                // Pointer to relative position:
                entry.sym.feat_rel_pos = lm_rel_pos;

                // Pointers to placeholders of future numeric results of the spanning tree:
                entry.sym.rel_pose_base_from_obs  =
                        (new_k2f_edge.is_first_obs_of_unknown) ?
                                NULL // Use special value "NULL" when the CPose is fixed to the origin.
                                :
                                & rba_state.spanning_tree.num[observing_kf_id][base_id];
        }

        m_profiler.leave("add_observation.jacobs.sym");

        m_profiler.leave("add_observation");

        return new_obs_idx;
}

} } // end NS