TerrainConfiguration.hpp

Go to the documentation of this file.

#ifndef __TERRAIN_CONFIGURATION_TYPES__
#define __TERRAIN_CONFIGURATION_TYPES__

#include <vector>
#include <base/Time.hpp>
#include <base/Eigen.hpp>

namespace terrain_estimator {

    enum TerrainType{
        UNKNOWN, 
        GRASS, 
        PATH,
        PEBBLES
    }; 

    struct TerrainProbability{
        TerrainType type; 
        double probability; 
        TerrainProbability()
            : type(UNKNOWN){}
    }; 
    
    struct TerrainClassification{
        base::Time time; 
        int wheel_idx; 
        std::vector<TerrainProbability> terrain;

        static TerrainClassification fromRGB( const base::Vector3d& color )
        {
            terrain_estimator::TerrainProbability tp;
            tp.probability = 0.9;
            // TODO perturb the output and also add other classes

            // HACK converting rgb values of the MLS into a 
            // classification
            // red - path
            // green - grass
            // blue - pebbles
            int max_idx;
            if( color.maxCoeff( &max_idx ) > 0.0 )
            {
                switch( max_idx )
                {
                    case(0): tp.type = terrain_estimator::PATH; break;
                    case(1): tp.type = terrain_estimator::GRASS; break;
                    case(2): tp.type = terrain_estimator::PEBBLES; break;
                }
            }
            else
                tp.type = terrain_estimator::UNKNOWN;

            terrain_estimator::TerrainClassification tc;
            tc.terrain.push_back( tp );

            return tc;
        }

        const base::Vector3d toRGB() const
        {
            // since currently the map patches provide a color
            // information we also convert the
            // terrain_classification information into a color
            // information based on the terrain classes.

            base::Vector3d prop_terrain = base::Vector3d::Zero();

            for( std::vector<terrain_estimator::TerrainProbability>::const_iterator it = terrain.begin();
                    it != terrain.end(); it++)
            {
                const terrain_estimator::TerrainProbability &tp( *it );
                switch( tp.type )
                {
                    case terrain_estimator::PATH: prop_terrain[0] = tp.probability; break;
                    case terrain_estimator::GRASS: prop_terrain[1] = tp.probability; break;
                    case terrain_estimator::PEBBLES: prop_terrain[2] = tp.probability; break;
                    default: break;
                }
            }

            return prop_terrain;
        }

        double jointProbability( const TerrainClassification& tc )
        {
            // for now, do this in RGB space
            // TODO this only works when there are only 3 classes
            // will need to update for more classes later
            base::Vector3d prop_terrain( toRGB() );
            base::Vector3d visual_terrain( tc.toRGB() );

            // for now use the crudest way possible to get the probability
            // for the measurement
            //double prob = 1.0 - (visual_terrain - prop_terrain).norm() / sqrt( 3.0 );

            int idx1, idx2;
            prop_terrain.maxCoeff( &idx1 );
            visual_terrain.maxCoeff( &idx2 );

            double prob = (idx1 == idx2) ? 0.9 : 0.1;

            return prob;
        }
    }; 
    
  
    struct SVMConfiguration{
        std::vector<double> function; 
        double offset; 
        double lower_threshold;
        TerrainType lower_type;
        double upper_threshold; 
        TerrainType upper_type; 
        #ifndef __orogen
        SVMConfiguration()
            : lower_type(UNKNOWN),
            upper_type(UNKNOWN){}
        #endif
    };
}
#endif