<commit_before>/*************************************************************************
 *
 *  $RCSfile: unomodel.hxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: mtg $ $Date: 2001-04-05 22:15:45 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library 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 this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef UNOMODEL_HXX
#define UNOMODEL_HXX

#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XSERVICEINFO_HPP_
#include <com/sun/star/lang/XServiceInfo.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XUNOTUNNEL_HPP_
#include <com/sun/star/lang/XUnoTunnel.hpp>
#endif
#ifndef _SFX_SFXBASEMODEL_HXX_
#include <sfx2/sfxbasemodel.hxx>
#endif
#ifndef _CPPUHELPER_IMPLBASE3_HXX_
#include <cppuhelper/implbase3.hxx> // helper for implementations
#endif

struct SfxItemPropertyMap;

//-----------------------------------------------------------------------------
typedef
cppu::WeakImplHelper3
<
    ::com::sun::star::beans::XPropertySet,
    ::com::sun::star::lang::XServiceInfo,
    ::com::sun::star::lang::XUnoTunnel
>
SmModelBaseClass;
class SmModel : public SmModelBaseClass,
                    public SfxBaseModel
{

    SfxItemPropertyMap*     _pMap;

public:
    SmModel( SfxObjectShell *pObjSh = 0 );
    virtual ~SmModel();
    const SfxItemPropertyMap * getPropertyMap() const { return _pMap;}

    //XInterface
    virtual     ::com::sun::star::uno::Any SAL_CALL queryInterface( const ::com::sun::star::uno::Type& aType ) throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL acquire(  ) throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL release(  ) throw(::com::sun::star::uno::RuntimeException);

    //XTypeProvider
    virtual ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Type > SAL_CALL getTypes(  ) throw(::com::sun::star::uno::RuntimeException);

    static const ::com::sun::star::uno::Sequence< sal_Int8 > & getUnoTunnelId();

    //XUnoTunnel
    virtual sal_Int64 SAL_CALL getSomething( const ::com::sun::star::uno::Sequence< sal_Int8 >& aIdentifier ) throw(::com::sun::star::uno::RuntimeException);

    //XPropertySet
    virtual ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySetInfo > SAL_CALL getPropertySetInfo(  ) throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL setPropertyValue( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Any& aValue ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::beans::PropertyVetoException, ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Any SAL_CALL getPropertyValue( const ::rtl::OUString& PropertyName ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL addPropertyChangeListener( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertyChangeListener >& xListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL removePropertyChangeListener( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertyChangeListener >& aListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL addVetoableChangeListener( const ::rtl::OUString& PropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XVetoableChangeListener >& aListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL removeVetoableChangeListener( const ::rtl::OUString& PropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XVetoableChangeListener >& aListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);

    //XServiceInfo
    virtual rtl::OUString SAL_CALL getImplementationName(void) throw( ::com::sun::star::uno::RuntimeException );
    virtual BOOL SAL_CALL supportsService(const rtl::OUString& ServiceName) throw( ::com::sun::star::uno::RuntimeException );
    virtual ::com::sun::star::uno::Sequence< rtl::OUString > SAL_CALL getSupportedServiceNames(void) throw( ::com::sun::star::uno::RuntimeException );

    inline ::rtl::OUString SmModel::getImplementationName_Static() throw(  );

    void * SAL_CALL operator new( size_t ) throw();
    void SAL_CALL operator delete( void * ) throw();
};

inline ::rtl::OUString SmModel::getImplementationName_Static() throw(  )
{
    return rtl::OUString::createFromAscii("math.SmModel");
}

#endif
<commit_msg>Implement XPropertyState so we only export non-default values and XMultiPropertySet so we can set them all at a single call to the model<commit_after>/*************************************************************************
 *
 *  $RCSfile: unomodel.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: mtg $ $Date: 2001-04-18 15:27:41 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library 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 this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef UNOMODEL_HXX
#define UNOMODEL_HXX

#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XMULTIPROPERTYSET_HPP_
#include <com/sun/star/beans/XMultiPropertySet.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSTATE_HPP_
#include <com/sun/star/beans/XPropertyState.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XSERVICEINFO_HPP_
#include <com/sun/star/lang/XServiceInfo.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XUNOTUNNEL_HPP_
#include <com/sun/star/lang/XUnoTunnel.hpp>
#endif
#ifndef _SFX_SFXBASEMODEL_HXX_
#include <sfx2/sfxbasemodel.hxx>
#endif
#ifndef _CPPUHELPER_IMPLBASE5_HXX_
#include <cppuhelper/implbase5.hxx> // helper for implementations
#endif

struct SfxItemPropertyMap;
class SmFormat;

//-----------------------------------------------------------------------------
typedef
cppu::WeakImplHelper5
<
    ::com::sun::star::beans::XPropertySet,
    ::com::sun::star::beans::XMultiPropertySet,
    ::com::sun::star::beans::XPropertyState,
    ::com::sun::star::lang::XServiceInfo,
    ::com::sun::star::lang::XUnoTunnel
>
SmModelBaseClass;
class SmModel : public SmModelBaseClass,
                    public SfxBaseModel
{

    SfxItemPropertyMap*     _pMap;
    void SAL_CALL _setPropertyValue ( const ::com::sun::star::uno::Any& aValue,
                                      SfxObjectShell* pObjShell, SmFormat &aFormat, const SfxItemPropertyMap* pMap);
    void SAL_CALL _getPropertyValue ( ::com::sun::star::uno::Any& rValue,
                                      SfxObjectShell * pObjShell,   const SmFormat &aFormat, const SfxItemPropertyMap *pMap );
    void SAL_CALL _getPropertyDefault ( ::com::sun::star::uno::Any& rValue,
                                      SfxObjectShell * pObjShell,   const SmFormat &aFormat, const SfxItemPropertyMap *pMap );

public:
    SmModel( SfxObjectShell *pObjSh = 0 );
    virtual ~SmModel();
    const SfxItemPropertyMap * getPropertyMap() const { return _pMap;}
    ::com::sun::star::uno::Sequence < ::com::sun::star::beans::PropertyValue > SAL_CALL getPropertyValueSequence ( const ::com::sun::star::uno::Sequence < ::rtl::OUString > & rPropertyNames);

    //XInterface
    virtual     ::com::sun::star::uno::Any SAL_CALL queryInterface( const ::com::sun::star::uno::Type& aType ) throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL acquire(  ) throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL release(  ) throw(::com::sun::star::uno::RuntimeException);

    //XTypeProvider
    virtual ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Type > SAL_CALL getTypes(  ) throw(::com::sun::star::uno::RuntimeException);

    static const ::com::sun::star::uno::Sequence< sal_Int8 > & getUnoTunnelId();

    //XUnoTunnel
    virtual sal_Int64 SAL_CALL getSomething( const ::com::sun::star::uno::Sequence< sal_Int8 >& aIdentifier ) throw(::com::sun::star::uno::RuntimeException);

    //XPropertySet
    virtual ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySetInfo > SAL_CALL getPropertySetInfo(  ) throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL setPropertyValue( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Any& aValue ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::beans::PropertyVetoException, ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Any SAL_CALL getPropertyValue( const ::rtl::OUString& PropertyName ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL addPropertyChangeListener( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertyChangeListener >& xListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL removePropertyChangeListener( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertyChangeListener >& aListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL addVetoableChangeListener( const ::rtl::OUString& PropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XVetoableChangeListener >& aListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL removeVetoableChangeListener( const ::rtl::OUString& PropertyName, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XVetoableChangeListener >& aListener ) throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);

    //XMultiPropertySet
    virtual void SAL_CALL setPropertyValues( const ::com::sun::star::uno::Sequence< ::rtl::OUString >& aPropertyNames, const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& aValues )
        throw (::com::sun::star::beans::PropertyVetoException, ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any > SAL_CALL getPropertyValues( const ::com::sun::star::uno::Sequence< ::rtl::OUString >& aPropertyNames )
        throw (::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL addPropertiesChangeListener( const ::com::sun::star::uno::Sequence< ::rtl::OUString >& aPropertyNames, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertiesChangeListener >& xListener )
        throw (::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL removePropertiesChangeListener( const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertiesChangeListener >& xListener )
        throw (::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL firePropertiesChangeEvent( const ::com::sun::star::uno::Sequence< ::rtl::OUString >& aPropertyNames, const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertiesChangeListener >& xListener )
        throw (::com::sun::star::uno::RuntimeException);

    //XPropertyState
    virtual ::com::sun::star::beans::PropertyState SAL_CALL getPropertyState( const ::rtl::OUString& PropertyName )
        throw (::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Sequence< ::com::sun::star::beans::PropertyState > SAL_CALL getPropertyStates( const ::com::sun::star::uno::Sequence< ::rtl::OUString >& aPropertyName )
        throw (::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL setPropertyToDefault( const ::rtl::OUString& PropertyName )
        throw (::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Any SAL_CALL getPropertyDefault( const ::rtl::OUString& aPropertyName )
        throw (::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);

    //XServiceInfo
    virtual rtl::OUString SAL_CALL getImplementationName(void) throw( ::com::sun::star::uno::RuntimeException );
    virtual BOOL SAL_CALL supportsService(const rtl::OUString& ServiceName) throw( ::com::sun::star::uno::RuntimeException );
    virtual ::com::sun::star::uno::Sequence< rtl::OUString > SAL_CALL getSupportedServiceNames(void) throw( ::com::sun::star::uno::RuntimeException );

    inline ::rtl::OUString SmModel::getImplementationName_Static() throw(  );

    void * SAL_CALL operator new( size_t ) throw();
    void SAL_CALL operator delete( void * ) throw();
};

inline ::rtl::OUString SmModel::getImplementationName_Static() throw(  )
{
    return rtl::OUString::createFromAscii("math.SmModel");
}

#endif
<|endoftext|>


<commit_before>#include "packet.h"
#include "main.h"
#include "states.h"
#include "spiMaster.h"
#include "tracking.h"

const volatile bool SPI2_LOOPBACK = false;

IntervalTimer timer, timer2, timer3;
volatile uint16_t state = SETUP_STATE;
volatile uint16_t previousState = SETUP_STATE;
volatile bool changingState = false;
volatile unsigned int timeAlive = 0;
volatile unsigned int errors = 0;
volatile unsigned int bugs = 0;
long startOfLoop = 0;
volatile bool ignoreLoopTime = false;
volatile unsigned int lastLoopTime = 0;
volatile unsigned int maxLoopTime = 0;
volatile unsigned int lastLoopState = state;
volatile unsigned int lastAnalogRead = 0;
volatile unsigned int numLoops = 0; // Mostly just to count % of loops that we need to clear dma buffer

volatile unsigned int numFastLoops = 0;
volatile unsigned int numSlowLoops = 0;
volatile unsigned int numMediumLoops = 0;

void setup() {
  Serial.begin(115200);
  debugPrintf("Serial is on\n");
  delay(5000);
  // Every 2.75 seconds - integer quantity of seconds prevents us from seeing
  // dma offset change, as sampling rate is a clean multiple of buffer size
  const int heartRate = 2751234;
  timer.begin(heartbeat, heartRate);
  delay(1);
  timer2.begin(heartbeat2, heartRate);
  delay(1);
  timer3.begin(heartbeat3, heartRate);
  debugPrintf("Set up heartbeat interrupt\n");
  analogReadResolution(16);
  debugPrintf("Begin Spi2\n");
  debugPrintf("Setting up packet:\n");
  spiSlave.packet_setup();
  debugPrintf("Done.\n");
  debugPrintf("Setting up dma:\n");
  quadCell.spiMasterSetup();
  debugPrintf("Done!\n");
  debugPrintf("Setting up tracking:\n");
  pointer.trackingSetup();
  debugPrintf("Done!\n");
  state = IDLE_STATE;
  delay(1000);
  debugPrintf("Running tests:\n");
  runTests();
  //debugPrintf("Hey %d\n", assert(state != IDLE_STATE));
}

bool assertionError(const char* file, int line, const char* assertion) {
    errors++;
    bugs++;
    Serial.printf("%s, %d: assertion 'assertion' failed, total errors %d, bugs %d\n", file, line, errors, bugs);
    return false;
}

extern uint16_t packet[];
extern uint16_t spi_tx_out[];
extern DMAChannel dma_rx;
extern DMAChannel dma_tx;
extern DMAChannel dma_tx2;
void heartbeat() {
    debugPrintf("State %d,", state);
    debugPrintf("transmitting %d, packetsReceived %d, ", spiSlave.transmitting, spiSlave.packetsReceived);
    debugPrintf("%d errors, bugs %d, last loop %d micros, max loop time %d micros", errors, bugs, lastLoopTime, maxLoopTime);
    maxLoopTime = 0;
}

extern volatile unsigned int frontOfBuffer, backOfBuffer;
void heartbeat2() {
    debugPrintf(", last state %d, last read %d", lastLoopState, lastAnalogRead);
    debugPrintf(", %d loops, time alive %d, numSamplesRead %d\n", numLoops, timeAlive, quadCell.numSamplesRead);
    debugPrintf("%d last, %d max, %d fast, %d med, %d slow loops\n", lastLoopTime, maxLoopTime, numFastLoops, numMediumLoops, numSlowLoops);
    quadCell.quadCellHeartBeat();

    debugPrintf("pid._dt: %d\n", (int) (pid._dt * 10000));
}

void heartbeat3() {
    if (state == TRACKING_STATE) {
        pointer.trackingHeartbeat();
    } else if (state == CALIBRATION_STATE) {
        pointer.calibrationHeartbeat();
    }

    debugPrintf("-----------------------\n");
}

void taskIdle(void) {
    // :)
}

void leaveCurrentState() {
    assert(previousState != SHUTDOWN_STATE);
    if (previousState == IDLE_STATE) {
        //leaveIdle(); // Nothing to do here
    } else if (previousState == TRACKING_STATE) {
        pointer.leaveTracking();
    } else if (previousState == CALIBRATION_STATE) {
        pointer.leaveCalibration();
    }
}

void enterNextState() {
    if (state == IDLE_STATE) {
        //enterIdle(); // Nothing to do here
    } else if (state == TRACKING_STATE) {
        pointer.enterTracking();
    } else if (state == CALIBRATION_STATE) {
        pointer.enterCalibration();
    }
}

void checkTasks(void) {
    long startOfLoop = micros();
    assert(state <= MAX_STATE);
    if (changingState) {
        noInterrupts();
        leaveCurrentState();
        enterNextState();
        changingState = false;
        interrupts();
    }
    if (state == IDLE_STATE) {
        taskIdle();
    } else if (state == TRACKING_STATE) {
        pointer.taskTracking();
    } else if (state == CALIBRATION_STATE) {
        pointer.taskCalibration();
    }

    // Save this into a long because elapsedMillis is not guaranteed in interrupts
    timeAlive = millis();
    if (ignoreLoopTime) {
        ignoreLoopTime = false;
    } else {
        lastLoopTime = micros() - startOfLoop;
        lastLoopState = state;
        if (lastLoopTime > maxLoopTime) {
            maxLoopTime = lastLoopTime;
        }
        if (lastLoopTime < 10) {
            numFastLoops++;
        } else if (lastLoopTime < 100) {
            numMediumLoops++;
        } else {
            numSlowLoops++;
        }
        // For now just assert, there isn't really a way to recover from a long main loop
        assert(lastLoopTime <= 1000000.0);
    }
    numLoops++;
}

void loop() {
    checkTasks();
}
<commit_msg>Remove sleep<commit_after>#include "packet.h"
#include "main.h"
#include "states.h"
#include "spiMaster.h"
#include "tracking.h"

const volatile bool SPI2_LOOPBACK = false;

IntervalTimer timer, timer2, timer3;
volatile uint16_t state = SETUP_STATE;
volatile uint16_t previousState = SETUP_STATE;
volatile bool changingState = false;
volatile unsigned int timeAlive = 0;
volatile unsigned int errors = 0;
volatile unsigned int bugs = 0;
long startOfLoop = 0;
volatile bool ignoreLoopTime = false;
volatile unsigned int lastLoopTime = 0;
volatile unsigned int maxLoopTime = 0;
volatile unsigned int lastLoopState = state;
volatile unsigned int lastAnalogRead = 0;
volatile unsigned int numLoops = 0; // Mostly just to count % of loops that we need to clear dma buffer

volatile unsigned int numFastLoops = 0;
volatile unsigned int numSlowLoops = 0;
volatile unsigned int numMediumLoops = 0;

void setup() {
  Serial.begin(115200);
  debugPrintf("Serial is on\n");
  // Every 2.75 seconds - integer quantity of seconds prevents us from seeing
  // dma offset change, as sampling rate is a clean multiple of buffer size
  const int heartRate = 2751234;
  timer.begin(heartbeat, heartRate);
  delay(1);
  timer2.begin(heartbeat2, heartRate);
  delay(1);
  timer3.begin(heartbeat3, heartRate);
  debugPrintf("Set up heartbeat interrupt\n");
  analogReadResolution(16);
  debugPrintf("Begin Spi2\n");
  debugPrintf("Setting up packet:\n");
  spiSlave.packet_setup();
  debugPrintf("Done.\n");
  debugPrintf("Setting up dma:\n");
  quadCell.spiMasterSetup();
  debugPrintf("Done!\n");
  debugPrintf("Setting up tracking:\n");
  pointer.trackingSetup();
  debugPrintf("Done!\n");
  state = IDLE_STATE;
  delay(1000);
  debugPrintf("Running tests:\n");
  runTests();
  //debugPrintf("Hey %d\n", assert(state != IDLE_STATE));
}

bool assertionError(const char* file, int line, const char* assertion) {
    errors++;
    bugs++;
    Serial.printf("%s, %d: assertion 'assertion' failed, total errors %d, bugs %d\n", file, line, errors, bugs);
    return false;
}

extern uint16_t packet[];
extern uint16_t spi_tx_out[];
extern DMAChannel dma_rx;
extern DMAChannel dma_tx;
extern DMAChannel dma_tx2;
void heartbeat() {
    debugPrintf("State %d,", state);
    debugPrintf("transmitting %d, packetsReceived %d, ", spiSlave.transmitting, spiSlave.packetsReceived);
    debugPrintf("%d errors, bugs %d, last loop %d micros, max loop time %d micros", errors, bugs, lastLoopTime, maxLoopTime);
    maxLoopTime = 0;
}

extern volatile unsigned int frontOfBuffer, backOfBuffer;
void heartbeat2() {
    debugPrintf(", last state %d, last read %d", lastLoopState, lastAnalogRead);
    debugPrintf(", %d loops, time alive %d, numSamplesRead %d\n", numLoops, timeAlive, quadCell.numSamplesRead);
    debugPrintf("%d last, %d max, %d fast, %d med, %d slow loops\n", lastLoopTime, maxLoopTime, numFastLoops, numMediumLoops, numSlowLoops);
    quadCell.quadCellHeartBeat();

    debugPrintf("pid._dt: %d\n", (int) (pid._dt * 10000));
}

void heartbeat3() {
    if (state == TRACKING_STATE) {
        pointer.trackingHeartbeat();
    } else if (state == CALIBRATION_STATE) {
        pointer.calibrationHeartbeat();
    }

    debugPrintf("-----------------------\n");
}

void taskIdle(void) {
    // :)
}

void leaveCurrentState() {
    assert(previousState != SHUTDOWN_STATE);
    if (previousState == IDLE_STATE) {
        //leaveIdle(); // Nothing to do here
    } else if (previousState == TRACKING_STATE) {
        pointer.leaveTracking();
    } else if (previousState == CALIBRATION_STATE) {
        pointer.leaveCalibration();
    }
}

void enterNextState() {
    if (state == IDLE_STATE) {
        //enterIdle(); // Nothing to do here
    } else if (state == TRACKING_STATE) {
        pointer.enterTracking();
    } else if (state == CALIBRATION_STATE) {
        pointer.enterCalibration();
    }
}

void checkTasks(void) {
    long startOfLoop = micros();
    assert(state <= MAX_STATE);
    if (changingState) {
        noInterrupts();
        leaveCurrentState();
        enterNextState();
        changingState = false;
        interrupts();
    }
    if (state == IDLE_STATE) {
        taskIdle();
    } else if (state == TRACKING_STATE) {
        pointer.taskTracking();
    } else if (state == CALIBRATION_STATE) {
        pointer.taskCalibration();
    }

    // Save this into a long because elapsedMillis is not guaranteed in interrupts
    timeAlive = millis();
    if (ignoreLoopTime) {
        ignoreLoopTime = false;
    } else {
        lastLoopTime = micros() - startOfLoop;
        lastLoopState = state;
        if (lastLoopTime > maxLoopTime) {
            maxLoopTime = lastLoopTime;
        }
        if (lastLoopTime < 10) {
            numFastLoops++;
        } else if (lastLoopTime < 100) {
            numMediumLoops++;
        } else {
            numSlowLoops++;
        }
        // For now just assert, there isn't really a way to recover from a long main loop
        assert(lastLoopTime <= 1000000.0);
    }
    numLoops++;
}

void loop() {
    checkTasks();
}
<|endoftext|>


<commit_before>// $Id$
//
//  Copyright (C) 2003-2006 Rational Discovery LLC
//
//   @@ All Rights Reserved  @@
//
#include <GraphMol/ROMol.h>
#include <GraphMol/Atom.h>
#include <GraphMol/Bond.h>
#include <GraphMol/BondIterators.h>
#include <GraphMol/MolOps.h>
#include <memory.h>
#include <boost/shared_array.hpp>
#include <algorithm>

namespace RDKit{

  const int LOCAL_INF=(int)1e8;

  // local utility namespace
  namespace {
    /* ----------------------------------------------

    This implements the Floyd-Warshall all-pairs-shortest-paths
    algorithm as described on pg 564 of the White Book (Cormen, Leiserson, Rivest)

    Arguments:
    adjMat: the adjacency matrix.  This is overwritten with the shortest
    paths, should be dim x dim
    dim: the size of adjMat
    pathMat: the path matrix, should be dim x dim

    -----------------------------------------------*/
    template<class T> void
    FloydWarshall(int dim,T *adjMat,int *pathMat)
    {
      int k,i,j;
      T *currD,*lastD,*tTemp;
      int *currP,*lastP,*iTemp;

      currD = new T[dim*dim];
      currP = new int[dim*dim];
      lastD = new T[dim*dim];
      lastP = new int[dim*dim];

      memcpy(static_cast<void *>(lastD),static_cast<void *>(adjMat),dim*dim*sizeof(T));

      // initialize the paths
      for(i=0;i<dim;i++){
	int itab=i*dim;
	for(j=0;j<dim;j++){
	  if(i==j || adjMat[itab+j]==LOCAL_INF){
	    pathMat[itab+j] = -1;
	  }else{
	    pathMat[itab+j] = i;
	  }
	}
      }
      memcpy(static_cast<void *>(lastP),static_cast<void *>(pathMat),dim*dim*sizeof(int));

      for(k=0;k<dim;k++){
	int ktab=k*dim;
	for(i=0;i<dim;i++){
	  int itab=i*dim;
	  for(j=0;j<dim;j++){
	    T v1=lastD[itab+j];
	    T v2=lastD[itab+k]+lastD[ktab+j];
	    if(v1<=v2){
	      currD[itab+j] = v1;
	      currP[itab+j] = lastP[itab+j];
	    }else{
	      currD[itab+j] = v2;
	      currP[itab+j] = lastP[ktab+j];
	    }
	  }
	}
	tTemp = currD;
	currD = lastD;
	lastD = tTemp;

	iTemp = currP;
	currP = lastP;
	lastP = iTemp;
      }
      memcpy(static_cast<void *>(adjMat),static_cast<void *>(lastD),dim*dim*sizeof(T));
      memcpy(static_cast<void *>(pathMat),static_cast<void *>(lastP),dim*dim*sizeof(int));

      delete [] currD;
      delete [] currP;
      delete [] lastD;
      delete [] lastP;
    }

    template<class T> void
    FloydWarshall(int dim,T *adjMat,int *pathMat,const std::vector<int> &activeAtoms)
    {
      T *currD,*lastD,*tTemp;
      int *currP,*lastP,*iTemp;

      currD = new T[dim*dim];
      currP = new int[dim*dim];
      lastD = new T[dim*dim];
      lastP = new int[dim*dim];

      memcpy(static_cast<void *>(lastD),static_cast<void *>(adjMat),dim*dim*sizeof(T));

      // initialize the paths
      for(std::vector<int>::const_iterator i=activeAtoms.begin();
	  i!=activeAtoms.end();
	  i++){
	int itab=(*i)*dim;
	for(std::vector<int>::const_iterator j=activeAtoms.begin();
	    j!=activeAtoms.end();
	    j++){
	  if((*i)==(*j) || adjMat[itab+(*j)]==LOCAL_INF){
	    pathMat[itab+(*j)] = -1;
	  }else{
	    pathMat[itab+(*j)] = (*i);
	  }
	}
      }
      memcpy(static_cast<void *>(lastP),static_cast<void *>(pathMat),dim*dim*sizeof(int));

      for(std::vector<int>::const_iterator k=activeAtoms.begin();
	  k!=activeAtoms.end();
	  k++){
	int ktab=(*k)*dim;
	for(std::vector<int>::const_iterator i=activeAtoms.begin();
	    i!=activeAtoms.end();
	    i++){
	  int itab=(*i)*dim;
	  for(std::vector<int>::const_iterator j=activeAtoms.begin();
	      j!=activeAtoms.end();
	      j++){
	    T v1=lastD[itab+(*j)];
	    T v2=lastD[itab+(*k)]+lastD[ktab+(*j)];
	    if(v1<=v2){
	      currD[itab+(*j)] = v1;
	      currP[itab+(*j)] = lastP[itab+(*j)];
	    }else{
	      currD[itab+(*j)] = v2;
	      currP[itab+(*j)] = lastP[ktab+(*j)];
	    }
	  }
	}
	tTemp = currD;
	currD = lastD;
	lastD = tTemp;

	iTemp = currP;
	currP = lastP;
	lastP = iTemp;
      }
      memcpy(static_cast<void *>(adjMat),static_cast<void *>(lastD),dim*dim*sizeof(T));
      memcpy(static_cast<void *>(pathMat),static_cast<void *>(lastP),dim*dim*sizeof(int));

      delete [] currD;
      delete [] currP;
      delete [] lastD;
      delete [] lastP;
    }
  } // end of local utility namespace

  namespace MolOps {
    double *getDistanceMat(const ROMol &mol,bool useBO,
				   bool useAtomWts,
				   bool force,
				   const char *propNamePrefix){
      std::string propName;
      boost::shared_array<double> sptr;
      if(propNamePrefix){
	propName = propNamePrefix;
      } else {
	propName = "";
      }
      propName+="DistanceMatrix";
      // make sure we don't use the nonBO cache for the BO matrix and vice versa:
      if(useBO) propName+="BO";
      if(!force && mol.hasProp(propName)){
	mol.getProp(propName,sptr);
	return sptr.get();
      }
      int nAts=mol.getNumAtoms();
      double *dMat = new double[nAts*nAts];
      int i,j;
      // initialize off diagonals to LOCAL_INF and diagonals to 0
      for(i=0;i<nAts*nAts;i++) dMat[i] = LOCAL_INF;
      for(i=0;i<nAts;i++) dMat[i*nAts+i]=0.0;

      ROMol::EDGE_ITER firstB,lastB;
      boost::tie(firstB,lastB) = mol.getEdges();
      ROMol::GRAPH_MOL_BOND_PMAP::const_type bondMap = mol.getBondPMap();
      while(firstB!=lastB){
	const Bond *bond = bondMap[*firstB];
	i = bond->getBeginAtomIdx();
	j = bond->getEndAtomIdx();
	double contrib;
	if(useBO){
	  if(!bond->getIsAromatic()){
	    contrib = 1./bond->getBondTypeAsDouble();
	  } else {
	    contrib = 2. / 3.;
	  }
	} else {
	  contrib = 1.0;
	}
	dMat[i*nAts+j]=contrib;
	dMat[j*nAts+i]=contrib;
	firstB++;
      }

      int *pathMat = new int[nAts*nAts];
      memset(static_cast<void *>(pathMat),0,nAts*nAts*sizeof(int));
      FloydWarshall(nAts,dMat,pathMat);

      if(useAtomWts){
	for (i = 0; i < nAts; i++) {
	  int anum = mol.getAtomWithIdx(i)->getAtomicNum();
	  dMat[i*nAts+i] = 6.0/anum;
	}
      }
      sptr.reset(dMat);
      mol.setProp(propName,sptr,true);
      boost::shared_array<int> iSptr(pathMat);
      mol.setProp(propName+"_Paths",iSptr,true);

      return dMat;
    };

    double *getDistanceMat(const ROMol &mol,
				   const std::vector<int> &activeAtoms,
				   const std::vector<const Bond *> &bonds,
				   bool useBO,
				   bool useAtomWts){

      const int nAts=activeAtoms.size();

      double *dMat = new double[nAts*nAts];
      int i,j;
      // initialize off diagonals to LOCAL_INF and diagonals to 0
      for(i=0;i<nAts*nAts;i++) dMat[i] = LOCAL_INF;
      for(i=0;i<nAts;i++) dMat[i*nAts+i]=0.0;

      for(std::vector<const Bond *>::const_iterator bi=bonds.begin();
	  bi!=bonds.end();bi++){
	const Bond *bond=*bi;
	i=std::find(activeAtoms.begin(),activeAtoms.end(),
		    bond->getBeginAtomIdx()) - activeAtoms.begin();
	j=std::find(activeAtoms.begin(),activeAtoms.end(),
		    bond->getEndAtomIdx()) - activeAtoms.begin();
	double contrib;
	if(useBO){
	  if(!bond->getIsAromatic()){
	    contrib = 1./bond->getBondTypeAsDouble();
	  } else {
	    contrib = 2. / 3.;
	  }
	} else {
	  contrib = 1.0;
	}
	dMat[i*nAts+j]=contrib;
	dMat[j*nAts+i]=contrib;
      }

      int *pathMat = new int[nAts*nAts];
      memset(static_cast<void *>(pathMat),0,nAts*nAts*sizeof(int));
      FloydWarshall(nAts,dMat,pathMat);
      delete [] pathMat;

      if(useAtomWts){
	for(i=0;i<nAts;i++){
	  int anum = mol.getAtomWithIdx(activeAtoms[i])->getAtomicNum();
	  dMat[i*nAts+i] = 6.0/anum;
	}
      }
      return dMat;
    };

    // NOTE: do *not* delete results
    double *getAdjacencyMatrix(const ROMol &mol,
				       bool useBO,
				       int emptyVal,
				       bool force,
				       const char *propNamePrefix)
    {
      std::string propName;
      boost::shared_array<double> sptr;
      if(propNamePrefix){
	propName = propNamePrefix;
      } else {
	propName = "";
      }
      propName+="AdjacencyMatrix";
      if(!force && mol.hasProp(propName)){
	mol.getProp(propName,sptr);
	return sptr.get();
      }

      int nAts = mol.getNumAtoms();
      double *res = new double[nAts*nAts];
      memset(static_cast<void *>(res),emptyVal,nAts*nAts*sizeof(double));

      ROMol::ConstBondIterator bondIt;
      for(bondIt=mol.beginBonds();bondIt!=mol.endBonds();bondIt++){
	if(!useBO){
	  int beg=(*bondIt)->getBeginAtomIdx();
	  int end=(*bondIt)->getEndAtomIdx();
	  res[beg*nAts+end] = 1;
	  res[end*nAts+beg] = 1;
	}
	else {
	  int begIdx=(*bondIt)->getBeginAtomIdx();
	  int endIdx=(*bondIt)->getEndAtomIdx();
	  Atom const *beg=mol.getAtomWithIdx(begIdx);
	  Atom const *end=mol.getAtomWithIdx(endIdx);
	  res[begIdx*nAts+endIdx] = (*bondIt)->getValenceContrib(beg);
	  res[endIdx*nAts+begIdx] = (*bondIt)->getValenceContrib(end);
	}
      }
      sptr.reset(res);
      mol.setProp(propName,sptr,true);

      return res;
    };

    INT_LIST getShortestPath(const ROMol &mol, int aid1, int aid2) {

      int nats = mol.getNumAtoms();
      RANGE_CHECK(0,aid1,nats-1);
      RANGE_CHECK(0,aid2,nats-1);
      CHECK_INVARIANT(aid1 != aid2, "");

      INT_VECT pred, doneAtms;

      pred.reserve(nats);
      doneAtms.reserve(nats);
      int ai;
      for (ai = 0; ai < nats; ++ai) {
	doneAtms[ai] = 0;
      }

      std::deque<int> bfsQ;

      bfsQ.push_back(aid1);
      bool done = false;
      ROMol::ADJ_ITER nbrIdx,endNbrs;
      while ((!done) && (bfsQ.size() > 0)) {
	int curAid = bfsQ.front();
	boost::tie(nbrIdx,endNbrs) = mol.getAtomNeighbors(mol.getAtomWithIdx(curAid));
	while (nbrIdx != endNbrs) {
	  if (doneAtms[*nbrIdx] == 0) {
	    pred[*nbrIdx] = curAid;
	    if ((*nbrIdx) == aid2) {
	      done = true;
	      break;
	    }
	    bfsQ.push_back(*nbrIdx);
	  }
	  nbrIdx++;
	}
	doneAtms[curAid] = 1;
	bfsQ.pop_front();
      }

      INT_LIST res;
      done = false;
      int prev = aid2;
      while (!done) {
	prev = pred[prev];
	if (prev != aid1) {
	  res.push_front(prev);
	} else {
	  done = true;
	}
      }

      return res;

    }
  } // end of namespace MolOps
} // end of namespace RDKit
<commit_msg>fix for 1607290 on VC8. Use resize instead of reserve in getShortestPath function for couple of std::vectors<commit_after>// $Id$
//
//  Copyright (C) 2003-2006 Rational Discovery LLC
//
//   @@ All Rights Reserved  @@
//
#include <GraphMol/ROMol.h>
#include <GraphMol/Atom.h>
#include <GraphMol/Bond.h>
#include <GraphMol/BondIterators.h>
#include <GraphMol/MolOps.h>
#include <memory.h>
#include <boost/shared_array.hpp>
#include <algorithm>

namespace RDKit{

  const int LOCAL_INF=(int)1e8;

  // local utility namespace
  namespace {
    /* ----------------------------------------------

    This implements the Floyd-Warshall all-pairs-shortest-paths
    algorithm as described on pg 564 of the White Book (Cormen, Leiserson, Rivest)

    Arguments:
    adjMat: the adjacency matrix.  This is overwritten with the shortest
    paths, should be dim x dim
    dim: the size of adjMat
    pathMat: the path matrix, should be dim x dim

    -----------------------------------------------*/
    template<class T> void
    FloydWarshall(int dim,T *adjMat,int *pathMat)
    {
      int k,i,j;
      T *currD,*lastD,*tTemp;
      int *currP,*lastP,*iTemp;

      currD = new T[dim*dim];
      currP = new int[dim*dim];
      lastD = new T[dim*dim];
      lastP = new int[dim*dim];

      memcpy(static_cast<void *>(lastD),static_cast<void *>(adjMat),dim*dim*sizeof(T));

      // initialize the paths
      for(i=0;i<dim;i++){
	int itab=i*dim;
	for(j=0;j<dim;j++){
	  if(i==j || adjMat[itab+j]==LOCAL_INF){
	    pathMat[itab+j] = -1;
	  }else{
	    pathMat[itab+j] = i;
	  }
	}
      }
      memcpy(static_cast<void *>(lastP),static_cast<void *>(pathMat),dim*dim*sizeof(int));

      for(k=0;k<dim;k++){
	int ktab=k*dim;
	for(i=0;i<dim;i++){
	  int itab=i*dim;
	  for(j=0;j<dim;j++){
	    T v1=lastD[itab+j];
	    T v2=lastD[itab+k]+lastD[ktab+j];
	    if(v1<=v2){
	      currD[itab+j] = v1;
	      currP[itab+j] = lastP[itab+j];
	    }else{
	      currD[itab+j] = v2;
	      currP[itab+j] = lastP[ktab+j];
	    }
	  }
	}
	tTemp = currD;
	currD = lastD;
	lastD = tTemp;

	iTemp = currP;
	currP = lastP;
	lastP = iTemp;
      }
      memcpy(static_cast<void *>(adjMat),static_cast<void *>(lastD),dim*dim*sizeof(T));
      memcpy(static_cast<void *>(pathMat),static_cast<void *>(lastP),dim*dim*sizeof(int));

      delete [] currD;
      delete [] currP;
      delete [] lastD;
      delete [] lastP;
    }

    template<class T> void
    FloydWarshall(int dim,T *adjMat,int *pathMat,const std::vector<int> &activeAtoms)
    {
      T *currD,*lastD,*tTemp;
      int *currP,*lastP,*iTemp;

      currD = new T[dim*dim];
      currP = new int[dim*dim];
      lastD = new T[dim*dim];
      lastP = new int[dim*dim];

      memcpy(static_cast<void *>(lastD),static_cast<void *>(adjMat),dim*dim*sizeof(T));

      // initialize the paths
      for(std::vector<int>::const_iterator i=activeAtoms.begin();
	  i!=activeAtoms.end();
	  i++){
	int itab=(*i)*dim;
	for(std::vector<int>::const_iterator j=activeAtoms.begin();
	    j!=activeAtoms.end();
	    j++){
	  if((*i)==(*j) || adjMat[itab+(*j)]==LOCAL_INF){
	    pathMat[itab+(*j)] = -1;
	  }else{
	    pathMat[itab+(*j)] = (*i);
	  }
	}
      }
      memcpy(static_cast<void *>(lastP),static_cast<void *>(pathMat),dim*dim*sizeof(int));

      for(std::vector<int>::const_iterator k=activeAtoms.begin();
	  k!=activeAtoms.end();
	  k++){
	int ktab=(*k)*dim;
	for(std::vector<int>::const_iterator i=activeAtoms.begin();
	    i!=activeAtoms.end();
	    i++){
	  int itab=(*i)*dim;
	  for(std::vector<int>::const_iterator j=activeAtoms.begin();
	      j!=activeAtoms.end();
	      j++){
	    T v1=lastD[itab+(*j)];
	    T v2=lastD[itab+(*k)]+lastD[ktab+(*j)];
	    if(v1<=v2){
	      currD[itab+(*j)] = v1;
	      currP[itab+(*j)] = lastP[itab+(*j)];
	    }else{
	      currD[itab+(*j)] = v2;
	      currP[itab+(*j)] = lastP[ktab+(*j)];
	    }
	  }
	}
	tTemp = currD;
	currD = lastD;
	lastD = tTemp;

	iTemp = currP;
	currP = lastP;
	lastP = iTemp;
      }
      memcpy(static_cast<void *>(adjMat),static_cast<void *>(lastD),dim*dim*sizeof(T));
      memcpy(static_cast<void *>(pathMat),static_cast<void *>(lastP),dim*dim*sizeof(int));

      delete [] currD;
      delete [] currP;
      delete [] lastD;
      delete [] lastP;
    }
  } // end of local utility namespace

  namespace MolOps {
    double *getDistanceMat(const ROMol &mol,bool useBO,
				   bool useAtomWts,
				   bool force,
				   const char *propNamePrefix){
      std::string propName;
      boost::shared_array<double> sptr;
      if(propNamePrefix){
	propName = propNamePrefix;
      } else {
	propName = "";
      }
      propName+="DistanceMatrix";
      // make sure we don't use the nonBO cache for the BO matrix and vice versa:
      if(useBO) propName+="BO";
      if(!force && mol.hasProp(propName)){
	mol.getProp(propName,sptr);
	return sptr.get();
      }
      int nAts=mol.getNumAtoms();
      double *dMat = new double[nAts*nAts];
      int i,j;
      // initialize off diagonals to LOCAL_INF and diagonals to 0
      for(i=0;i<nAts*nAts;i++) dMat[i] = LOCAL_INF;
      for(i=0;i<nAts;i++) dMat[i*nAts+i]=0.0;

      ROMol::EDGE_ITER firstB,lastB;
      boost::tie(firstB,lastB) = mol.getEdges();
      ROMol::GRAPH_MOL_BOND_PMAP::const_type bondMap = mol.getBondPMap();
      while(firstB!=lastB){
	const Bond *bond = bondMap[*firstB];
	i = bond->getBeginAtomIdx();
	j = bond->getEndAtomIdx();
	double contrib;
	if(useBO){
	  if(!bond->getIsAromatic()){
	    contrib = 1./bond->getBondTypeAsDouble();
	  } else {
	    contrib = 2. / 3.;
	  }
	} else {
	  contrib = 1.0;
	}
	dMat[i*nAts+j]=contrib;
	dMat[j*nAts+i]=contrib;
	firstB++;
      }

      int *pathMat = new int[nAts*nAts];
      memset(static_cast<void *>(pathMat),0,nAts*nAts*sizeof(int));
      FloydWarshall(nAts,dMat,pathMat);

      if(useAtomWts){
	for (i = 0; i < nAts; i++) {
	  int anum = mol.getAtomWithIdx(i)->getAtomicNum();
	  dMat[i*nAts+i] = 6.0/anum;
	}
      }
      sptr.reset(dMat);
      mol.setProp(propName,sptr,true);
      boost::shared_array<int> iSptr(pathMat);
      mol.setProp(propName+"_Paths",iSptr,true);

      return dMat;
    };

    double *getDistanceMat(const ROMol &mol,
				   const std::vector<int> &activeAtoms,
				   const std::vector<const Bond *> &bonds,
				   bool useBO,
				   bool useAtomWts){

      const int nAts=activeAtoms.size();

      double *dMat = new double[nAts*nAts];
      int i,j;
      // initialize off diagonals to LOCAL_INF and diagonals to 0
      for(i=0;i<nAts*nAts;i++) dMat[i] = LOCAL_INF;
      for(i=0;i<nAts;i++) dMat[i*nAts+i]=0.0;

      for(std::vector<const Bond *>::const_iterator bi=bonds.begin();
	  bi!=bonds.end();bi++){
	const Bond *bond=*bi;
	i=std::find(activeAtoms.begin(),activeAtoms.end(),
		    bond->getBeginAtomIdx()) - activeAtoms.begin();
	j=std::find(activeAtoms.begin(),activeAtoms.end(),
		    bond->getEndAtomIdx()) - activeAtoms.begin();
	double contrib;
	if(useBO){
	  if(!bond->getIsAromatic()){
	    contrib = 1./bond->getBondTypeAsDouble();
	  } else {
	    contrib = 2. / 3.;
	  }
	} else {
	  contrib = 1.0;
	}
	dMat[i*nAts+j]=contrib;
	dMat[j*nAts+i]=contrib;
      }

      int *pathMat = new int[nAts*nAts];
      memset(static_cast<void *>(pathMat),0,nAts*nAts*sizeof(int));
      FloydWarshall(nAts,dMat,pathMat);
      delete [] pathMat;

      if(useAtomWts){
	for(i=0;i<nAts;i++){
	  int anum = mol.getAtomWithIdx(activeAtoms[i])->getAtomicNum();
	  dMat[i*nAts+i] = 6.0/anum;
	}
      }
      return dMat;
    };

    // NOTE: do *not* delete results
    double *getAdjacencyMatrix(const ROMol &mol,
				       bool useBO,
				       int emptyVal,
				       bool force,
				       const char *propNamePrefix)
    {
      std::string propName;
      boost::shared_array<double> sptr;
      if(propNamePrefix){
	propName = propNamePrefix;
      } else {
	propName = "";
      }
      propName+="AdjacencyMatrix";
      if(!force && mol.hasProp(propName)){
	mol.getProp(propName,sptr);
	return sptr.get();
      }

      int nAts = mol.getNumAtoms();
      double *res = new double[nAts*nAts];
      memset(static_cast<void *>(res),emptyVal,nAts*nAts*sizeof(double));

      ROMol::ConstBondIterator bondIt;
      for(bondIt=mol.beginBonds();bondIt!=mol.endBonds();bondIt++){
	if(!useBO){
	  int beg=(*bondIt)->getBeginAtomIdx();
	  int end=(*bondIt)->getEndAtomIdx();
	  res[beg*nAts+end] = 1;
	  res[end*nAts+beg] = 1;
	}
	else {
	  int begIdx=(*bondIt)->getBeginAtomIdx();
	  int endIdx=(*bondIt)->getEndAtomIdx();
	  Atom const *beg=mol.getAtomWithIdx(begIdx);
	  Atom const *end=mol.getAtomWithIdx(endIdx);
	  res[begIdx*nAts+endIdx] = (*bondIt)->getValenceContrib(beg);
	  res[endIdx*nAts+begIdx] = (*bondIt)->getValenceContrib(end);
	}
      }
      sptr.reset(res);
      mol.setProp(propName,sptr,true);

      return res;
    };

    INT_LIST getShortestPath(const ROMol &mol, int aid1, int aid2) {

      int nats = mol.getNumAtoms();
      RANGE_CHECK(0,aid1,nats-1);
      RANGE_CHECK(0,aid2,nats-1);
      CHECK_INVARIANT(aid1 != aid2, "");

      INT_VECT pred, doneAtms;

      //pred.reserve(nats);
      //doneAtms.reserve(nats);
      pred.resize(nats);
      doneAtms.resize(nats);
      int ai;
      for (ai = 0; ai < nats; ++ai) {
	doneAtms[ai] = 0;
      }

      std::deque<int> bfsQ;

      bfsQ.push_back(aid1);
      bool done = false;
      ROMol::ADJ_ITER nbrIdx,endNbrs;
      while ((!done) && (bfsQ.size() > 0)) {
	int curAid = bfsQ.front();
	boost::tie(nbrIdx,endNbrs) = mol.getAtomNeighbors(mol.getAtomWithIdx(curAid));
	while (nbrIdx != endNbrs) {
	  if (doneAtms[*nbrIdx] == 0) {
	    pred[*nbrIdx] = curAid;
	    if ((*nbrIdx) == aid2) {
	      done = true;
	      break;
	    }
	    bfsQ.push_back(*nbrIdx);
	  }
	  nbrIdx++;
	}
	doneAtms[curAid] = 1;
	bfsQ.pop_front();
      }

      INT_LIST res;
      done = false;
      int prev = aid2;
      while (!done) {
	prev = pred[prev];
	if (prev != aid1) {
	  res.push_front(prev);
	} else {
	  done = true;
	}
      }

      return res;

    }
  } // end of namespace MolOps
} // end of namespace RDKit
<|endoftext|>


<commit_before>/*
 *  Photoconsistency-Visual-Odometry
 *  Multiscale Photoconsistency Visual Odometry from RGBD Images
 *  Copyright (c) 2012-2014, Miguel Algaba Borrego
 *
 *  http://code.google.com/p/photoconsistency-visual-odometry/
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *      * Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *      * Redistributions in binary form must reproduce the above copyright
 *        notice, this list of conditions and the following disclaimer in the
 *        documentation and/or other materials provided with the distribution.
 *      * Neither the name of the holder(s) nor the
 *        names of its contributors may be used to endorse or promote products
 *        derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 *  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 *  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#define USE_PHOTOCONSISTENCY_ODOMETRY_METHOD 0 // CPhotoconsistencyOdometryAnalytic: 0
                                               // CPhotoconsistencyOdometryCeres: 1
                                               // CPhotoconsistencyOdometryBiObjective: 2

#if USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 0
  #include "CPhotoconsistencyOdometryAnalytic.h"
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 1
  #include "CPhotoconsistencyOdometryCeres.h"
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 2
  #include "CPhotoconsistencyOdometryBiObjective.h"
#endif

#include "CSensorIdentifier.h"
#include "CSensorData.h"
#include "CCameraRecord.h"
#include "CMultiSensorData.h"
#include "CMultiSensorDataSource.h"

#include <iomanip>
#include "boost/filesystem/path.hpp"
#include "boost/filesystem/operations.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/contrib/contrib.hpp" //TickMeter

void printHelp()
{
  std::cout << "./PhotoconsistencyFrameAlignment <config_file.yml> "
            << "<rgbd_dataset_directory> "
            << "<output_trajectory_file>" << std::endl;
}

int parseInputArguments( int argc, char* argv[],
                         boost::filesystem::path & configFile,
                         boost::filesystem::path & rgbDataFile,
                         boost::filesystem::path & depthDataFile,
                         boost::filesystem::path & outputTrajectoryFile )
{
  if( argc<4 )
  {
    printHelp();
    return EXIT_FAILURE;
  }

  configFile = boost::filesystem::path( argv[1] );
  if( !boost::filesystem::exists( configFile ) )
  {
    std::cerr << "Input config file " << configFile << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  boost::filesystem::path rgbdDatasetDirectory( argv[2] );
  if( !boost::filesystem::exists( rgbdDatasetDirectory ) )
  {
    std::cerr << "Input RGBD dataset directory " << rgbdDatasetDirectory << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  rgbDataFile = rgbdDatasetDirectory / boost::filesystem::path( "rgb.txt" );
  if( !boost::filesystem::exists( rgbDataFile ) )
  {
    std::cerr << "Input RGB data file " << rgbDataFile << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  depthDataFile = rgbdDatasetDirectory / boost::filesystem::path( "depth.txt" );
  if( !boost::filesystem::exists( depthDataFile ) )
  {
    std::cerr << "Input depth data file " << depthDataFile << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  outputTrajectoryFile = boost::filesystem::path( argv[3] );
  boost::filesystem::path outputDirectory( outputTrajectoryFile.parent_path() );
  if( !boost::filesystem::exists( outputDirectory ) )
  {
    if( !boost::filesystem::create_directories( outputDirectory ) )
    {
      std::cerr << "Cannot create output directory " << outputDirectory << std::endl;
      return EXIT_FAILURE;
    }
  }

  return 0;
}

int main( int argc, char* argv[] )
{
  // Basic algebra typedefs
  typedef double CoordinateType;
  typedef phovo::Numeric::Matrix33RowMajor< CoordinateType > Matrix33Type;
  typedef phovo::Numeric::Matrix44RowMajor< CoordinateType > Matrix44Type;
  typedef phovo::Numeric::VectorCol3< CoordinateType >       Vector3Type;
  typedef phovo::Numeric::VectorCol6< CoordinateType >       Vector6Type;
  typedef Eigen::Quaternion< CoordinateType >                QuaternionType;

  // Internal sensor data typedefs
  typedef unsigned char              PixelType;
  typedef cv::Mat_< PixelType >      IntensityImageType;
  typedef cv::Mat_< CoordinateType > DepthImageType;

  // Multi-sensor data typedefs
  typedef double                                                         TimeStampType;
  typedef phovo::SensorIdentifierType                                    SensorIdentifierType;
  typedef phovo::CMultiSensorData< SensorIdentifierType, TimeStampType > MultiSensorDataType;

  // Multi-sensor data source typedefs (RGBD data record)
  typedef phovo::CSensorData< IntensityImageType, TimeStampType >        IntensityImageDataType;
  typedef phovo::CSensorData< DepthImageType, TimeStampType >            DepthImageDataType;
  typedef phovo::CCameraRecord< IntensityImageDataType, Vector6Type >    IntensityImageRecordType;
  typedef IntensityImageRecordType::ImageDataSharedPointer               IntensityImageDataSharedPointer;
  typedef phovo::CCameraRecord< DepthImageDataType, Vector6Type >        DepthImageRecordType;
  typedef DepthImageRecordType::ImageDataSharedPointer                   DepthImageDataSharedPointer;
  typedef phovo::CMultiSensorData< SensorIdentifierType, TimeStampType > MultiSensorDataType;
  typedef phovo::CMultiSensorDataSource< MultiSensorDataType >           MultiSensorDataSourceType;

  // Photo-consistency visual odometry typedefs
#if USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 0
  typedef phovo::Analytic::CPhotoconsistencyOdometryAnalytic< PixelType, CoordinateType > PhotoconsistencyVisualOdometryType;
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 1
  typedef phovo::Ceres::CPhotoconsistencyOdometryCeres< PixelType, CoordinateType > PhotoconsistencyVisualOdometryType;
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 2
  typedef phovo::Analytic::CPhotoconsistencyOdometryBiObjective< PixelType, CoordinateType > PhotoconsistencyVisualOdometryType;
#endif

  // Parse the input arguments
  boost::filesystem::path configFile;
  boost::filesystem::path rgbDataFile;
  boost::filesystem::path depthDataFile;
  boost::filesystem::path outputTrajectoryFile;
  CoordinateType depthScalingFactor = 1. / 5000.;
  if( parseInputArguments( argc, argv, configFile, rgbDataFile, depthDataFile, outputTrajectoryFile ) )
  {
    return EXIT_FAILURE;
  }

  // Set the photo-consistency visual odometry parameters
  Matrix33Type intrinsicMatrix;
  intrinsicMatrix << 517.3, 0., 318.6,
                     0., 516.5, 255.3,
                     0., 0., 1.;
  Matrix44Type pose = Matrix44Type::Identity();
  Vector6Type stateVector = Vector6Type::Zero();
  PhotoconsistencyVisualOdometryType photoconsistencyOdometry;
  photoconsistencyOdometry.ReadConfigurationFile( configFile.string() );
  photoconsistencyOdometry.SetIntrinsicMatrix( intrinsicMatrix );

  // Open the output trajectory file
  std::ofstream trajectoryFile( outputTrajectoryFile.string() );
  if( !trajectoryFile.is_open() )
  {
    std::cerr << "Cannot open output trajectory file " << outputTrajectoryFile.string() << std::endl;
    return EXIT_FAILURE;
  }
  trajectoryFile << "# estimated trajectory" << std::endl;
  trajectoryFile << "# timestamp tx ty tz qx qy qz qw" << std::endl;

  // Set multi-sensor data record and start retrieving frames
  IntensityImageRecordType::SharedPointer intensityImageRecord( new IntensityImageRecordType );
  intensityImageRecord->SetFileName( rgbDataFile.string() );
  DepthImageRecordType::SharedPointer depthImageRecord( new DepthImageRecordType );
  depthImageRecord->SetFileName( depthDataFile.string() );
  MultiSensorDataSourceType::SharedPointer multisensorDataSource( new MultiSensorDataSourceType );
  multisensorDataSource->SetSensorDataSource( phovo::IntensityCameraIdentifier, intensityImageRecord );
  multisensorDataSource->SetSensorDataSource( phovo::DepthCameraIdentifier, depthImageRecord );
  multisensorDataSource->Start();

  MultiSensorDataSourceType::MultiSensorDataSharedPointer previousIntensityDepthData =
    multisensorDataSource->GetMultiSensorData();
  if( previousIntensityDepthData )
  {
    // Extract the previous RGB and depth images from the multi-sensor data object
    IntensityImageType previousIntensityImage =
      *previousIntensityDepthData->GetData< IntensityImageDataType >( phovo::IntensityCameraIdentifier )->GetData();
    DepthImageType previousDepthImage =
      *previousIntensityDepthData->GetData< DepthImageDataType >( phovo::DepthCameraIdentifier )->GetData() * depthScalingFactor;

    MultiSensorDataSourceType::MultiSensorDataSharedPointer currentIntensityDepthData =
      multisensorDataSource->GetMultiSensorData();
    while( currentIntensityDepthData && ( cv::waitKey(5) != 'q' ) )
    {
      // Extract the current RGB and depth images from the multi-sensor data object
      IntensityImageDataSharedPointer currentIntensityImageData =
        currentIntensityDepthData->GetData< IntensityImageDataType >( phovo::IntensityCameraIdentifier );
      IntensityImageType currentIntensityImage = *currentIntensityImageData->GetData();
      DepthImageDataSharedPointer currentDepthImageData =
        currentIntensityDepthData->GetData< DepthImageDataType >( phovo::DepthCameraIdentifier );
      DepthImageType currentDepthImage = *currentDepthImageData->GetData() * depthScalingFactor;

      photoconsistencyOdometry.SetSourceFrame( previousIntensityImage, previousDepthImage );
      photoconsistencyOdometry.SetTargetFrame( currentIntensityImage, previousDepthImage );
      photoconsistencyOdometry.SetInitialStateVector( stateVector );

      // Optimize the problem to estimate the rigid transformation
      cv::TickMeter tm;tm.start();
      photoconsistencyOdometry.Optimize();
      tm.stop();
      std::cout << "Time = " << tm.getTimeSec() << " sec." << std::endl;

      // Update the global pose of the sensor
      Matrix44Type Rt = photoconsistencyOdometry.GetOptimalRigidTransformationMatrix();
      pose *= Rt.inverse();
      Matrix33Type R = pose.block( 0, 0, 3, 3 );
      Vector3Type t = pose.block( 0, 3, 3, 1 );
      QuaternionType q( R );

      // Write the current timestamped pose to the output trajectory file
      trajectoryFile << std::setprecision( std::numeric_limits< TimeStampType >::digits10 + 1 )
                     << currentIntensityImageData->GetTimeStamp() << " "
                     << t( 0 ) << " " << t( 1 ) << " " << t( 2 ) << " "
                     << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;

      // Show results
      std::cout << "Rt:" << std::endl << Rt << std::endl;
      IntensityImageType warpedImage;
      phovo::warpImage< PixelType, CoordinateType >( previousIntensityImage,
                                                     previousDepthImage,
                                                     warpedImage, Rt, intrinsicMatrix );
      IntensityImageType imgDiff;
      cv::absdiff( currentIntensityImage, warpedImage, imgDiff );
      cv::imshow( "imgDiff", imgDiff );

      // Update the previous and current frames
      previousIntensityImage = currentIntensityImage;
      previousDepthImage = currentDepthImage;
      currentIntensityDepthData = multisensorDataSource->GetMultiSensorData();
    }
  }

  // Stop grabbing sensor frames and close the output trajectory file
  multisensorDataSource->Stop();
  trajectoryFile.close();

  return EXIT_SUCCESS;
}

<commit_msg>Bug-fix: using the current depth image<commit_after>/*
 *  Photoconsistency-Visual-Odometry
 *  Multiscale Photoconsistency Visual Odometry from RGBD Images
 *  Copyright (c) 2012-2014, Miguel Algaba Borrego
 *
 *  http://code.google.com/p/photoconsistency-visual-odometry/
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *      * Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *      * Redistributions in binary form must reproduce the above copyright
 *        notice, this list of conditions and the following disclaimer in the
 *        documentation and/or other materials provided with the distribution.
 *      * Neither the name of the holder(s) nor the
 *        names of its contributors may be used to endorse or promote products
 *        derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 *  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 *  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#define USE_PHOTOCONSISTENCY_ODOMETRY_METHOD 0 // CPhotoconsistencyOdometryAnalytic: 0
                                               // CPhotoconsistencyOdometryCeres: 1
                                               // CPhotoconsistencyOdometryBiObjective: 2

#if USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 0
  #include "CPhotoconsistencyOdometryAnalytic.h"
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 1
  #include "CPhotoconsistencyOdometryCeres.h"
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 2
  #include "CPhotoconsistencyOdometryBiObjective.h"
#endif

#include "CSensorIdentifier.h"
#include "CSensorData.h"
#include "CCameraRecord.h"
#include "CMultiSensorData.h"
#include "CMultiSensorDataSource.h"

#include <iomanip>
#include "boost/filesystem/path.hpp"
#include "boost/filesystem/operations.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/contrib/contrib.hpp" //TickMeter

void printHelp()
{
  std::cout << "./PhotoconsistencyFrameAlignment <config_file.yml> "
            << "<rgbd_dataset_directory> "
            << "<output_trajectory_file>" << std::endl;
}

int parseInputArguments( int argc, char* argv[],
                         boost::filesystem::path & configFile,
                         boost::filesystem::path & rgbDataFile,
                         boost::filesystem::path & depthDataFile,
                         boost::filesystem::path & outputTrajectoryFile )
{
  if( argc<4 )
  {
    printHelp();
    return EXIT_FAILURE;
  }

  configFile = boost::filesystem::path( argv[1] );
  if( !boost::filesystem::exists( configFile ) )
  {
    std::cerr << "Input config file " << configFile << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  boost::filesystem::path rgbdDatasetDirectory( argv[2] );
  if( !boost::filesystem::exists( rgbdDatasetDirectory ) )
  {
    std::cerr << "Input RGBD dataset directory " << rgbdDatasetDirectory << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  rgbDataFile = rgbdDatasetDirectory / boost::filesystem::path( "rgb.txt" );
  if( !boost::filesystem::exists( rgbDataFile ) )
  {
    std::cerr << "Input RGB data file " << rgbDataFile << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  depthDataFile = rgbdDatasetDirectory / boost::filesystem::path( "depth.txt" );
  if( !boost::filesystem::exists( depthDataFile ) )
  {
    std::cerr << "Input depth data file " << depthDataFile << " does not exist" << std::endl;
    return EXIT_FAILURE;
  }

  outputTrajectoryFile = boost::filesystem::path( argv[3] );
  boost::filesystem::path outputDirectory( outputTrajectoryFile.parent_path() );
  if( !boost::filesystem::exists( outputDirectory ) )
  {
    if( !boost::filesystem::create_directories( outputDirectory ) )
    {
      std::cerr << "Cannot create output directory " << outputDirectory << std::endl;
      return EXIT_FAILURE;
    }
  }

  return 0;
}

int main( int argc, char* argv[] )
{
  // Basic algebra typedefs
  typedef double CoordinateType;
  typedef phovo::Numeric::Matrix33RowMajor< CoordinateType > Matrix33Type;
  typedef phovo::Numeric::Matrix44RowMajor< CoordinateType > Matrix44Type;
  typedef phovo::Numeric::VectorCol3< CoordinateType >       Vector3Type;
  typedef phovo::Numeric::VectorCol6< CoordinateType >       Vector6Type;
  typedef Eigen::Quaternion< CoordinateType >                QuaternionType;

  // Internal sensor data typedefs
  typedef unsigned char              PixelType;
  typedef cv::Mat_< PixelType >      IntensityImageType;
  typedef cv::Mat_< CoordinateType > DepthImageType;

  // Multi-sensor data typedefs
  typedef double                                                         TimeStampType;
  typedef phovo::SensorIdentifierType                                    SensorIdentifierType;
  typedef phovo::CMultiSensorData< SensorIdentifierType, TimeStampType > MultiSensorDataType;

  // Multi-sensor data source typedefs (RGBD data record)
  typedef phovo::CSensorData< IntensityImageType, TimeStampType >        IntensityImageDataType;
  typedef phovo::CSensorData< DepthImageType, TimeStampType >            DepthImageDataType;
  typedef phovo::CCameraRecord< IntensityImageDataType, Vector6Type >    IntensityImageRecordType;
  typedef IntensityImageRecordType::ImageDataSharedPointer               IntensityImageDataSharedPointer;
  typedef phovo::CCameraRecord< DepthImageDataType, Vector6Type >        DepthImageRecordType;
  typedef DepthImageRecordType::ImageDataSharedPointer                   DepthImageDataSharedPointer;
  typedef phovo::CMultiSensorData< SensorIdentifierType, TimeStampType > MultiSensorDataType;
  typedef phovo::CMultiSensorDataSource< MultiSensorDataType >           MultiSensorDataSourceType;

  // Photo-consistency visual odometry typedefs
#if USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 0
  typedef phovo::Analytic::CPhotoconsistencyOdometryAnalytic< PixelType, CoordinateType > PhotoconsistencyVisualOdometryType;
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 1
  typedef phovo::Ceres::CPhotoconsistencyOdometryCeres< PixelType, CoordinateType > PhotoconsistencyVisualOdometryType;
#elif USE_PHOTOCONSISTENCY_ODOMETRY_METHOD == 2
  typedef phovo::Analytic::CPhotoconsistencyOdometryBiObjective< PixelType, CoordinateType > PhotoconsistencyVisualOdometryType;
#endif

  // Parse the input arguments
  boost::filesystem::path configFile;
  boost::filesystem::path rgbDataFile;
  boost::filesystem::path depthDataFile;
  boost::filesystem::path outputTrajectoryFile;
  CoordinateType depthScalingFactor = 1. / 5000.;
  if( parseInputArguments( argc, argv, configFile, rgbDataFile, depthDataFile, outputTrajectoryFile ) )
  {
    return EXIT_FAILURE;
  }

  // Set the photo-consistency visual odometry parameters
  Matrix33Type intrinsicMatrix;
  intrinsicMatrix << 517.3, 0., 318.6,
                     0., 516.5, 255.3,
                     0., 0., 1.;
  Matrix44Type pose = Matrix44Type::Identity();
  Vector6Type stateVector = Vector6Type::Zero();
  PhotoconsistencyVisualOdometryType photoconsistencyOdometry;
  photoconsistencyOdometry.ReadConfigurationFile( configFile.string() );
  photoconsistencyOdometry.SetIntrinsicMatrix( intrinsicMatrix );

  // Open the output trajectory file
  std::ofstream trajectoryFile( outputTrajectoryFile.string() );
  if( !trajectoryFile.is_open() )
  {
    std::cerr << "Cannot open output trajectory file " << outputTrajectoryFile.string() << std::endl;
    return EXIT_FAILURE;
  }
  trajectoryFile << "# estimated trajectory" << std::endl;
  trajectoryFile << "# timestamp tx ty tz qx qy qz qw" << std::endl;

  // Set multi-sensor data record and start retrieving frames
  IntensityImageRecordType::SharedPointer intensityImageRecord( new IntensityImageRecordType );
  intensityImageRecord->SetFileName( rgbDataFile.string() );
  DepthImageRecordType::SharedPointer depthImageRecord( new DepthImageRecordType );
  depthImageRecord->SetFileName( depthDataFile.string() );
  MultiSensorDataSourceType::SharedPointer multisensorDataSource( new MultiSensorDataSourceType );
  multisensorDataSource->SetSensorDataSource( phovo::IntensityCameraIdentifier, intensityImageRecord );
  multisensorDataSource->SetSensorDataSource( phovo::DepthCameraIdentifier, depthImageRecord );
  multisensorDataSource->Start();

  MultiSensorDataSourceType::MultiSensorDataSharedPointer previousIntensityDepthData =
    multisensorDataSource->GetMultiSensorData();
  if( previousIntensityDepthData )
  {
    // Extract the previous RGB and depth images from the multi-sensor data object
    IntensityImageType previousIntensityImage =
      *previousIntensityDepthData->GetData< IntensityImageDataType >( phovo::IntensityCameraIdentifier )->GetData();
    DepthImageType previousDepthImage =
      *previousIntensityDepthData->GetData< DepthImageDataType >( phovo::DepthCameraIdentifier )->GetData() * depthScalingFactor;

    MultiSensorDataSourceType::MultiSensorDataSharedPointer currentIntensityDepthData =
      multisensorDataSource->GetMultiSensorData();
    while( currentIntensityDepthData && ( cv::waitKey(5) != 'q' ) )
    {
      // Extract the current RGB and depth images from the multi-sensor data object
      IntensityImageDataSharedPointer currentIntensityImageData =
        currentIntensityDepthData->GetData< IntensityImageDataType >( phovo::IntensityCameraIdentifier );
      IntensityImageType currentIntensityImage = *currentIntensityImageData->GetData();
      DepthImageDataSharedPointer currentDepthImageData =
        currentIntensityDepthData->GetData< DepthImageDataType >( phovo::DepthCameraIdentifier );
      DepthImageType currentDepthImage = *currentDepthImageData->GetData() * depthScalingFactor;

      photoconsistencyOdometry.SetSourceFrame( previousIntensityImage, previousDepthImage );
      photoconsistencyOdometry.SetTargetFrame( currentIntensityImage, currentDepthImage );
      photoconsistencyOdometry.SetInitialStateVector( stateVector );

      // Optimize the problem to estimate the rigid transformation
      cv::TickMeter tm;tm.start();
      photoconsistencyOdometry.Optimize();
      tm.stop();
      std::cout << "Time = " << tm.getTimeSec() << " sec." << std::endl;

      // Update the global pose of the sensor
      Matrix44Type Rt = photoconsistencyOdometry.GetOptimalRigidTransformationMatrix();
      pose *= Rt.inverse();
      Matrix33Type R = pose.block( 0, 0, 3, 3 );
      Vector3Type t = pose.block( 0, 3, 3, 1 );
      QuaternionType q( R );

      // Write the current timestamped pose to the output trajectory file
      trajectoryFile << std::setprecision( std::numeric_limits< TimeStampType >::digits10 + 1 )
                     << currentIntensityImageData->GetTimeStamp() << " "
                     << t( 0 ) << " " << t( 1 ) << " " << t( 2 ) << " "
                     << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;

      // Show results
      std::cout << "Rt:" << std::endl << Rt << std::endl;
      IntensityImageType warpedImage;
      phovo::warpImage< PixelType, CoordinateType >( previousIntensityImage,
                                                     previousDepthImage,
                                                     warpedImage, Rt, intrinsicMatrix );
      IntensityImageType imgDiff;
      cv::absdiff( currentIntensityImage, warpedImage, imgDiff );
      cv::imshow( "imgDiff", imgDiff );

      // Update the previous and current frames
      previousIntensityImage = currentIntensityImage;
      previousDepthImage = currentDepthImage;
      currentIntensityDepthData = multisensorDataSource->GetMultiSensorData();
    }
  }

  // Stop grabbing sensor frames and close the output trajectory file
  multisensorDataSource->Stop();
  trajectoryFile.close();

  return EXIT_SUCCESS;
}

<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    ThresholdImageFilter.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

// Software Guide : BeginLatex
//
// \begin{figure}
// \center
// \includegraphics[height=5cm]{ThresholdTransferFunctionBelow.eps}
// \includegraphics[height=5cm]{ThresholdImageFilterOutputBelow.eps}
// \caption{Effect of using the threshold-below mode.}
// \label{fig:ThresholdTransferFunctionBelow}
// \end{figure}
//
// \begin{figure}
// \center
// \includegraphics[height=5cm]{ThresholdTransferFunctionAbove.eps}
// \includegraphics[height=5cm]{ThresholdImageFilterOutputAbove.eps}
// \caption{Effect of using the threshold-above mode.}
// \label{fig:ThresholdTransferFunctionAbove}
// \end{figure}
//
// \begin{figure}
// \center
// \includegraphics[height=5cm]{ThresholdTransferFunctionOutside.eps}
// \includegraphics[height=5cm]{ThresholdImageFilterOutputOutside.eps}
// \caption{Effect of using the threshold-outside mode.}
// \label{fig:ThresholdTransferFunctionOutside}
// \end{figure}
//
// This example illustrates the use of the \code{ThresholdImageFilter}.  This
// filter can be used to transform the intensity levels of an image in three
// different ways.
//
// \begin{itemize}
//
// \item First, the user can define a particular threshold , all the pixels
// with values below this threshold will be replaced by a user defined value
// called here the \code{OutsideValue}.  Pixels with values above the threshold
// remain unchanged. This is illustrated in figure
// \ref{fig:ThresholdTransferFunctionBelow}.
//
// \item Second, the user can define a particular threshold , all the pixels
// with values above this threshold will be replaced by the
// \code{OutsideValue}.  Pixels with values below the threshold remain
// unchanged. This is illustrated in figure
// \ref{fig:ThresholdTransferFunctionAbove}.
//
// \item Third, the user can provide two thresholds. All the pixels with
// intensity values inside the range defined by the two thresholds will remain
// unchanged. Pixels with values outside this range will be assigned to the
// \code{OutsideValue}. This is illustrated in figure
// \ref{fig:ThresholdTransferFunctionOutside}.
//
// \end{itemize}
//
// The mode of operation of the filter is the defined by calling one of the
// following methods
//
// \begin{itemize}
// \item \texttt{ThresholdBelow()}
// \item \texttt{ThresholdAbove()}
// \item \texttt{ThresholdOutside()}
// \end{itemize}
//
// \index{itk::ThresholdImageFilter!Instantiation|textbf}
// \index{itk::ThresholdImageFilter!Header|textbf}
// \index{itk::ThresholdImageFilter!ThresholdAbove()}
// \index{itk::ThresholdImageFilter!ThresholdBelow()}
// \index{itk::ThresholdImageFilter!ThresholdOutside()}
//
// The first step required to use this filter is to include its header file.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
#include "itkThresholdImageFilter.h"
// Software Guide : EndCodeSnippet

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"

int main( int argc, char ** argv )
{

  if( argc < 5 )
    {
    std::cerr << "Usage: " << argv[0] << " inputImageFile ";
    std::cerr << " outputImageFile1 outputImageFile2 outputImageFile3" << std::endl;
    return 1;
    }

  //  Software Guide : BeginLatex
  //
  //  Then we must decide what pixel type to use for the image. This filter is
  //  templated over a single image type since the output image must be able to
  //  represent the unchanged pixels.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef  unsigned char  PixelType;
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The image type is now defined using the pixel type and the dimension.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef itk::Image< PixelType,  3 >   ImageType;
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The filter type can be instantiated using the image type defined above.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef itk::ThresholdImageFilter< ImageType >  FilterType;
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  An \code{ImageFileReader} class is also instantiated in order to read
  //  image data from a file.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef itk::ImageFileReader< ImageType >  ReaderType;
  // Software Guide : EndCodeSnippet

  // An ImageFileWriter is instantiated in order to write the output image to a
  // file.
  typedef itk::ImageFileWriter< ImageType >  WriterType;

  //  Software Guide : BeginLatex
  //
  //  Both the filter and the reader are created by invoking their \code{New()}
  //  methods and assigning the result to SmartPointers.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  ReaderType::Pointer reader = ReaderType::New();
  FilterType::Pointer filter = FilterType::New();
  // Software Guide : EndCodeSnippet

  WriterType::Pointer writer = WriterType::New();

  writer->SetInput( filter->GetOutput() );

  reader->SetFileName( argv[1] );

  //  Software Guide : BeginLatex
  //
  //  The image obtained with the reader is passed as input to the
  //  \code{ThresholdImageFilter}.
  //
  //  \index{itk::ThresholdImageFilter!SetInput()}
  //  \index{itk::FileImageReader!GetOutput()}
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->SetInput( reader->GetOutput() );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The method \code{SetOutsideValue()} defines the intensity value to be
  //  assigned to those pixels whose intensities are outside the range defined
  //  by the lower and upper thresholds.
  //
  //  \index{itk::ThresholdImageFilter!SetOutsideValue()}
  //  \index{SetOutsideValue()!itk::ThresholdImageFilter}
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->SetOutsideValue( 0 );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The method \code{ThresholdBelow()} define the intensity value below which
  //  pixels  of the input image will be transformed into the \code{OutsideValue}.
  //
  //  \index{itk::ThresholdImageFilter!ThresholdBelow()|textbf}
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->ThresholdBelow( 180 );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The execution of the filter is triggered by invoking the \code{Update()}
  //  method. If the filter's output has been passed as input to subsequent
  //  filters, the \code{Update()} call on any of the posterior filters in the
  //  pipeline will indirectly trigger the update of this filter too.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->Update();
  // Software Guide : EndCodeSnippet

  writer->SetFileName( argv[2] );
  writer->Update();

  //  Software Guide : BeginLatex
  //
  //  The output of this process is shown on figure
  //  \ref{fig:ThresholdTransferFunctionBelow}.  The second mode of operation
  //  of the filter is enabled by invoking the method \code{ThresholdAbove()}
  //  and then triggering the update of the filter.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->ThresholdAbove( 180 );
  filter->Update();
  // Software Guide : EndCodeSnippet

  writer->SetFileName( argv[3] );
  writer->Update();

  //  Software Guide : BeginLatex
  //
  //  The output of this process is shown on figure
  //  \ref{fig:ThresholdTransferFunctionAbove}.  The third mode of operation
  //  of the filter is enabled by invoking the method \code{ThresholdOutside()}
  //  and then triggering the update of the filter.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->ThresholdOutside( 170,190 );
  filter->Update();
  // Software Guide : EndCodeSnippet

  writer->SetFileName( argv[4] );
  writer->Update();

  //  Software Guide : BeginLatex
  //
  //  The output of this process is shown on figure
  //  \ref{fig:ThresholdTransferFunctionOutside}.  The examples in this section
  //  also illustrate the limitations of this filter for performing
  //  segmentation by itself. These limitations are particularly noticeable in
  //  noisy images and in images lacking spatial uniformity as is the case of
  //  MRI due to field bias.
  //
  //  Software Guide : EndLatex

  return 0;

}

<commit_msg>ENH: Set to 2D for generating images for the software guide<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    ThresholdImageFilter.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

// Software Guide : BeginLatex
//
// \begin{figure}
// \center
// \includegraphics[height=5cm]{ThresholdTransferFunctionBelow.eps}
// \includegraphics[height=5cm]{ThresholdImageFilterOutputBelow.eps}
// \caption{Effect of using the threshold-below mode.}
// \label{fig:ThresholdTransferFunctionBelow}
// \end{figure}
//
// \begin{figure}
// \center
// \includegraphics[height=5cm]{ThresholdTransferFunctionAbove.eps}
// \includegraphics[height=5cm]{ThresholdImageFilterOutputAbove.eps}
// \caption{Effect of using the threshold-above mode.}
// \label{fig:ThresholdTransferFunctionAbove}
// \end{figure}
//
// \begin{figure}
// \center
// \includegraphics[height=5cm]{ThresholdTransferFunctionOutside.eps}
// \includegraphics[height=5cm]{ThresholdImageFilterOutputOutside.eps}
// \caption{Effect of using the threshold-outside mode.}
// \label{fig:ThresholdTransferFunctionOutside}
// \end{figure}
//
// This example illustrates the use of the \code{ThresholdImageFilter}.  This
// filter can be used to transform the intensity levels of an image in three
// different ways.
//
// \begin{itemize}
//
// \item First, the user can define a particular threshold , all the pixels
// with values below this threshold will be replaced by a user defined value
// called here the \code{OutsideValue}.  Pixels with values above the threshold
// remain unchanged. This is illustrated in figure
// \ref{fig:ThresholdTransferFunctionBelow}.
//
// \item Second, the user can define a particular threshold , all the pixels
// with values above this threshold will be replaced by the
// \code{OutsideValue}.  Pixels with values below the threshold remain
// unchanged. This is illustrated in figure
// \ref{fig:ThresholdTransferFunctionAbove}.
//
// \item Third, the user can provide two thresholds. All the pixels with
// intensity values inside the range defined by the two thresholds will remain
// unchanged. Pixels with values outside this range will be assigned to the
// \code{OutsideValue}. This is illustrated in figure
// \ref{fig:ThresholdTransferFunctionOutside}.
//
// \end{itemize}
//
// The mode of operation of the filter is the defined by calling one of the
// following methods
//
// \begin{itemize}
// \item \texttt{ThresholdBelow()}
// \item \texttt{ThresholdAbove()}
// \item \texttt{ThresholdOutside()}
// \end{itemize}
//
// \index{itk::ThresholdImageFilter!Instantiation|textbf}
// \index{itk::ThresholdImageFilter!Header|textbf}
// \index{itk::ThresholdImageFilter!ThresholdAbove()}
// \index{itk::ThresholdImageFilter!ThresholdBelow()}
// \index{itk::ThresholdImageFilter!ThresholdOutside()}
//
// The first step required to use this filter is to include its header file.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
#include "itkThresholdImageFilter.h"
// Software Guide : EndCodeSnippet

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"

int main( int argc, char ** argv )
{

  if( argc < 5 )
    {
    std::cerr << "Usage: " << argv[0] << " inputImageFile ";
    std::cerr << " outputImageFile1 outputImageFile2 outputImageFile3" << std::endl;
    return 1;
    }

  //  Software Guide : BeginLatex
  //
  //  Then we must decide what pixel type to use for the image. This filter is
  //  templated over a single image type since the output image must be able to
  //  represent the unchanged pixels.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef  unsigned char  PixelType;
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The image type is now defined using the pixel type and the dimension.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef itk::Image< PixelType,  2 >   ImageType;
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The filter type can be instantiated using the image type defined above.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef itk::ThresholdImageFilter< ImageType >  FilterType;
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  An \code{ImageFileReader} class is also instantiated in order to read
  //  image data from a file.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef itk::ImageFileReader< ImageType >  ReaderType;
  // Software Guide : EndCodeSnippet

  // An ImageFileWriter is instantiated in order to write the output image to a
  // file.
  typedef itk::ImageFileWriter< ImageType >  WriterType;

  //  Software Guide : BeginLatex
  //
  //  Both the filter and the reader are created by invoking their \code{New()}
  //  methods and assigning the result to SmartPointers.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  ReaderType::Pointer reader = ReaderType::New();
  FilterType::Pointer filter = FilterType::New();
  // Software Guide : EndCodeSnippet

  WriterType::Pointer writer = WriterType::New();

  writer->SetInput( filter->GetOutput() );

  reader->SetFileName( argv[1] );

  //  Software Guide : BeginLatex
  //
  //  The image obtained with the reader is passed as input to the
  //  \code{ThresholdImageFilter}.
  //
  //  \index{itk::ThresholdImageFilter!SetInput()}
  //  \index{itk::FileImageReader!GetOutput()}
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->SetInput( reader->GetOutput() );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The method \code{SetOutsideValue()} defines the intensity value to be
  //  assigned to those pixels whose intensities are outside the range defined
  //  by the lower and upper thresholds.
  //
  //  \index{itk::ThresholdImageFilter!SetOutsideValue()}
  //  \index{SetOutsideValue()!itk::ThresholdImageFilter}
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->SetOutsideValue( 0 );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The method \code{ThresholdBelow()} define the intensity value below which
  //  pixels  of the input image will be transformed into the \code{OutsideValue}.
  //
  //  \index{itk::ThresholdImageFilter!ThresholdBelow()|textbf}
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->ThresholdBelow( 180 );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The execution of the filter is triggered by invoking the \code{Update()}
  //  method. If the filter's output has been passed as input to subsequent
  //  filters, the \code{Update()} call on any of the posterior filters in the
  //  pipeline will indirectly trigger the update of this filter too.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->Update();
  // Software Guide : EndCodeSnippet

  writer->SetFileName( argv[2] );
  writer->Update();

  //  Software Guide : BeginLatex
  //
  //  The output of this process is shown on figure
  //  \ref{fig:ThresholdTransferFunctionBelow}.  The second mode of operation
  //  of the filter is enabled by invoking the method \code{ThresholdAbove()}
  //  and then triggering the update of the filter.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->ThresholdAbove( 180 );
  filter->Update();
  // Software Guide : EndCodeSnippet

  writer->SetFileName( argv[3] );
  writer->Update();

  //  Software Guide : BeginLatex
  //
  //  The output of this process is shown on figure
  //  \ref{fig:ThresholdTransferFunctionAbove}.  The third mode of operation
  //  of the filter is enabled by invoking the method \code{ThresholdOutside()}
  //  and then triggering the update of the filter.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->ThresholdOutside( 170,190 );
  filter->Update();
  // Software Guide : EndCodeSnippet

  writer->SetFileName( argv[4] );
  writer->Update();

  //  Software Guide : BeginLatex
  //
  //  The output of this process is shown on figure
  //  \ref{fig:ThresholdTransferFunctionOutside}.  The examples in this section
  //  also illustrate the limitations of this filter for performing
  //  segmentation by itself. These limitations are particularly noticeable in
  //  noisy images and in images lacking spatial uniformity as is the case of
  //  MRI due to field bias.
  //
  //  Software Guide : EndLatex

  return 0;

}

<|endoftext|>


<commit_before>AliAnalysisTask *AddTask_mkoehler_CCbar(Bool_t hasMC_aod = kFALSE){
  //get the current analysis manager
  AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
  if (!mgr) {
    ::Error("AddTask_mkoehler_CCbar", "No analysis manager found.");
    return NULL;
  }
  if (!mgr->GetInputEventHandler()) {
    ::Error("AddTask_mkoehler_CCbar", "This task requires an input event handler");
    return NULL;
  }

  //Do we have an MC handler?
  Bool_t hasMC=(mgr->GetMCtruthEventHandler()!=0x0);

  TString configFile("ConfigCCbar_mk_pp.C");
  if (hasMC) configFile="$ALICE_ROOT/PWGDQ/dielectron/macros/ConfigJpsi2eeEff.C";//check later
  Bool_t isAOD=mgr->GetInputEventHandler()->IsA()==AliAODInputHandler::Class();

  //create task and add it to the manager
  AliAnalysisTaskMultiDielectron *task=new AliAnalysisTaskMultiDielectron("MultiDie");
  mgr->AddTask(task);

  //load dielectron configuration file
  TString checkconfig="ConfigCCbar_mk_pp";
  if (!gROOT->GetListOfGlobalFunctions()->FindObject(checkconfig.Data()))
    gROOT->LoadMacro(configFile.Data());

  //add dielectron analysis with different cuts to the task
  for (Int_t i=0; i<nDie; ++i){ //nDie defined in config file
    AliDielectron *jpsi=ConfigCCbar_mk_pp(i,isAOD);
    if (isAOD) jpsi->SetHasMC(hasMC_aod);
    if (jpsi) task->AddDielectron(jpsi);
  }

  //Add event filter
  AliDielectronEventCuts *eventCuts=new AliDielectronEventCuts("eventCuts","Vertex Track && |vtxZ|<10 && ncontrib>0");
  if(isAOD) eventCuts->SetVertexType(AliDielectronEventCuts::kVtxAny);
  eventCuts->SetRequireVertex();
  eventCuts->SetMinVtxContributors(1);
  eventCuts->SetVertexZ(-10.,10.);
  task->SetEventFilter(eventCuts);

  // pileup rejection
//    task->SetRejectPileup();
   task->SetTriggerMask(AliVEvent::kEMC7);
   task->UsePhysicsSelection();
  //----------------------
  //create data containers
  //----------------------

  //create output container

  AliAnalysisDataContainer *cOutputHist1 =
    mgr->CreateContainer("mkoehler_pp_QA", TList::Class(), AliAnalysisManager::kOutputContainer,
                         "LMEEoutput.root");

  AliAnalysisDataContainer *cOutputHist2 =
    mgr->CreateContainer("mkoehler_pp_CF", TList::Class(), AliAnalysisManager::kOutputContainer,
                         "LMEEoutput.root");

  AliAnalysisDataContainer *cOutputHist3 =
    mgr->CreateContainer("mkoehler_pp_EventStat", TH1D::Class(), AliAnalysisManager::kOutputContainer,
                        "LMEEoutput.root");

  mgr->ConnectInput(task,  0, mgr->GetCommonInputContainer());
  mgr->ConnectOutput(task, 1, cOutputHist1);
  mgr->ConnectOutput(task, 2, cOutputHist2);
  mgr->ConnectOutput(task, 3, cOutputHist3);

  return task;
}
<commit_msg>-correct path to config (markus)<commit_after>AliAnalysisTask *AddTask_mkoehler_CCbar(Bool_t hasMC_aod = kFALSE){
  //get the current analysis manager
  AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
  if (!mgr) {
    ::Error("AddTask_mkoehler_CCbar", "No analysis manager found.");
    return NULL;
  }
  if (!mgr->GetInputEventHandler()) {
    ::Error("AddTask_mkoehler_CCbar", "This task requires an input event handler");
    return NULL;
  }

  //Do we have an MC handler?
  Bool_t hasMC=(mgr->GetMCtruthEventHandler()!=0x0);

  TString configFile("$ALICE_ROOT/PWGDQ/dielectron/macrosLMEE/ConfigCCbar_mk_pp.C");
  if (hasMC) configFile="$ALICE_ROOT/PWGDQ/dielectron/macros/ConfigJpsi2eeEff.C";//check later
  Bool_t isAOD=mgr->GetInputEventHandler()->IsA()==AliAODInputHandler::Class();

  //create task and add it to the manager
  AliAnalysisTaskMultiDielectron *task=new AliAnalysisTaskMultiDielectron("MultiDie");
  mgr->AddTask(task);

  //load dielectron configuration file
  TString checkconfig="ConfigCCbar_mk_pp";
  if (!gROOT->GetListOfGlobalFunctions()->FindObject(checkconfig.Data()))
    gROOT->LoadMacro(configFile.Data());

  //add dielectron analysis with different cuts to the task
  for (Int_t i=0; i<nDie; ++i){ //nDie defined in config file
    AliDielectron *jpsi=ConfigCCbar_mk_pp(i,isAOD);
    if (isAOD) jpsi->SetHasMC(hasMC_aod);
    if (jpsi) task->AddDielectron(jpsi);
  }

  //Add event filter
  AliDielectronEventCuts *eventCuts=new AliDielectronEventCuts("eventCuts","Vertex Track && |vtxZ|<10 && ncontrib>0");
  if(isAOD) eventCuts->SetVertexType(AliDielectronEventCuts::kVtxAny);
  eventCuts->SetRequireVertex();
  eventCuts->SetMinVtxContributors(1);
  eventCuts->SetVertexZ(-10.,10.);
  task->SetEventFilter(eventCuts);

  // pileup rejection
//    task->SetRejectPileup();
   task->SetTriggerMask(AliVEvent::kEMC7);
   task->UsePhysicsSelection();
  //----------------------
  //create data containers
  //----------------------

  //create output container

  AliAnalysisDataContainer *cOutputHist1 =
    mgr->CreateContainer("mkoehler_pp_QA", TList::Class(), AliAnalysisManager::kOutputContainer,
                         "LMEEoutput.root");

  AliAnalysisDataContainer *cOutputHist2 =
    mgr->CreateContainer("mkoehler_pp_CF", TList::Class(), AliAnalysisManager::kOutputContainer,
                         "LMEEoutput.root");

  AliAnalysisDataContainer *cOutputHist3 =
    mgr->CreateContainer("mkoehler_pp_EventStat", TH1D::Class(), AliAnalysisManager::kOutputContainer,
                        "LMEEoutput.root");

  mgr->ConnectInput(task,  0, mgr->GetCommonInputContainer());
  mgr->ConnectOutput(task, 1, cOutputHist1);
  mgr->ConnectOutput(task, 2, cOutputHist2);
  mgr->ConnectOutput(task, 3, cOutputHist3);

  return task;
}
<|endoftext|>