voxel_map.h

// This file is part of snark, a generic and flexible library
// for robotics research.
//
// Copyright (C) 2011 The University of Sydney
//
// snark is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// snark is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
// for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with snark. If not, see <http://www.gnu.org/licenses/>.

#ifndef SNARK_POINT_CLOUD_VOXELMAP_H
#define SNARK_POINT_CLOUD_VOXELMAP_H

#include <boost/array.hpp>
#include <boost/functional/hash.hpp>
#include <boost/unordered_map.hpp>
#include <Eigen/Core>
#include <comma/base/types.h>

namespace snark {

template < typename Array, std::size_t Size >
struct array_hash : public std::unary_function< Array, std::size_t >
{
    std::size_t operator()( Array const& array ) const
    {
        std::size_t seed = 0;
        for( std::size_t i = 0; i < Size; ++i ) { boost::hash_combine( seed, array[i] ); }
        return seed;
        // return boost::hash_range( &array[0], &array[Size] ); // not so easy...
    }
};

template < typename V, unsigned int D, typename P = Eigen::Matrix< double, D, 1 > >
class voxel_map : public boost::unordered_map< boost::array< comma::int32, D >, V, snark::array_hash< boost::array< comma::int32, D >, D > >
{
    public:
        enum { dimensions = D };
        
        typedef V voxel_type;
        
        typedef P point_type;
        
        typedef boost::array< comma::int32, D > index_type;
        
        typedef boost::unordered_map< index_type, voxel_type, snark::array_hash< index_type, D > > base_type;
        
        typedef typename base_type::iterator iterator;
        
        typedef typename base_type::const_iterator const_iterator;

        voxel_map( const point_type& origin, const point_type& resolution );
        
        voxel_map( const point_type& resolution );
        
        iterator touch_at( const point_type& point );
        
        std::pair< iterator, bool > insert( const point_type& point, const voxel_type& voxel );
        
        index_type index_of( const point_type& point ) const;
        
        static index_type index_of( const point_type& point, const point_type& origin, const point_type& resolution );
        
        static index_type index_of( const point_type& point, const point_type& resolution );
        
        iterator find( const point_type& point );
        
        const_iterator find( const point_type& point ) const;
        
        iterator find( const index_type& index );
        
        const_iterator find( const index_type& index ) const;
        
        const point_type& origin() const;
        
        const point_type& resolution() const;

    private:
        point_type origin_;
        point_type resolution_;
};

template < typename V, unsigned int D, typename P >
inline voxel_map< V, D, P >::voxel_map( const typename voxel_map< V, D, P >::point_type& origin, const typename voxel_map< V, D, P >::point_type& resolution )
    : origin_( origin )
    , resolution_( resolution )
{
}

template < typename V, unsigned int D, typename P >
inline voxel_map< V, D, P >::voxel_map( const typename voxel_map< V, D, P >::point_type& resolution )
    : origin_( point_type::Zero() ) // todo: use traits, if decoupling from eigen required
    , resolution_( resolution )
{
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::iterator voxel_map< V, D, P >::touch_at( const typename voxel_map< V, D, P >::point_type& point )
{
    index_type index = index_of( point );
    iterator it = this->base_type::find( index );
    if( it != this->end() ) { return it; }
    return this->base_type::insert( std::make_pair( index, voxel_type() ) ).first;
}

template < typename V, unsigned int D, typename P >
inline std::pair< typename voxel_map< V, D, P >::iterator, bool > voxel_map< V, D, P >::insert( const typename voxel_map< V, D, P >::point_type& point, const typename voxel_map< V, D, P >::voxel_type& voxel )
{
    return this->base_type::insert( std::make_pair( index_of( point ), voxel ) );
}

namespace impl {

static int negative_flooring_ = static_cast< int >( -1.5 ) == -1 ? -1 : static_cast< int >( -1.5 ) == -2 ? 0 : 0;
static int positive_flooring_ = static_cast< int >( 1.5 ) == 1 ? 0 : static_cast< int >( 1.5 ) == 2 ? -1 : -1;

} // namespace impl {

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::index_type voxel_map< V, D, P >::index_of( const typename voxel_map< V, D, P >::point_type& point, const typename voxel_map< V, D, P >::point_type& origin, const typename voxel_map< V, D, P >::point_type& resolution )
{
    point_type diff = ( point - origin ).array() / resolution.array();
    index_type index;
    for( unsigned int i = 0; i < dimensions; ++i )
    {
        int d = diff[i];
        index[i] = d;
        if( diff[i] == d ) { continue; }
        index[i] += diff[i] < 0 ? impl::negative_flooring_ : ( d == 0 ? 0 : impl::positive_flooring_ );
    }
    return index;
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::index_type voxel_map< V, D, P >::index_of( const typename voxel_map< V, D, P >::point_type& point, const typename voxel_map< V, D, P >::point_type& resolution )
{
    return index_of( point, point_type::Zero(), resolution );
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::index_type voxel_map< V, D, P >::index_of( const typename voxel_map< V, D, P >::point_type& point ) const
{
    return index_of( point, origin_, resolution_ );
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::iterator voxel_map< V, D, P >::find( const typename voxel_map< V, D, P >::point_type& point )
{
    index_type i = index_of( point );
    return this->base_type::find( i );
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::const_iterator voxel_map< V, D, P >::find( const typename voxel_map< V, D, P >::point_type& point ) const
{
    index_type i = index_of( point );
    return this->base_type::find( i );
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::iterator voxel_map< V, D, P >::find( const typename voxel_map< V, D, P >::index_type& index )
{
    return this->base_type::find( index ); // otherwise strange things happen... debug, when we have time
}

template < typename V, unsigned int D, typename P >
inline typename voxel_map< V, D, P >::const_iterator voxel_map< V, D, P >::find( const typename voxel_map< V, D, P >::index_type& index ) const
{
    return this->base_type::find( index ); // otherwise strange things happen... debug, when we have time
}

template < typename V, unsigned int D, typename P >
inline const typename voxel_map< V, D, P >::point_type& voxel_map< V, D, P >::origin() const { return origin_; }

template < typename V, unsigned int D, typename P >
inline const typename voxel_map< V, D, P >::point_type& voxel_map< V, D, P >::resolution() const { return resolution_; }

} // namespace snark {

#endif // SNARK_POINT_CLOUD_VOXELMAP_H