algorithms.hpp

#ifndef ALGORITHMS_HPP
#define ALGORITHMS_HPP

#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <graphmap/transformation.hpp>
#include <graphmap/graphmapException.hpp>

#include <rtslam/quatTools.hpp>

#include <iostream>
#include <algorithm>

namespace jafar{
namespace graphmap {

template <typename OutputIterator, typename OutputIteratorValueType>
class path_to_back_edge_recorder : public boost::default_dfs_visitor
{
public:
  path_to_back_edge_recorder(OutputIterator out) :
    m_out(out), m_path()
  { }

  template <class Edge, class Graph>
  void tree_edge(Edge e, Graph&) { m_path.push_back(e); }
  template <class Vertex, class Graph>
  void finish_vertex(Vertex, Graph&){ if(!m_path.empty()) m_path.pop_back();  }
  template <class Edge, class Graph>
  void back_edge(Edge e, Graph&)
  {
    if(e == m_path.back()) return; // avoid going back through the same undirected edge

    m_path.push_back(e);
    *m_out++ = m_path;
    m_path.pop_back();
  }

private:
  OutputIterator m_out;
  OutputIteratorValueType m_path;
};

//object generator function
template <typename BackInsertionSequence>
path_to_back_edge_recorder< std::back_insert_iterator<BackInsertionSequence>, typename BackInsertionSequence::value_type >
make_path_to_back_edge_recorder(BackInsertionSequence& backinsertersequence) {
  return path_to_back_edge_recorder<std::back_insert_iterator<BackInsertionSequence>, typename BackInsertionSequence::value_type >(std::back_inserter(backinsertersequence));
}

template <typename OutputIterator, typename OutputIteratorValueType, typename Vertex>
class path_to_target_recorder : public boost::default_dfs_visitor
{
public:
  path_to_target_recorder(OutputIterator out, Vertex target) :
    m_out(out), m_path(), m_target(target)
  { }

  template <class Edge, class Graph>
  void tree_edge(Edge e, Graph&) { m_path.push_back(e); }
  template <class Graph>
  void finish_vertex(Vertex, Graph&){ if(!m_path.empty()) m_path.pop_back();  }
  template <class Graph>
  void discover_vertex(Vertex u, Graph& g) {
    if(u == m_target){
      *m_out++ = m_path;
    }
  }
private:
  OutputIterator m_out;
  OutputIteratorValueType m_path;
  Vertex m_source, m_target;
};

//object generator function
template <typename BackInsertionSequence, typename Vertex>
path_to_target_recorder< std::back_insert_iterator<BackInsertionSequence>, typename BackInsertionSequence::value_type, Vertex >
make_path_to_target_recorder(BackInsertionSequence& backinsertersequence, Vertex target) {
  return path_to_target_recorder<std::back_insert_iterator<BackInsertionSequence>, typename BackInsertionSequence::value_type, Vertex >(std::back_inserter(backinsertersequence),target);
}


template<typename Loop,typename Graph>
void tag_loop(Loop &loop, Graph &g)
{
  typename Loop::iterator it = loop.begin();
  std::vector<unsigned int> common_ids, intersection; 

  common_ids = g[(*it)].loop_ids; it++;

  for( ; it != loop.end(); ++it)
  {
    std::set_intersection (g[(*it)].loop_ids.begin(),g[(*it)].loop_ids.end(),
                 common_ids.begin(),common_ids.end(),
                 std::back_inserter(intersection));
    if(intersection.empty())  // No common loop id: assign a new one and return
    {
      unsigned int new_id = g[boost::graph_bundle].loopIdFactory.getId();
      JFR_DEBUG("New loop! ID: " << new_id << std::endl);
      for(typename Loop::iterator it2  = loop.begin(); it2 != loop.end(); ++it2)
        g[(*it2)].loop_ids.push_back(new_id);
      return;
    }else
    {
      common_ids = intersection;
      intersection.clear();
    }
  }

  JFR_DEBUG_COND(common_ids.size() == 1,"Old loop... ID: " << common_ids.front() << std::endl);
  JFR_PRED_ERROR(common_ids.size() != 1, GraphmapException, GraphmapException::LOOP_WITH_MULTIPLE_IDS, "loop has " << common_ids.size() << " ids");
}

template <typename Path, typename Graph>
void compute_loop_from_path(Path& path, Graph& g)
{
  using namespace boost;

  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::edge_descriptor Edge;

  Edge back_edge = path.back();
  Vertex loop_head = target(back_edge,g);

  if(source(path.front(),g) != loop_head){
    typename Path::iterator it;
    for(it = path.begin(); it != path.end(); ++it)
      if(source(*it,g) == loop_head){
        path.erase(path.begin(),it); //Erase all edges that do not belong to the loop
        return;
      }
  }
}

template <typename Graph, typename Loops>
void find_all_loops(
    typename boost::graph_traits<Graph>::vertex_descriptor entry,
    Graph& g,
    Loops& loops) // A container of sets of edges
{
  using namespace boost;

  function_requires< BidirectionalGraphConcept<Graph> >();
  typedef typename graph_traits<Graph>::edge_descriptor Edge;
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

  //std::vector<default_color_type> color_map(num_vertices(g));

  // Find paths from the entry vertex to the back edges in the graph
  depth_first_search(g,visitor(make_path_to_back_edge_recorder(loops)));
  //undirected_dfs(g,visitor(make_path_to_back_edge_recorder(loops)));

  // Find all the vertices in each loop
  for(typename Loops::iterator it = loops.begin();
    it != loops.end(); ++it)
  {
    compute_loop_from_path(*it,g);
    tag_loop(*it,g);
  }


}

template <typename Graph, typename Loops>
void find_loops_from_edge(
    typename boost::graph_traits<Graph>::edge_descriptor entry_edge,
    Graph& g,
    Loops& loops) // A container of sets of edges
{
  using namespace boost;

  function_requires< BidirectionalGraphConcept<Graph> >();
  typedef typename graph_traits<Graph>::edge_descriptor Edge;
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::edge_iterator EdgeIterator;
  typedef adjacency_list<listS, vecS, undirectedS> UndirectedGraph;
  typedef typename graph_traits<UndirectedGraph>::edge_descriptor UndirectedEdge;
  typedef typename graph_traits<UndirectedGraph>::vertex_descriptor UndirectedVertex;
  typedef typename std::list<UndirectedEdge> UndirectedEdgeList;
  typedef typename std::vector<UndirectedEdgeList> UndirectedLoops;

  UndirectedLoops und_loops;
  UndirectedGraph undirected_g;
  std::map<Edge,UndirectedEdge> direct_to_undirect_edge_map;
  std::map<UndirectedEdge,Edge> undirect_to_direct_edge_map;

  // copy the directed graph to a undirected one
  EdgeIterator ei, ei_end;
  UndirectedEdge und_e;
  for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei){
    tie(und_e,boost::tuples::ignore) = add_edge(source(*ei, g),target(*ei, g),undirected_g);
    direct_to_undirect_edge_map[*ei] = und_e;
    undirect_to_direct_edge_map[und_e] = *ei;
  }

  // record the source and target vertexes in the undirected graph
  UndirectedVertex source_v = source(direct_to_undirect_edge_map[entry_edge],undirected_g);
  UndirectedVertex target_v = target(direct_to_undirect_edge_map[entry_edge],undirected_g);

  // remove the entry edge from the undirected graph
  remove_edge(direct_to_undirect_edge_map[entry_edge],undirected_g);

  // search for a path from source to target vertexes
  std::vector < default_color_type > color_map(num_vertices(undirected_g));
  depth_first_visit(undirected_g, source_v,
            make_path_to_target_recorder(und_loops,target_v),
            make_iterator_property_map(color_map.begin(),
                         get(vertex_index, undirected_g), color_map[0]));

  // convert the loop from undirected to directed, adding the entry edge
  for(typename UndirectedLoops::iterator it = und_loops.begin();
    it != und_loops.end(); ++it)
  {
    typename Loops::value_type directed_loop;
    for(typename UndirectedEdgeList::iterator it2 = it->begin();
      it2 != it->end(); ++it2)
    {
      directed_loop.push_back(undirect_to_direct_edge_map[*it2]);
    }
    directed_loop.push_back(entry_edge);
    loops.push_back(directed_loop);
  }

  // Tag the loops
  for(typename Loops::iterator it = loops.begin(); it != loops.end(); ++it)
  {
    tag_loop(*it,g);
  }

}

template <typename TransformationPairVector>
void optimize(TransformationPairVector &t_pair_vector, double threshold)
{
  unsigned int loop_size = t_pair_vector.size();

  // Define the optimization vector size
  int t_size = t_pair_vector[0].first.size();
  int opt_vec_size = t_size * loop_size;

  jblas::vec x_u(opt_vec_size); // vector of original transformations
  jblas::sym_mat P_u(opt_vec_size,opt_vec_size); // matrix of original covariances OBS: need to be sym_mat to avoid ambiguity using prod_JPJt
  jblas::vec x_prev(opt_vec_size); // vector of optimazed transformations
  jblas::vec x_pos(opt_vec_size); // vector of optimazed transformations

  // Initialize x_u, P_u, x_i = x_0 and h_i = h_0
  for(unsigned int i=0; i < loop_size; ++i)
  {
    ublas::subrange(x_u,i*t_size,(i+1)*t_size) = t_pair_vector[i].first.x();
    ublas::subrange(P_u,i*t_size,(i+1)*t_size,i*t_size,(i+1)*t_size) = t_pair_vector[i].first.P();
    ublas::subrange(x_prev,i*t_size,(i+1)*t_size) = t_pair_vector[i].second.x();
  }

  bool converged = false;

  // Jacobian of h evaluated in x_i
  jblas::mat H_i(t_size,opt_vec_size);

  // Auxiliary variables to calculate x_i
  jblas::vec a(opt_vec_size);
  jblas::vec b(opt_vec_size);
  jblas::vec c(t_size);
  jblas::mat d(t_size,t_size);
  jblas::mat d_inv(t_size,t_size);
  jblas::mat e(opt_vec_size,t_size);
  jblas::mat f(opt_vec_size,t_size);
  jblas::vec g(opt_vec_size);

  jblas::vec h_i = jblas::zero_vec(t_size); // composition of transformations
  jblas::vec h_k(t_size); // incremental composition of transformations (used to calculate H_i)
  jblas::vec x_k(t_size); // transformation of one specific edge

  jblas::mat J_1(t_size,t_size), J_2(t_size,t_size); // Jacobians used to evaluate H_i

  // dummy variables used to be able to get the right jacobians in H_i evaluation
  jblas::mat dummy_J(t_size,t_size);
  jblas::vec dummy_C(t_size);

  // temporary variables used to evaluate H_i
  jblas::vec h_k_inv(t_size), h_k_inv_h_i(t_size), x_k_inv(t_size), h_k_x_k_inv(t_size);

  while(!converged){
    // evaluate h_i
    for(unsigned int i=0; i < loop_size; ++i){
      x_k = ublas::subrange(x_prev,i*t_size,(i+1)*t_size);
      h_i = rtslam::quaternion::composeFrames(h_i, x_k);
    }
    // evaluate H_i
    h_k = jblas::zero_vec(t_size);
    for(unsigned int i=0; i < t_pair_vector.size(); ++i)
    {
      x_k = ublas::subrange(x_prev,i*t_size,(i+1)*t_size);
      h_k = rtslam::quaternion::composeFrames(h_k,x_k);

      h_k_inv = rtslam::quaternion::invertFrame(h_k);
      h_k_inv_h_i = rtslam::quaternion::composeFrames(h_k_inv,h_i);
      rtslam::quaternion::composeFrames(h_k, h_k_inv_h_i, dummy_C, J_1, dummy_J); // get J_1

      x_k_inv = rtslam::quaternion::invertFrame(x_k);
      h_k_x_k_inv = rtslam::quaternion::composeFrames(h_k,x_k_inv);
      rtslam::quaternion::composeFrames(h_k_x_k_inv, x_k,dummy_C, dummy_J, J_2); // get J_2

      ublas::subrange(H_i,0,t_size,i*t_size,(i+1)*t_size) = ublas::prod(J_1,J_2); // put dh/dx_i in the correct position in H_i
    }

    // evaluate x_( i+1 ) = x_u + P_u*(H_i.transp()) * (H_i*P_u*H_I.transp()).inv() * (H_i*(x_i - x_u) - h_i)
    //                            \_______e________/   \__________d_________/              \____a____/
    //                                                 \__________d_inv___________/   \_______b______/
    //                            \________________________f______________________/   \___________c_________/
    //                            \_________________________________g_______________________________________/
    //                      \__________________________________x_pos________________________________________/
    a = x_prev - x_u;
    b = ublas::prod(H_i,a);
    c = b - h_i;
    d = jmath::ublasExtra::prod_JPJt(P_u,H_i);
    jmath::ublasExtra::lu_inv(d, d_inv);
    e = ublas::prod(P_u,ublas::trans(H_i));
    f = ublas::prod(e,d_inv);
    g = ublas::prod(f,c);
    x_pos = x_u + g;

    if( ublas::norm_2(x_pos - x_prev) < threshold)
      converged = true;

    x_prev = x_pos;
  }

  // store optimized transformations in the edges
  for(unsigned int i=0; i < t_pair_vector.size(); ++i)
  {
    t_pair_vector[i].second.x() = ublas::subrange(x_pos,i*t_size,(i+1)*t_size);
//    t_pair_vector[i].second.P() = ???; //Is it needed?
  }
}

template<class VecG, class VecL, class VecC, class CovMatG, class CovMatL, class CovMatC>
void composeFramesWithCov(const VecG & G, const VecL & L, VecC & C,
              const CovMatG & PG, const CovMatL & PL, CovMatC & PC){

  jblas::mat C_g(7,7), C_l(7,7);
  rtslam::quaternion::composeFrames(G, L, C, C_g, C_l);
  PC = jmath::ublasExtra::prod_JPJt(PG,C_g) +
     jmath::ublasExtra::prod_JPJt(PL,C_l);
}

} // namespace graphmap
} // namespace jafar

#endif // ALGORITHMS_HPP