WMDistanceMap.cpp 14.5 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
//---------------------------------------------------------------------------
//
// Project: OpenWalnut ( http://www.openwalnut.org )
//
// Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
// For more information see http://www.openwalnut.org/copying
//
// This file is part of OpenWalnut.
//
// OpenWalnut 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.
//
// OpenWalnut 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 OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
//
//---------------------------------------------------------------------------

25
#include <stdint.h>
26
#include <string>
27 28
#include <algorithm>
#include <vector>
29 30 31 32

#include "WMDistanceMap.h"

#include "../../kernel/WKernel.h"
33
#include "../../kernel/WModuleFactory.h"
34 35
#include "../../dataHandler/WSubject.h"
#include "../../dataHandler/WGridRegular3D.h"
36
#include "../../common/WProgress.h"
37 38 39 40 41

WMDistanceMap::WMDistanceMap():
    WModule()
{
    // WARNING: initializing connectors inside the constructor will lead to an exception.
42 43
    // NOTE: Do not use the module factory inside this constructor. This will cause a dead lock as the module factory is locked
    // during construction of this instance and can then not be used to create another instance (Isosurface in this case).
44
}
45

46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63
WMDistanceMap::~WMDistanceMap()
{
    // cleanup
    removeConnectors();
}

boost::shared_ptr< WModule > WMDistanceMap::factory() const
{
    return boost::shared_ptr< WModule >( new WMDistanceMap() );
}

const std::string WMDistanceMap::getName() const
{
    return "Distance Map";
}

const std::string WMDistanceMap::getDescription() const
{
Alexander Wiebel's avatar
Alexander Wiebel committed
64
    return "Computes a smoothed version of the dataset and a distance map on it.";
65 66 67 68
}

void WMDistanceMap::moduleMain()
{
69
    // use the m_input "data changed" flag
70
    m_moduleState.setResetable( true, true );
71 72
    m_moduleState.add( m_input->getDataChangedCondition() );

73 74 75
    // signal ready state
    ready();

76 77
    // loop until the module container requests the module to quit
    while ( !m_shutdownFlag() )
78
    {
79 80 81 82 83 84 85 86 87 88
        // acquire data from the input connector
        m_dataSet = m_input->getData();
        if ( !m_dataSet.get() )
        {
            // OK, the output has not yet sent data
            // NOTE: see comment at the end of this while loop for m_moduleState
            debugLog() << "Waiting for data ...";
            m_moduleState.wait();
            continue;
        }
89

90 91 92 93
        // found some data
        debugLog() << "Data changed. Updating ...";
        boost::shared_ptr< WValueSet< float > > distanceMapValueSet = createOffset( m_dataSet );
        m_distanceMapDataSet = boost::shared_ptr< WDataSetSingle >( new WDataSetSingle( distanceMapValueSet, m_dataSet->getGrid() ) );
94

95
        WLogger::getLogger()->addLogMessage( "Done!", "Distance Map", LL_INFO );
96

97 98 99 100
        // update the output
        m_output->updateData( m_distanceMapDataSet );

        // wait for new data change event or quit event
101 102
        m_moduleState.wait();
    }
103 104 105 106 107 108
}

void WMDistanceMap::connectors()
{
    // initialize connectors

109 110 111
    m_input = boost::shared_ptr<WModuleInputData< WDataSetSingle > >(
        new WModuleInputData< WDataSetSingle >( shared_from_this(),
                                                               "in", "Dataset to compute distance map for." )
112 113 114 115 116
        );

    // add it to the list of connectors. Please note, that a connector NOT added via addConnector will not work as expected.
    addConnector( m_input );

117 118 119 120 121 122 123 124 125
    m_output = boost::shared_ptr<WModuleOutputData< WDataSetSingle > >(
        new WModuleOutputData< WDataSetSingle >( shared_from_this(),
                                                               "out", "Distance map for the input data set." )
        );

    // add it to the list of connectors. Please note, that a connector NOT added via addConnector will not work as expected.
    addConnector( m_output );

    // call WModule's initialization
126 127 128 129 130
    WModule::connectors();
}

void WMDistanceMap::properties()
{
131
    // no properties
132
}
133

134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163
template< typename T > boost::shared_ptr< WValueSet< float > > makeFloatValueSetHelper( boost::shared_ptr< WValueSet< T > > inSet )
{
    assert( inSet->dimension() == 1 );
    assert( inSet->order() == 0 );

    std::vector< float > data( inSet->size() );
    for( unsigned int i = 0; i < inSet->size(); ++i )
    {
        data[i] = static_cast< float >( inSet->getScalar( i ) );
    }

    boost::shared_ptr< WValueSet< float > > outSet;
    outSet = boost::shared_ptr< WValueSet< float > >( new WValueSet< float >( ( *inSet ).order(), ( *inSet ).dimension(), data, W_DT_FLOAT ) );

    return outSet;
}

boost::shared_ptr< WValueSet< float > > makeFloatValueSet( boost::shared_ptr< WValueSetBase > inSet )
{
    assert( inSet->dimension() == 1 );
    assert( inSet->order() == 0 );

    switch( inSet->getDataType() )
    {
        case W_DT_UNSIGNED_CHAR:
            return makeFloatValueSetHelper( boost::shared_dynamic_cast< WValueSet< unsigned char > >( inSet ) );
            break;
        case W_DT_INT16:
            return makeFloatValueSetHelper( boost::shared_dynamic_cast< WValueSet< int16_t > >( inSet ) );
            break;
164 165 166
        case W_DT_SIGNED_INT:
            return makeFloatValueSetHelper( boost::shared_dynamic_cast< WValueSet< int32_t > >( inSet ) );
            break;
167 168 169 170 171 172 173 174 175
        case W_DT_FLOAT:
            return boost::shared_dynamic_cast< WValueSet< float > >( inSet );
            break;
        default:
            assert( false && "Unknow data type in makeFloatDataSet" );
    }
}

boost::shared_ptr< WValueSet< float > > WMDistanceMap::createOffset( boost::shared_ptr< const WDataSetSingle > dataSet )
176 177 178
{
    std::vector<float> floatDataset;

179
    // wiebel: I know that this is not the most speed and memory efficient way to deal with different data types.
Alexander Wiebel's avatar
Alexander Wiebel committed
180
    //         However, it seems the most feasible at the moment (2009-11-24).
181
    boost::shared_ptr< WValueSet< float > > valueSet = makeFloatValueSet( ( *dataSet ).getValueSet() );
182
    assert( valueSet );
183

184
    boost::shared_ptr< WGridRegular3D > grid = boost::shared_dynamic_cast< WGridRegular3D >( ( *dataSet ).getGrid() );
185
    assert( grid );
186 187

    // TODO(wiebel): we should be able to do all this without the value vector.
188 189 190 191 192 193 194 195 196 197 198 199 200 201
    const std::vector< float >* source = valueSet->rawDataVectorPointer();


    int b, r, c, bb, rr, r0, b0, c0;
    int i, istart, iend;
    int nbands, nrows, ncols, npixels;
    int d, d1, d2, cc1, cc2;
    float u, dmin, dmax;
    bool *srcpix;
    double g, *array;

    nbands = grid->getNbCoordsZ();
    nrows = grid->getNbCoordsY();
    ncols = grid->getNbCoordsX();
202 203 204
#ifdef _WIN32
    npixels = max( nbands, nrows );
#else
205
    npixels = std::max( nbands, nrows );
206
#endif
207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228
    array = new double[npixels];

    npixels = nbands * nrows * ncols;

    floatDataset.resize( npixels );
    for( int i = 0; i < npixels; ++i)
    {
        floatDataset[i] = 0.0;
    }

    bool* bitmask = new bool[npixels];
    for( int i = 0; i < npixels; ++i)
    {
        if (source->at(i) < 0.01)
            bitmask[i] = true;
        else
            bitmask[i] = false;
    }

    dmax = 999999999.0;

    // first pass
229 230 231 232 233

    boost::shared_ptr< WProgress > progress1 = boost::shared_ptr< WProgress >(
            new WProgress( "Distance Map", nbands + nbands + nrows + nbands + nbands + nbands )
    );
    m_progress->addSubProgress( progress1 );
234 235
    for( b = 0; b < nbands; ++b)
    {
236
        ++*progress1;
237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277
        for( r = 0; r < nrows; ++r)
        {
            for( c = 0; c < ncols; ++c)
            {
                //if (VPixel(src,b,r,c,VBit) == 1)
                if( bitmask[b * nrows * ncols + r * ncols + c] )
                {
                    floatDataset[b * nrows * ncols + r * ncols + c] = 0;
                    continue;
                }

                srcpix = bitmask + b * nrows * ncols + r * ncols + c;
                cc1 = c;
                while (cc1 < ncols && *srcpix++ == 0)
                    cc1++;
                d1 = ( cc1 >= ncols ? ncols : ( cc1 - c ) );

                srcpix = bitmask + b * nrows * ncols + r * ncols + c;
                cc2 = c;
                while( cc2 >= 0 && *srcpix-- == 0 )
                    cc2--;
                d2 = ( cc2 <= 0 ? ncols : ( c - cc2 ) );

                if( d1 <= d2 )
                {
                    d = d1;
                    c0 = cc1;
                }
                else
                {
                    d = d2;
                    c0 = cc2;
                }
                floatDataset[b * nrows * ncols + r * ncols + c] = static_cast< float >( d * d );
            }
        }
    }

    // second pass
    for( b = 0; b < nbands; b++ )
    {
278
        ++*progress1;
279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315
        for( c = 0; c < ncols; c++ )
        {
            for( r = 0; r < nrows; r++ )
                array[r] = static_cast< double >( floatDataset[b * nrows * ncols + r * ncols + c] );

            for( r = 0; r < nrows; r++ )
            {
                if( bitmask[b * nrows * ncols + r * ncols + c] == 1 )
                    continue;

                dmin = dmax;
                r0 = r;
                g = sqrt( array[r] );
                istart = r - static_cast< int >( g );
                if( istart < 0 )
                    istart = 0;
                iend = r + static_cast< int >( g + 1 );
                if( iend >= nrows )
                    iend = nrows;

                for( rr = istart; rr < iend; rr++ )
                {
                    u = array[rr] + ( r - rr ) * ( r - rr );
                    if( u < dmin )
                    {
                        dmin = u;
                        r0 = rr;
                    }
                }
                floatDataset[b * nrows * ncols + r * ncols + c] = dmin;
            }
        }
    }

    // third pass
    for( r = 0; r < nrows; r++ )
    {
316
        ++*progress1;
317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407
        for( c = 0; c < ncols; c++ )
        {
            for( b = 0; b < nbands; b++ )
                array[b] = static_cast< double >( floatDataset[b * nrows * ncols + r * ncols + c]  );

            for( b = 0; b < nbands; b++ )
            {
                if (bitmask[b * nrows * ncols + r * ncols + c] == 1)
                    continue;

                dmin = dmax;
                b0 = b;

                g = sqrt( array[b] );
                istart = b - static_cast< int >( g - 1 );
                if( istart < 0 )
                    istart = 0;
                iend = b + static_cast< int >( g + 1 );
                if( iend >= nbands )
                    iend = nbands;

                for( bb = istart; bb < iend; bb++ )
                {
                    u = array[bb] + ( b - bb ) * ( b - bb );
                    if( u < dmin )
                    {
                        dmin = u;
                        b0 = bb;
                    }
                }
                floatDataset[b * nrows * ncols + r * ncols + c] = dmin;
            }
        }
    }

    delete[] array;

    float max = 0;
    for( i = 0; i < npixels; ++i)
    {
        floatDataset[i] = sqrt( static_cast< double >( floatDataset[i] ) );
        if (floatDataset[i] > max)
            max = floatDataset[i];
    }
    for( i = 0; i < npixels; ++i)
    {
        floatDataset[i] = floatDataset[i] / max;
    }

    // filter with gauss
    // create the filter kernel
    double sigma = 4;

    int dim = static_cast< int >( ( 3.0 * sigma + 1 ) );
    int n = 2* dim + 1;
    double step = 1;

    float* kernel = new float[n];

    double sum = 0;
    double x = -static_cast< float >( dim );

    double uu;
    for( int i = 0; i < n; ++i)
    {
        uu = xxgauss( x, sigma );
        sum += uu;
        kernel[i] = uu;
        x += step;
    }

    /* normalize */
    for( int i = 0; i < n; ++i)
    {
        uu = kernel[i];
        uu /= sum;
        kernel[i] = uu;
    }

    d = n / 2;
    float* float_pp;
    std::vector<float> tmp( npixels );
    int c1, cc;

    for( int i = 0; i < npixels; ++i )
    {
        tmp[i] = 0.0;
    }

    for( b = 0; b < nbands; ++b )
    {
408
        ++*progress1;
409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428
        for( r = 0; r < nrows; ++r )
        {
            for( c = d; c < ncols - d; ++c )
            {
                float_pp = kernel;
                sum = 0;
                c0 = c - d;
                c1 = c + d;
                for( cc = c0; cc <= c1; cc++ )
                {
                    x = floatDataset[b * nrows * ncols + r * ncols + cc];
                    sum += x * ( *float_pp++ );
                }
                tmp[b * nrows * ncols + r * ncols + c] = sum;
            }
        }
    }
    int r1;
    for( b = 0; b < nbands; ++b )
    {
429
        ++*progress1;
430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449
        for( r = d; r < nrows - d; ++r )
        {
            for( c = 0; c < ncols; ++c )
            {
                float_pp = kernel;
                sum = 0;
                r0 = r - d;
                r1 = r + d;
                for( rr = r0; rr <= r1; rr++ )
                {
                    x = tmp[b * nrows * ncols + rr * ncols + c];
                    sum += x * ( *float_pp++ );
                }
                floatDataset[b * nrows * ncols + r * ncols + c] = sum;
            }
        }
    }
    int b1;
    for( b = d; b < nbands - d; ++b )
    {
450
        ++*progress1;
451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471
        for( r = 0; r < nrows; ++r )
        {
            for( c = 0; c < ncols; ++c )
            {
                float_pp = kernel;
                sum = 0;
                b0 = b - d;
                b1 = b + d;
                for( bb = b0; bb <= b1; bb++ )
                {
                    x = floatDataset[bb * nrows * ncols + r * ncols + c];
                    sum += x * ( *float_pp++ );
                }
                tmp[b * nrows * ncols + r * ncols + c] = sum;
            }
        }
    }
    delete[] bitmask;
    delete[] kernel;

    floatDataset = tmp;
472 473 474
    boost::shared_ptr< WValueSet< float > > resultValueSet;
    resultValueSet = boost::shared_ptr< WValueSet< float > >(
        new WValueSet< float >( valueSet->order(), valueSet->dimension(), floatDataset, W_DT_FLOAT ) );
475

476 477
    progress1->finish();

478
    return resultValueSet;
479 480 481 482 483 484 485 486 487
}

double WMDistanceMap::xxgauss( double x, double sigma )
{
    double y, z, a = 2.506628273;
    z = x / sigma;
    y = exp( ( double ) -z * z * 0.5 ) / ( sigma * a );
    return y;
}