gtsam/nonlinear/LieValues-inl.h

/**
 * @file LieValues.cpp
 * @author Richard Roberts
 */

#pragma once

#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <utility>
#include <iostream>
#include <stdexcept>

#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h>

#include <gtsam/nonlinear/LieValues.h>

#define INSTANTIATE_LIE_CONFIG(J) \
  /*INSTANTIATE_LIE(T);*/ \
  /*template LieValues<J> expmap(const LieValues<J>&, const VectorValues&);*/ \
  /*template LieValues<J> expmap(const LieValues<J>&, const Vector&);*/ \
  /*template VectorValues logmap(const LieValues<J>&, const LieValues<J>&);*/ \
  template class LieValues<J>;

using namespace std;

namespace gtsam {

/* ************************************************************************* */
  template<class J>
  void LieValues<J>::print(const string &s) const {
       cout << "LieValues " << s << ", size " << values_.size() << "\n";
       BOOST_FOREACH(const KeyValuePair& v, values_) {
         gtsam::print(v.second, (string)(v.first));
       }
     }

  /* ************************************************************************* */
  template<class J>
  bool LieValues<J>::equals(const LieValues<J>& expected, double tol) const {
    if (values_.size() != expected.values_.size()) return false;
    BOOST_FOREACH(const KeyValuePair& v, values_) {
    	if (!expected.exists(v.first)) return false;
      if(!gtsam::equal(v.second, expected[v.first], tol))
        return false;
    }
    return true;
  }

  /* ************************************************************************* */
  template<class J>
  const typename J::Value& LieValues<J>::at(const J& j) const {
    const_iterator it = values_.find(j);
    if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
    else return it->second;
  }

  /* ************************************************************************* */
  template<class J>
  size_t LieValues<J>::dim() const {
  	size_t n = 0;
  	BOOST_FOREACH(const KeyValuePair& value, values_)
  		n += value.second.dim();
  	return n;
  }

  /* ************************************************************************* */
  template<class J>
  VectorValues LieValues<J>::zero(const Ordering& ordering) const {
  	VectorValues z(this->dims(ordering));
  	z.makeZero();
  	return z;
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::insert(const J& name, const typename J::Value& val) {
    values_.insert(make_pair(name, val));
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::insert(const LieValues<J>& cfg) {
	  BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
		 insert(v.first, v.second);
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::update(const LieValues<J>& cfg) {
	  BOOST_FOREACH(const KeyValuePair& v, values_) {
	  	boost::optional<typename J::Value> t = cfg.exists_(v.first);
	  	if (t) values_[v.first] = *t;
	  }
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::update(const J& j, const typename J::Value& val) {
	  	values_[j] = val;
  }

  /* ************************************************************************* */
  template<class J>
  std::list<J> LieValues<J>::keys() const {
	  std::list<J> ret;
	  BOOST_FOREACH(const KeyValuePair& v, values_)
		  ret.push_back(v.first);
	  return ret;
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::erase(const J& j) {
    size_t dim; // unused
    erase(j, dim);
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::erase(const J& j, size_t& dim) {
    iterator it = values_.find(j);
    if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
    dim = it->second.dim();
    values_.erase(it);
  }

  /* ************************************************************************* */
  // todo: insert for every element is inefficient
  template<class J>
  LieValues<J> LieValues<J>::expmap(const VectorValues& delta, const Ordering& ordering) const {
		LieValues<J> newValues;
		typedef pair<J,typename J::Value> KeyValue;
		BOOST_FOREACH(const KeyValue& value, this->values_) {
			const J& j = value.first;
			const typename J::Value& pj = value.second;
			const Vector& dj = delta[ordering[j]];
			newValues.insert(j, pj.expmap(dj));
		}
		return newValues;
	}

  /* ************************************************************************* */
  template<class J>
  std::vector<size_t> LieValues<J>::dims(const Ordering& ordering) const {
    _ValuesDimensionCollector dimCollector(ordering);
    this->apply(dimCollector);
    return dimCollector.dimensions;
  }

  /* ************************************************************************* */
  template<class J>
  Ordering::shared_ptr LieValues<J>::orderingArbitrary(varid_t firstVar) const {
    // Generate an initial key ordering in iterator order
    _ValuesKeyOrderer keyOrderer(firstVar);
    this->apply(keyOrderer);
    return keyOrderer.ordering;
  }

//  /* ************************************************************************* */
//  // todo: insert for every element is inefficient
//  template<class J>
//  LieValues<J> LieValues<J>::expmap(const Vector& delta) const {
//    if(delta.size() != dim()) {
//    	cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
//      throw invalid_argument("Delta vector length does not match config dimensionality.");
//    }
//    LieValues<J> newValues;
//    int delta_offset = 0;
//		typedef pair<J,typename J::Value> KeyValue;
//		BOOST_FOREACH(const KeyValue& value, this->values_) {
//			const J& j = value.first;
//			const typename J::Value& pj = value.second;
//      int cur_dim = pj.dim();
//      newValues.insert(j,pj.expmap(sub(delta, delta_offset, delta_offset+cur_dim)));
//      delta_offset += cur_dim;
//    }
//    return newValues;
//  }

  /* ************************************************************************* */
  // todo: insert for every element is inefficient
  // todo: currently only logmaps elements in both configs
  template<class J>
  inline VectorValues LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering) const {
  	VectorValues delta(this->dims(ordering));
  	logmap(cp, ordering, delta);
  	return delta;
  }

  /* ************************************************************************* */
  template<class J>
  void LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const {
    typedef pair<J,typename J::Value> KeyValue;
    BOOST_FOREACH(const KeyValue& value, cp) {
      assert(this->exists(value.first));
      delta[ordering[value.first]] = this->at(value.first).logmap(value.second);
    }
  }

}