<commit_before>/*************************************************************************
 *
 *  $RCSfile: bootstrap.hxx,v $
 *
 *  $Revision: 1.12 $
 *
 *  last change: $Author: jb $ $Date: 2001-04-05 14:31: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 CONFIGMGR_BOOTSTRAP_HXX_
#define CONFIGMGR_BOOTSTRAP_HXX_

#ifndef _COM_SUN_STAR_UNO_REFERENCE_HXX_
#include <com/sun/star/uno/Reference.hxx>
#endif
#ifndef _COM_SUN_STAR_UNO_SEQUENCE_HXX_
#include <com/sun/star/uno/Sequence.hxx>
#endif
#ifndef _COM_SUN_STAR_UNO_ANY_HXX_
#include <com/sun/star/uno/Any.hxx>
#endif
#ifndef _COM_SUN_STAR_LANG_ILLEGALARGUMENTEXCEPTION_HPP_
#include <com/sun/star/lang/IllegalArgumentException.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XMULTISERVICEFACTORY_HPP_
#include <com/sun/star/lang/XMultiServiceFactory.hpp>
#endif
#ifndef _RTL_USTRING_HXX_
#include <rtl/ustring.hxx>
#endif
#ifndef _COMPHELPER_STLTYPES_HXX_
#include <comphelper/stl_types.hxx>
#endif

namespace osl {
    class Profile;
}

namespace configmgr
{
    class IConfigSession;

    // ===================================================================================

    #define PORTAL_SESSION_IDENTIFIER           "portal"
    #define REMOTE_SESSION_IDENTIFIER           "remote"
    #define LOCAL_SESSION_IDENTIFIER            "local"
    #define SETUP_SESSION_IDENTIFIER            "setup"
    #define PLUGIN_SESSION_IDENTIFIER           "plugin"

    // ===================================================================================
    // = ConnectionSettings
    // ===================================================================================
    class ConnectionSettings
    {
    public:
        enum SETTING_ORIGIN
        {
            SO_SREGISTRY,
            SO_OVERRIDE,
            SO_FALLBACK,
            SO_UNKNOWN
        };

    protected:
        // ine single setting
        struct Setting
        {
            ::com::sun::star::uno::Any      aValue;
            SETTING_ORIGIN                  eOrigin;

            Setting() : eOrigin(SO_UNKNOWN) { }
            Setting(const ::rtl::OUString& _rValue, SETTING_ORIGIN _eOrigin) : aValue(::com::sun::star::uno::makeAny(_rValue)), eOrigin(_eOrigin) { }
            Setting(const sal_Int32 _nValue, SETTING_ORIGIN _eOrigin) : aValue(::com::sun::star::uno::makeAny(_nValue)), eOrigin(_eOrigin) { }
            Setting(const ::com::sun::star::uno::Any& _rValue, SETTING_ORIGIN _eOrigin) : aValue(_rValue), eOrigin(_eOrigin) { }
        };
        DECLARE_STL_USTRINGACCESS_MAP( Setting, SettingsImpl );
        SettingsImpl        m_aImpl;

        ::osl::Profile*     m_pSRegistry;
        sal_Bool            m_bFoundRegistry;

    public:
        /// default ctor
        ConnectionSettings();

        /// dtor
        ~ConnectionSettings();

        /// copy ctor
        ConnectionSettings(const ConnectionSettings& _rSource);

        /** construct a settings object.

The runtime overrides given will be merged with the settings found in a sversion.ini (sversionrc),
            if any. The former overrule the latter

        */
        ConnectionSettings(const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& _rRuntimeOverrides );

        /// merge the given overrides into a new ConnectionSettings object
        ConnectionSettings& mergeOverrides(const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& _rOverrides);

        /// merge the given overrides into the object itself
        ConnectionSettings createMergedSettings(const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& _rOverrides) const
        {
            return ConnectionSettings(*this).mergeOverrides(_rOverrides);
        }

        // check setting existence
        sal_Bool            hasRegistry() const { return m_bFoundRegistry; }
        sal_Bool            hasUser() const;
        sal_Bool            hasPassword() const;
        sal_Bool            hasLocale() const;
        sal_Bool            hasAsyncSetting() const;
        sal_Bool            hasServer() const;
        sal_Bool            hasPort() const;
        sal_Bool            hasTimeout() const;
        sal_Bool            hasService() const;

        sal_Bool            isLocalSession() const;
        sal_Bool            isRemoteSession() const;

        sal_Bool            isValidSourcePath() const;
        sal_Bool            isValidUpdatePath() const;

        // get a special setting
        ::rtl::OUString     getSessionType() const;
        ::rtl::OUString     getUser() const;
        ::rtl::OUString     getPassword() const;
        ::rtl::OUString     getLocale() const;
        ::rtl::OUString     getSourcePath() const;
        ::rtl::OUString     getUpdatePath() const;
        ::rtl::OUString     getServer() const;
        ::rtl::OUString     getService() const;
        sal_Int32           getPort() const;
        sal_Int32           getTimeout() const;
        sal_Bool            getAsyncSetting() const;

        // make sure this behaves as a user session
        void setUserSession();
        void setUserSession(const ::rtl::OUString& _rRemoteServiceName);

        // make sure this behaves as an administrative session
        void setAdminSession();
        void setAdminSession(const ::rtl::OUString& _rRemoteServiceName);

        // set a new session type. Must be one of the *_SESSION_IDENTIFIER defines
        void                setSessionType(const ::rtl::OUString& _rSessionIdentifier);
        // set a desired service, only necessary in remote environments
        sal_Bool            isServiceRequired() const;
        void                setService(const ::rtl::OUString& _rService);

        // set this to a wildcard locale
        void                setAnyLocale();

        IConfigSession* ConnectionSettings::createConnection(
            ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory > const& _rxServiceMgr) const;

    protected:
        void construct(const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& _rOverrides);

        // transfer runtime overwrites into m_aImpl. Existent settings will be overwritten.
        void implTranslate(const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& _rOverrides)
            throw (::com::sun::star::lang::IllegalArgumentException);

        /** normalize a path setting, delete it if the value could not be normalized
            @return <TRUE/> if the setting exists and is a valid path
        */
        sal_Bool implNormalizePathSetting(const sal_Char* _pSetting);

        // translate old settings, which exist for compatiblity only, into new ones
        void implTranslateCompatibilitySettings();

        // if we do not already have the given config path setting, ensure that it exists (calculated relative to a given path)
        void ensureConfigPath(const sal_Char* _pSetting, const ::rtl::OUString& _rBasePath, const sal_Char* _pRelative);

        sal_Bool        haveSetting(const sal_Char* _pName) const;
        void            putSetting(const sal_Char* _pName, const Setting& _rSetting);
        void            clearSetting(const sal_Char* _pName);

        ::rtl::OUString getStringSetting(const sal_Char* _pName) const;
        sal_Int32       getIntSetting(const sal_Char* _pName) const;
        sal_Bool        getBoolSetting(const sal_Char* _pName) const;
        Setting         getSetting(const sal_Char* _pName) const;
        Setting         getMaybeSetting(const sal_Char* _pName) const;

        ::rtl::OUString getProfileStringItem(const sal_Char* _pSection, const sal_Char* _pKey);
        sal_Int32       getProfileIntItem(const sal_Char* _pSection, const sal_Char* _pKey);

    private:
        // ensures that m_aImpl contains a session type
        // to be called from within construct only
        void implDetermineSessionType();

        // collect settings from the sregistry, put them into m_aImpl, if not overruled by runtime settings
        void implCollectSRegistrySetting();

        // clear items which are not relevant because of the session type origin
        void clearIrrelevantItems();
    };
}

#endif // CONFIGMGR_BOOTSTRAP_HXX_

<commit_msg>#81412# Cleaned up bootstrap settings handling; Added error recognition of bootstrap handling; Added new parameter 'reinitialize'<commit_after>/*************************************************************************
 *
 *  $RCSfile: bootstrap.hxx,v $
 *
 *  $Revision: 1.13 $
 *
 *  last change: $Author: jb $ $Date: 2001-05-18 16:13:30 $
 *
 *  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 CONFIGMGR_BOOTSTRAP_HXX_
#define CONFIGMGR_BOOTSTRAP_HXX_

#ifndef _COM_SUN_STAR_UNO_REFERENCE_HXX_
#include <com/sun/star/uno/Reference.hxx>
#endif
#ifndef _COM_SUN_STAR_UNO_SEQUENCE_HXX_
#include <com/sun/star/uno/Sequence.hxx>
#endif
#ifndef _COM_SUN_STAR_UNO_ANY_HXX_
#include <com/sun/star/uno/Any.hxx>
#endif
#ifndef _COM_SUN_STAR_LANG_ILLEGALARGUMENTEXCEPTION_HPP_
#include <com/sun/star/lang/IllegalArgumentException.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XMULTISERVICEFACTORY_HPP_
#include <com/sun/star/lang/XMultiServiceFactory.hpp>
#endif
#ifndef _RTL_USTRING_HXX_
#include <rtl/ustring.hxx>
#endif

#include <map>

namespace osl {
    class Profile;
}

namespace configmgr
{
    class IConfigSession;

    // ===================================================================================

    namespace uno = ::com::sun::star::uno;
    namespace lang = ::com::sun::star::lang;
    using ::rtl::OUString;
    using ::rtl::OString;

    // ===================================================================================
    #define PORTAL_SESSION_IDENTIFIER           "portal"
    #define REMOTE_SESSION_IDENTIFIER           "remote"
    #define LOCAL_SESSION_IDENTIFIER            "local"
    #define SETUP_SESSION_IDENTIFIER            "setup"
    #define PLUGIN_SESSION_IDENTIFIER           "plugin"

    // ===================================================================================

    enum BootstrapResult
    {
        BOOTSTRAP_DATA_OK = 0,
        MISSING_BOOTSTRAP_FILE,
        INVALID_BOOTSTRAP_DATA,
        INVALID_INSTALLATION,
        BOOTSTRAP_FAILURE
    };

    // ===================================================================================

    extern OUString getBootstrapErrorMessage( BootstrapResult rc );
    extern void raiseBootstrapException( BootstrapResult rc, OUString const& sURL, uno::Reference< uno::XInterface > xContext );
    extern void raiseBootstrapException( class BootstrapSettings const& rBootstrapData, uno::Reference< uno::XInterface > xContext );

    // ===================================================================================
    // = Settings
    // ===================================================================================
    class Settings
    {
    public:
        enum Origin
        {
            SO_UNKNOWN,
            SO_FALLBACK,
            SO_INIFILE,
            SO_OVERRIDE,
            SO_DEFAULT,
            SO_ADJUSTMENT,
            SO_MANUAL
        };
        typedef OString Name;

        // a single setting
        struct Setting
        {
            uno::Any        aValue;
            Origin          eOrigin;

            Setting() : aValue(), eOrigin(SO_UNKNOWN) { }
            Setting(const OUString& _rValue, Origin _eOrigin) : aValue(uno::makeAny(_rValue)), eOrigin(_eOrigin) { }
            Setting(const sal_Int32 _nValue, Origin _eOrigin) : aValue(uno::makeAny(_nValue)), eOrigin(_eOrigin) { }
            Setting(const uno::Any& _rValue, Origin _eOrigin) : aValue(_rValue), eOrigin(_eOrigin) { }

            uno::Any    value()  const { return aValue; }
            Origin      origin() const { return eOrigin; };
        };
    protected:
        typedef std::map< Name, Setting > SettingsImpl;
        SettingsImpl        m_aImpl;

    public:
        typedef SettingsImpl::const_iterator Iterator;
    public:
        /// default ctor
        Settings();

        /** construct a settings object
           containing the given overrides

        */
        Settings(const uno::Sequence< uno::Any >& _rOverrides, Origin _eOrigin = SO_OVERRIDE);

        /// merge the given overrides into the object itself
        void mergeOverrides(const Settings& _rOverrides);

        // add settings
        void override(const uno::Sequence< uno::Any >& _rOverrides, Origin _eOrigin = SO_OVERRIDE);

        // check setting existence
        sal_Bool        haveSetting(Name const& _pName) const;
        void            putSetting(Name const&  _pName, const Setting& _rSetting);
        void            clearSetting(Name const& _pName);

        OUString        getStringSetting(Name const& _pName) const;
        sal_Int32       getIntSetting(Name const& _pName) const;
        sal_Bool        getBoolSetting(Name const& _pName) const;
        Setting         getSetting(Name const& _pName) const;
        Setting         getMaybeSetting(Name const& _pName) const;

        Iterator begin()    const { return m_aImpl.begin(); }
        Iterator end()      const { return m_aImpl.end(); }

        void swap(Settings& _rOther) { m_aImpl.swap(_rOther.m_aImpl); }
    private:

        // transfer runtime overwrites into m_aImpl. Existent settings will be overwritten.
        void implTranslate(const uno::Sequence< uno::Any >& _rOverrides, Origin _eOrigin)
            throw (lang::IllegalArgumentException);

    };

    class ConnectionSettings
    {
        Settings m_aSettings;

        ConnectionSettings() : m_aSettings() {};
    public:
        static BootstrapResult findInifile(OUString& _rsInifile);

        static ConnectionSettings bootstrap(BootstrapResult& rc, OUString& _rsInifile);

        ConnectionSettings(const uno::Sequence< uno::Any >& _rOverrides,
                            Settings::Origin _eOrigin = Settings::SO_OVERRIDE);

        /// merge the given overrides into the object itself
        void mergeOverrides(const Settings& _rOverrides);
        /// merge the given overrides into the object itself
        void mergeOverrides(const ConnectionSettings& _rOverrides)
        { mergeOverrides(_rOverrides.m_aSettings); }

        void loadFromInifile(osl::Profile & rProfile,
                             Settings::Origin _eOrigin = Settings::SO_INIFILE);

        void fillFromInifile(osl::Profile & rProfile,
                             Settings::Origin _eOrigin = Settings::SO_INIFILE);

        bool validate();

        bool isComplete() const;
        bool isComplete(OUString const& aSessionType) const;

        sal_Bool    isSessionTypeKnown() const;

        sal_Bool    hasUser() const;
        sal_Bool    hasPassword() const;

        sal_Bool    hasLocale() const;
        sal_Bool    hasAsyncSetting() const;

        sal_Bool    hasServer() const;
        sal_Bool    hasPort() const;
        sal_Bool    hasTimeout() const;
        sal_Bool    hasService() const;

        sal_Bool    isPlugin() const;
        sal_Bool    isLocalSession() const;
        sal_Bool    isRemoteSession() const;

        sal_Bool    isSourcePathValid() const;
        sal_Bool    isUpdatePathValid() const;
        sal_Bool    hasReinitializeFlag() const;

        // get a special setting
        OUString    getSessionType() const;

        OUString    getUser() const;
        OUString    getPassword() const;

        OUString    getLocale() const;
        sal_Bool    getAsyncSetting() const;

        OUString    getSourcePath() const;
        OUString    getUpdatePath() const;
        sal_Bool    getReinitializeFlag() const;

        OUString    getServer() const;
        OUString    getService() const;
        sal_Int32   getPort() const;
        sal_Int32   getTimeout() const;

        // make sure this behaves as a user session
        void setUserSession();
        void setUserSession(const OUString& _rRemoteServiceName);

        // make sure this behaves as an administrative session
        void setAdminSession();
        void setAdminSession(const OUString& _rRemoteServiceName);

        // set a new session type. Must be one of the *_SESSION_IDENTIFIER defines
        void setSessionType(const OUString& _rSessionIdentifier, Settings::Origin _eOrigin /*= SO_MANUAL*/);

        // set a desired service, only necessary in remote environments
        sal_Bool    isServiceRequired() const;
        void        setService(const OUString& _rService, Settings::Origin _eOrigin /*= SO_MANUAL*/);

        // set this to a wildcard locale
        void        setAnyLocale(Settings::Origin _eOrigin /*= SO_MANUAL*/);

        IConfigSession* createConnection(
            uno::Reference< lang::XMultiServiceFactory > const& _rxServiceMgr) const;

        void swap(ConnectionSettings& _rOther) { m_aSettings.swap(_rOther.m_aSettings); }
    protected:

        bool checkSettings() const;

        /** @return <TRUE/> if the setting exists and is a valid path
        */
        sal_Bool isValidPathSetting(Settings::Name const& _pSetting) const;
        sal_Bool implPutPathSetting(Settings::Name const& _pSetting, OUString const& _sSystemPath, Settings::Origin _eOrigin);

        // translate old settings, which exist for compatiblity only, into new ones
        void implTranslateCompatibilitySettings();

        // if we do not already have the given config path setting, ensure that it exists (calculated relative to a given path)
        sal_Bool ensureConfigPath(Settings::Name const& _pSetting, const OUString& _rBasePath, const sal_Char* _pRelative = NULL);

        // if we do not already have path settings, ensure that they exists (in an office install)
        void implAdjustToInstallation(const OUString& _rOfficeInstallPath, const OUString& _rUserInstallPath);

    private:

        // collect settings from the sregistry, put them into m_aSettings
        void implCollectSRegistrySetting(osl::Profile & rProfile, Settings::Origin _eOrigin);

        static OUString     getProfileStringItem(osl::Profile & rProfile, OString const& _pSection, OString const& _pKey, rtl_TextEncoding _nEncoding);
        static sal_Int32    getProfileIntItem(osl::Profile & rProfile, OString const& _pSection, OString const& _pKey);

        // ensures that m_aImpl contains a session type
        void implDetermineSessionType();
        // clear items which are not relevant because of the session type origin
        void implClearIrrelevantItems();

    // convenience wrappers for Settings members
    public:
        sal_Bool    haveSetting(Settings::Name const& _pName) const
        { return m_aSettings.haveSetting(_pName); }

        Settings::Setting   getSetting(Settings::Name const& _pName) const
        { return m_aSettings.getSetting(_pName); }

    // private convenience wrappers for Settings members
    private:
        void        putSetting(Settings::Name const&  _pName, const Settings::Setting& _rSetting)
        { m_aSettings.putSetting(_pName,_rSetting); }

        void        clearSetting(Settings::Name const& _pName)
        { m_aSettings.clearSetting(_pName); }
    };
    /*  sal_Bool            hasRegistry() const { return m_bFoundRegistry; }
        ::osl::Profile*     m_pSRegistry;
        sal_Bool            m_bFoundRegistry;
*/
    class BootstrapSettings
    {
    public:
        OUString            url;
        BootstrapResult     status;
        // order dependency - settings must be last
        ConnectionSettings  settings;

        BootstrapSettings();

        bool hasBootstrapData() const;
    };
}

#endif // CONFIGMGR_BOOTSTRAP_HXX_

<|endoftext|>


<commit_before>#include "bitstream.h"

#include <complex>

#include "wdsp/wdsp.h"

#include "filters.h"

#define FS        250000.0
#define FC_0      57000.0
#define IBUFLEN   4096
#define OBUFLEN   128
#define BITBUFLEN 1024

namespace redsea {

int sign(double a) {
  return (a >= 0 ? 1 : 0);
}

BitStream::BitStream() : tot_errs_(2), reading_frame_(0), counter_(0), subcarr_freq_(FC_0), bit_buffer_(BITBUFLEN), mixer_phi_(0), clock_offset_(0), is_eof_(false), subcarr_lopass_fir_(wdsp::FIR(4000.0 / FS, 127)), subcarr_baseband_(IBUFLEN) {

}

void BitStream::deltaBit(int b) {
  bit_buffer_.append(b ^ dbit_);
  dbit_ = b;
}

void BitStream::biphase(double acc) {

  if (sign(acc) != sign(prev_acc_)) {
    tot_errs_[counter_ % 2] ++;
  }

  if (counter_ % 2 == reading_frame_) {
    deltaBit(sign(acc + prev_acc_));
  }
  if (counter_ == 0) {
    if (tot_errs_[1 - reading_frame_] < tot_errs_[reading_frame_]) {
      reading_frame_ = 1 - reading_frame_;
    }
    tot_errs_[0] = 0;
    tot_errs_[1] = 0;
  }

  prev_acc_ = acc;
  counter_ = (counter_ + 1) % 800;
}

void BitStream::demodulateMoreBits() {

  int16_t sample[IBUFLEN];
  int bytesread = fread(sample, sizeof(int16_t), IBUFLEN, stdin);
  if (bytesread < IBUFLEN) {
    is_eof_ = true;
    return;
  }

  for (int i = 0; i < bytesread; i++) {

    /* Subcarrier downmix & phase recovery */

    mixer_phi_ += 2 * M_PI * subcarr_freq_ * (1.0/FS);
    subcarr_baseband_.appendOverlapFiltered(wdsp::mix(sample[i] / 32768.0,
        mixer_phi_), subcarr_lopass_fir_);

    double pll_beta = 16e-3;

    /* Decimate band-limited signal */
    if (numsamples_ % 8 == 0) {

      std::complex<double> sc_sample = subcarr_baseband_.at(0);
      subcarr_baseband_.forward(8);

      double phi1 = arg(sc_sample);
      if (phi1 >= M_PI_2) {
        phi1 -= M_PI;
      } else if (phi1 <= -M_PI_2) {
        phi1 += M_PI;
      }

      mixer_phi_    -= pll_beta * phi1;
      subcarr_freq_ -= .5 * pll_beta * phi1;

      /* 1187.5 Hz clock */

      double clock_phi = mixer_phi_ / 48.0 + clock_offset_;
      double lo_clock  = (fmod(clock_phi, 2*M_PI) < M_PI ? 1 : -1);

      /* Clock phase recovery */

      if (sign(prev_bb_) != sign(real(sc_sample))) {
        double d_cphi = fmod(clock_phi, M_PI);
        if (d_cphi >= M_PI_2) d_cphi -= M_PI;
        clock_offset_ -= 0.005 * d_cphi;
      }

      /* biphase symbol integrate & dump */
      acc_ += real(sc_sample) * lo_clock;

      if (sign(lo_clock) != sign(prevclock_)) {
        biphase(acc_);
        acc_ = 0;
      }

      prevclock_ = lo_clock;
      prev_bb_ = real(sc_sample);
    }

    numsamples_ ++;

  }

}

int BitStream::getNextBit() {
  while (bit_buffer_.getFillCount() < 1 && !isEOF())
    demodulateMoreBits();

  return bit_buffer_.getNext();
}

bool BitStream::isEOF() const {
  return is_eof_;
}

} // namespace redsea
<commit_msg>dbg print pll freq<commit_after>#include "bitstream.h"

#include <complex>

#include "wdsp/wdsp.h"

#include "filters.h"

#define FS        250000.0
#define FC_0      57000.0
#define IBUFLEN   4096
#define OBUFLEN   128
#define BITBUFLEN 1024

namespace redsea {

int sign(double a) {
  return (a >= 0 ? 1 : 0);
}

BitStream::BitStream() : tot_errs_(2), reading_frame_(0), counter_(0), subcarr_freq_(FC_0), bit_buffer_(BITBUFLEN), mixer_phi_(0), clock_offset_(0), is_eof_(false), subcarr_lopass_fir_(wdsp::FIR(4000.0 / FS, 127)), subcarr_baseband_(IBUFLEN) {

}

void BitStream::deltaBit(int b) {
  bit_buffer_.append(b ^ dbit_);
  dbit_ = b;
}

void BitStream::biphase(double acc) {

  if (sign(acc) != sign(prev_acc_)) {
    tot_errs_[counter_ % 2] ++;
  }

  if (counter_ % 2 == reading_frame_) {
    deltaBit(sign(acc + prev_acc_));
  }
  if (counter_ == 0) {
    if (tot_errs_[1 - reading_frame_] < tot_errs_[reading_frame_]) {
      reading_frame_ = 1 - reading_frame_;
    }
    tot_errs_[0] = 0;
    tot_errs_[1] = 0;
  }

  prev_acc_ = acc;
  counter_ = (counter_ + 1) % 800;
}

void BitStream::demodulateMoreBits() {

  int16_t sample[IBUFLEN];
  int bytesread = fread(sample, sizeof(int16_t), IBUFLEN, stdin);
  if (bytesread < IBUFLEN) {
    is_eof_ = true;
    return;
  }

  for (int i = 0; i < bytesread; i++) {

    /* Subcarrier downmix & phase recovery */

    mixer_phi_ += 2 * M_PI * subcarr_freq_ * (1.0/FS);
    subcarr_baseband_.appendOverlapFiltered(wdsp::mix(sample[i] / 32768.0,
        mixer_phi_), subcarr_lopass_fir_);

    double pll_beta = 16e-3;

    /* Decimate band-limited signal */
    if (numsamples_ % 8 == 0) {

      std::complex<double> sc_sample = subcarr_baseband_.at(0);
      subcarr_baseband_.forward(8);

      double phi1 = arg(sc_sample);
      if (phi1 >= M_PI_2) {
        phi1 -= M_PI;
      } else if (phi1 <= -M_PI_2) {
        phi1 += M_PI;
      }

      mixer_phi_    -= pll_beta * phi1;
      subcarr_freq_ -= .5 * pll_beta * phi1;

      /* 1187.5 Hz clock */

      double clock_phi = mixer_phi_ / 48.0 + clock_offset_;
      double lo_clock  = (fmod(clock_phi, 2*M_PI) < M_PI ? 1 : -1);

      /* Clock phase recovery */

      if (sign(prev_bb_) != sign(real(sc_sample))) {
        double d_cphi = fmod(clock_phi, M_PI);
        if (d_cphi >= M_PI_2) d_cphi -= M_PI;
        clock_offset_ -= 0.005 * d_cphi;
      }

      /* biphase symbol integrate & dump */
      acc_ += real(sc_sample) * lo_clock;

      if (sign(lo_clock) != sign(prevclock_)) {
        biphase(acc_);
        acc_ = 0;
      }

      prevclock_ = lo_clock;
      prev_bb_ = real(sc_sample);
    }

    if (numsamples_ % 1000000 == 0)
      printf(":%.2f Hz\n", subcarr_freq_);

    numsamples_ ++;

  }

}

int BitStream::getNextBit() {
  while (bit_buffer_.getFillCount() < 1 && !isEOF())
    demodulateMoreBits();

  return bit_buffer_.getNext();
}

bool BitStream::isEOF() const {
  return is_eof_;
}

} // namespace redsea
<|endoftext|>


<commit_before>/*
    В программе ввести и инициализировать массив структур, каждая из которых
    описывает материальную точку. Элементы структурного типа: координаты и
    массы частиц, а также расстояния от центра масс до всех точек набора.
    Окончание ввода - ннулевое значение массы точки.

    Общая масса системы точек M = sum_{i} m_i, где под m_i обозначена масса
    отдельных точек.
    Координаты центра масс: x_c = sum_{i} (x_i * m_i) / M,
    y_c = sum_{i} (y_i * m_i) / M, z_c = sum_{i} (z_i * m_i) / M
    (x_i, y_i, z_i) - координаты отдельных точек.

    Расстояние до центра масс определяется как:
        r_i = sqrt( (x_i - x_c)^2 + (y_i - y_c)^2 + (z_i - z_c)^2 )

    Версия для С++, но с использованием realloc / free для выделение памяти под массив простых структур.
*/

#include <iostream>
#include <cstdlib>
#include <clocale> // установка вывода русских букв в командную строку (больше нужна для Windows OS)
#include <cmath>

using namespace std;

struct MassPoint {
    double coord[3];
    double mass;
};

struct PointSet {
    struct MassPoint m_point;
    double distance;
};

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "RUS");

    int i, count = 0;
    double rx, ry, rz;

    PointSet mass_center = { {0., 0., 0., 0.}, 0. };
    PointSet next_point;
    PointSet *p_array = nullptr;

    cout << "Введите точки (порядок ввода: масса, координата x, координата y, координата z).\nПоставьте 0 в качестве массы для прекращения задания точек" << endl;

    while ( true ) {
        cout << "Точка номер " << (count + 1) << endl << "\tмасса: ";
        cin >> next_point.m_point.mass;

        if ( next_point.m_point.mass < 0.0000000001 )
            break;

        cout << "\tкоординаты (3 значения через пробел): ";
        cin >> next_point.m_point.coord[0] >> next_point.m_point.coord[1] >> next_point.m_point.coord[2];

        count++;

        p_array = (struct PointSet *) realloc(p_array, count * sizeof(struct PointSet));
        if (p_array == nullptr) {
            cerr << "Проблема с выделением дополнительно памяти\n";
            return -1;
        }
        p_array[count - 1] = next_point;

        mass_center.m_point.mass += next_point.m_point.mass;
        mass_center.m_point.coord[0] += next_point.m_point.coord[0] * next_point.m_point.mass;
        mass_center.m_point.coord[1] += next_point.m_point.coord[1] * next_point.m_point.mass;
        mass_center.m_point.coord[2] += next_point.m_point.coord[2] * next_point.m_point.mass;
    }

    mass_center.m_point.coord[0] /= mass_center.m_point.mass;
    mass_center.m_point.coord[1] /= mass_center.m_point.mass;
    mass_center.m_point.coord[2] /= mass_center.m_point.mass;

    cout << "Итоги." << endl;
    cout << "Центр масс: " << endl;
    cout << "\tмасса = " << mass_center.m_point.mass << ", координаты = {" << mass_center.m_point.coord[0] <<
            ", " << mass_center.m_point.coord[1] <<
            ", " << mass_center.m_point.coord[2] << "}" << endl;

    cout << "\nВведённые точки:" << endl;
    for (i = 0; i < count; i++) {
        rx = p_array[i].m_point.coord[0] - mass_center.m_point.coord[0];
        ry = p_array[i].m_point.coord[1] - mass_center.m_point.coord[1];
        rz = p_array[i].m_point.coord[2] - mass_center.m_point.coord[2];

        p_array[i].distance = sqrt(rx*rx + ry*ry + rz*rz);

        cout << "Точка номер " << i + 1  << ", координаты: {" << p_array[i].m_point.coord[0] <<
            ", " << p_array[i].m_point.coord[1] <<
            ", " << p_array[i].m_point.coord[2] <<
            "}\n\tмасса = " << p_array[i].m_point.mass << ", расстояние до ц.м. = " << p_array[i].distance << endl;
    }

    free(p_array);

    return 0;
}
<commit_msg>Another style for 9_1.cpp programm<commit_after>/*
    В программе ввести и инициализировать массив структур, каждая из которых
    описывает материальную точку. Элементы структурного типа: координаты и
    массы частиц, а также расстояния от центра масс до всех точек набора.
    Окончание ввода - ннулевое значение массы точки.

    Общая масса системы точек M = sum_{i} m_i, где под m_i обозначена масса
    отдельных точек.
    Координаты центра масс: x_c = sum_{i} (x_i * m_i) / M,
    y_c = sum_{i} (y_i * m_i) / M, z_c = sum_{i} (z_i * m_i) / M
    (x_i, y_i, z_i) - координаты отдельных точек.

    Расстояние до центра масс определяется как:
        r_i = sqrt( (x_i - x_c)^2 + (y_i - y_c)^2 + (z_i - z_c)^2 )

    Версия для С++, но с использованием realloc / free для выделение памяти под массив простых структур.
*/

#include <iostream>
#include <cstdlib>
#include <clocale> // установка вывода русских букв в командную строку (больше нужна для Windows OS)
#include <cmath>

using namespace std;

struct MassPoint {
    double coord[3];
    double mass;
};

struct PointSet {
    struct MassPoint m_point;
    double distance;
};

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "RUS");

    int i, count = 0;
    double rx, ry, rz;

    PointSet mass_center = { {0., 0., 0., 0.}, 0. };
    PointSet next_point;
    PointSet *p_array = nullptr;

    cout << "Введите точки (порядок ввода: масса, координата x, координата y, координата z).\nПоставьте 0 в качестве массы для прекращения задания точек" << endl;

    while ( true ) {
        cout << "Точка номер " << (count + 1) << endl << "\tмасса: ";
        cin >> next_point.m_point.mass;

        if ( next_point.m_point.mass < 0.0000000001 )
            break;

        cout << "\tкоординаты (3 значения через пробел): ";
        cin >> next_point.m_point.coord[0] >> next_point.m_point.coord[1] >> next_point.m_point.coord[2];

        ++count;

        p_array = (PointSet *) realloc(p_array, count * sizeof(PointSet));
        if (p_array == nullptr) {
            cerr << "Проблема с выделением дополнительной памяти\n";
            return -1;
        }
        p_array[count - 1] = next_point;

        mass_center.m_point.mass += next_point.m_point.mass;
        mass_center.m_point.coord[0] += next_point.m_point.coord[0] * next_point.m_point.mass;
        mass_center.m_point.coord[1] += next_point.m_point.coord[1] * next_point.m_point.mass;
        mass_center.m_point.coord[2] += next_point.m_point.coord[2] * next_point.m_point.mass;
    }

    mass_center.m_point.coord[0] /= mass_center.m_point.mass;
    mass_center.m_point.coord[1] /= mass_center.m_point.mass;
    mass_center.m_point.coord[2] /= mass_center.m_point.mass;

    cout << "Итоги." << endl;
    cout << "Центр масс: " << endl;
    cout << "\tмасса = " << mass_center.m_point.mass << ", координаты = {" << mass_center.m_point.coord[0] <<
            ", " << mass_center.m_point.coord[1] <<
            ", " << mass_center.m_point.coord[2] << "}" << endl;

    cout << "\nВведённые точки:" << endl;
    for (i = 0; i < count; i++) {
        rx = p_array[i].m_point.coord[0] - mass_center.m_point.coord[0];
        ry = p_array[i].m_point.coord[1] - mass_center.m_point.coord[1];
        rz = p_array[i].m_point.coord[2] - mass_center.m_point.coord[2];

        p_array[i].distance = sqrt(rx*rx + ry*ry + rz*rz);

        cout << "Точка номер " << i + 1  << ", координаты: {" << p_array[i].m_point.coord[0] <<
            ", " << p_array[i].m_point.coord[1] <<
            ", " << p_array[i].m_point.coord[2] <<
            "}\n\tмасса = " << p_array[i].m_point.mass << ", расстояние до ц.м. = " << p_array[i].distance << endl;
    }

    free(p_array);

    return 0;
}
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2009-2012, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  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 copyright 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 THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS 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.
 *
 * $Id: test_filters.cpp 7683 2012-10-23 02:49:03Z rusu $
 *
 */

#include <gtest/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/covariance_sampling.h>
#include <pcl/filters/normal_space.h>

#include <pcl/common/transforms.h>
#include <pcl/common/eigen.h>

using namespace pcl;

PointCloud<PointXYZ>::Ptr cloud_walls (new PointCloud<PointXYZ> ()),
                          cloud_turtle (new PointCloud<PointXYZ> ());
PointCloud<PointNormal>::Ptr cloud_walls_normals (new PointCloud<PointNormal> ()),
                             cloud_turtle_normals (new PointCloud<PointNormal> ());

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (CovarianceSampling, Filters)
{
  CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
  covariance_sampling.setInputCloud (cloud_walls_normals);
  covariance_sampling.setNormals (cloud_walls_normals);
  covariance_sampling.setNumberOfSamples (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);
  double cond_num_walls = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_walls, 19.3518, 0.5);

  IndicesPtr walls_indices (new std::vector<int> ());
  covariance_sampling.filter (*walls_indices);

  covariance_sampling.setIndices (walls_indices);
  double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_walls_sampled, 4.0298, 0.5);

  EXPECT_EQ ((*walls_indices)[0], 316);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 4], 3208);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 2], 2546);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () * 3 / 4], 1886);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () - 1], 321);

  covariance_sampling.setInputCloud (cloud_turtle_normals);
  covariance_sampling.setNormals (cloud_turtle_normals);
  covariance_sampling.setIndices (IndicesPtr ());
  covariance_sampling.setNumberOfSamples (static_cast<unsigned int> (cloud_turtle_normals->size ()) / 8);
  double cond_num_turtle = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_turtle, 20661.7663, 0.5);

  IndicesPtr turtle_indices (new std::vector<int> ());
  covariance_sampling.filter (*turtle_indices);
  covariance_sampling.setIndices (turtle_indices);
  double cond_num_turtle_sampled = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_turtle_sampled, 5795.5057, 0.5);

  EXPECT_EQ ((*turtle_indices)[0], 80344);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () / 4], 145982);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () / 2], 104557);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () * 3 / 4], 41512);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () - 1], 136885);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (NormalSpaceSampling, Filters)
{
  NormalSpaceSampling<PointNormal, PointNormal> normal_space_sampling;
  normal_space_sampling.setInputCloud (cloud_walls_normals);
  normal_space_sampling.setNormals (cloud_walls_normals);
  normal_space_sampling.setBins (16, 16, 16);
  normal_space_sampling.setSeed (0);
  normal_space_sampling.setSample (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);

  IndicesPtr walls_indices (new std::vector<int> ());
  normal_space_sampling.filter (*walls_indices);

  CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
  covariance_sampling.setInputCloud (cloud_walls_normals);
  covariance_sampling.setNormals (cloud_walls_normals);
  covariance_sampling.setIndices (walls_indices);
  covariance_sampling.setNumberOfSamples (0);
  double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();

  EXPECT_NEAR (cond_num_walls_sampled, 7.8289, 0.5);

  EXPECT_EQ ((*walls_indices)[0], 2472);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 4], 2854);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 2], 1566);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () * 3 / 4], 2981);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () - 1], 2577);
}

/* ---[ */
int
main (int argc, char** argv)
{
  // Load two standard PCD files from disk
  if (argc < 3)
  {
    std::cerr << "No test files given. Please download `sac_plane_test.pcd` and 'cturtle.pcd' and pass them path to the test." << std::endl;
    return (-1);
  }

  // Load in the point clouds
  io::loadPCDFile (argv[1], *cloud_walls);
  io::loadPCDFile (argv[2], *cloud_turtle);

  // Compute the normals for each cloud, and then clean them up of any NaN values
  NormalEstimation<PointXYZ,PointNormal> ne;
  ne.setInputCloud (cloud_walls);
  ne.setRadiusSearch (0.02);
  ne.compute (*cloud_walls_normals);
  copyPointCloud (*cloud_walls, *cloud_walls_normals);

  std::vector<int> aux_indices;
  removeNaNFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
  removeNaNNormalsFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);

  ne = NormalEstimation<PointXYZ, PointNormal> ();
  ne.setInputCloud (cloud_turtle);
  ne.setKSearch (5);
  ne.compute (*cloud_turtle_normals);
  copyPointCloud (*cloud_turtle, *cloud_turtle_normals);
  removeNaNFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
  removeNaNNormalsFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
/* ]--- */
<commit_msg>changed unit test values<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2009-2012, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  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 copyright 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 THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS 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.
 *
 * $Id: test_filters.cpp 7683 2012-10-23 02:49:03Z rusu $
 *
 */

#include <gtest/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/covariance_sampling.h>
#include <pcl/filters/normal_space.h>

#include <pcl/common/transforms.h>
#include <pcl/common/eigen.h>

using namespace pcl;

PointCloud<PointXYZ>::Ptr cloud_walls (new PointCloud<PointXYZ> ()),
                          cloud_turtle (new PointCloud<PointXYZ> ());
PointCloud<PointNormal>::Ptr cloud_walls_normals (new PointCloud<PointNormal> ()),
                             cloud_turtle_normals (new PointCloud<PointNormal> ());

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (CovarianceSampling, Filters)
{
  CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
  covariance_sampling.setInputCloud (cloud_walls_normals);
  covariance_sampling.setNormals (cloud_walls_normals);
  covariance_sampling.setNumberOfSamples (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);
  double cond_num_walls = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_walls, 20.8774, 0.5);

  IndicesPtr walls_indices (new std::vector<int> ());
  covariance_sampling.filter (*walls_indices);

  covariance_sampling.setIndices (walls_indices);
  double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_walls_sampled, 4.0298, 0.5);

  EXPECT_EQ ((*walls_indices)[0], 315);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 4], 275);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 2], 1846);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () * 3 / 4], 1241);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () - 1], 2632);

  covariance_sampling.setInputCloud (cloud_turtle_normals);
  covariance_sampling.setNormals (cloud_turtle_normals);
  covariance_sampling.setIndices (IndicesPtr ());
  covariance_sampling.setNumberOfSamples (static_cast<unsigned int> (cloud_turtle_normals->size ()) / 8);
  double cond_num_turtle = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_turtle, 20661.7663, 0.5);

  IndicesPtr turtle_indices (new std::vector<int> ());
  covariance_sampling.filter (*turtle_indices);
  covariance_sampling.setIndices (turtle_indices);
  double cond_num_turtle_sampled = covariance_sampling.computeConditionNumber ();
  EXPECT_NEAR (cond_num_turtle_sampled, 5795.5057, 0.5);

  EXPECT_EQ ((*turtle_indices)[0], 80344);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () / 4], 145982);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () / 2], 104557);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () * 3 / 4], 41512);
  EXPECT_EQ ((*turtle_indices)[turtle_indices->size () - 1], 136885);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (NormalSpaceSampling, Filters)
{
  NormalSpaceSampling<PointNormal, PointNormal> normal_space_sampling;
  normal_space_sampling.setInputCloud (cloud_walls_normals);
  normal_space_sampling.setNormals (cloud_walls_normals);
  normal_space_sampling.setBins (16, 16, 16);
  normal_space_sampling.setSeed (0);
  normal_space_sampling.setSample (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);

  IndicesPtr walls_indices (new std::vector<int> ());
  normal_space_sampling.filter (*walls_indices);

  CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
  covariance_sampling.setInputCloud (cloud_walls_normals);
  covariance_sampling.setNormals (cloud_walls_normals);
  covariance_sampling.setIndices (walls_indices);
  covariance_sampling.setNumberOfSamples (0);
  double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();

  EXPECT_NEAR (cond_num_walls_sampled, 9.0989, 0.5);

  EXPECT_EQ ((*walls_indices)[0], 2432);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 4], 1947);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () / 2], 3148);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () * 3 / 4], 2443);
  EXPECT_EQ ((*walls_indices)[walls_indices->size () - 1], 2396);
}

/* ---[ */
int
main (int argc, char** argv)
{
  // Load two standard PCD files from disk
  if (argc < 3)
  {
    std::cerr << "No test files given. Please download `sac_plane_test.pcd` and 'cturtle.pcd' and pass them path to the test." << std::endl;
    return (-1);
  }

  // Load in the point clouds
  io::loadPCDFile (argv[1], *cloud_walls);
  io::loadPCDFile (argv[2], *cloud_turtle);

  // Compute the normals for each cloud, and then clean them up of any NaN values
  NormalEstimation<PointXYZ,PointNormal> ne;
  ne.setInputCloud (cloud_walls);
  ne.setRadiusSearch (0.02);
  ne.compute (*cloud_walls_normals);
  copyPointCloud (*cloud_walls, *cloud_walls_normals);

  std::vector<int> aux_indices;
  removeNaNFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
  removeNaNNormalsFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);

  ne = NormalEstimation<PointXYZ, PointNormal> ();
  ne.setInputCloud (cloud_turtle);
  ne.setKSearch (5);
  ne.compute (*cloud_turtle_normals);
  copyPointCloud (*cloud_turtle, *cloud_turtle_normals);
  removeNaNFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
  removeNaNNormalsFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
/* ]--- */
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "modules/perception/fusion/lib/data_fusion/type_fusion/dst_type_fusion/dst_type_fusion.h"

#include <algorithm>
#include <functional>
#include <map>
#include <string>
#include <vector>

#include "boost/format.hpp"
#include "gtest/gtest.h"
#include "modules/perception/fusion/base/sensor_data_manager.h"
#include "modules/perception/fusion/common/camera_util.h"

namespace apollo {
namespace perception {
namespace fusion {

TEST(DstTypeFusionTest, test_update_with_measurement) {
  FLAGS_work_root = "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion";
  FLAGS_obs_sensor_intrinsic_path = "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion/params";
  FLAGS_obs_sensor_meta_path = "./data/sensor_meta.pt";
  common::SensorManager *sensor_manager =
      lib::Singleton<common::SensorManager>::get_instance();
  EXPECT_TRUE(sensor_manager != nullptr);

  EXPECT_TRUE(sensor_manager->Init());

  std::cout << "start init dst app\n";
  bool flag = DstTypeFusion::Init();
  EXPECT_TRUE(flag);
  // create a track
  base::ObjectPtr base_lidar_object_ptr(new base::Object());
  base_lidar_object_ptr->type = base::ObjectType::VEHICLE;
  base_lidar_object_ptr->type_probs.resize(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.0);
  base_lidar_object_ptr->type_probs = std::vector<float>(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.1);
  base_lidar_object_ptr
      ->type_probs[static_cast<size_t>(base::ObjectType::VEHICLE)] = 0.6;

  base::FramePtr lidar_frame_ptr(new base::Frame());
  lidar_frame_ptr->sensor_info.name = "velodyne_64";
  lidar_frame_ptr->sensor_info.type = base::SensorType::VELODYNE_64;

  SensorPtr lidar_sensor_ptr(new Sensor(lidar_frame_ptr->sensor_info));
  lidar_sensor_ptr->AddFrame(lidar_frame_ptr);

  SensorFramePtr lidar_sensor_frame_ptr(new SensorFrame());
  lidar_sensor_frame_ptr->Initialize(lidar_frame_ptr,
      lidar_sensor_ptr->GetSensorId(), lidar_sensor_ptr->GetSensorType());
  SensorObjectPtr lidar_sensor_object_ptr(
      new SensorObject(base_lidar_object_ptr, lidar_sensor_frame_ptr));
  TrackPtr track(new Track());
  EXPECT_TRUE(DstTypeFusion::Init());

  EXPECT_TRUE(track->Initialize(lidar_sensor_object_ptr));

  DstTypeFusion type_fusion(track);
  CHECK_EQ(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  // create a camera measurement
  base::ObjectPtr base_camera_object_ptr(new base::Object());
  base_camera_object_ptr->type = base::ObjectType::PEDESTRIAN;
  base_camera_object_ptr->type_probs.resize(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE));
  base_camera_object_ptr->type_probs = std::vector<float>(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.1);
  base_camera_object_ptr
      ->type_probs[static_cast<size_t>(base::ObjectType::PEDESTRIAN)] = 0.4;

  base::FramePtr camera_frame_ptr(new base::Frame());
  camera_frame_ptr->sensor_info.type = base::SensorType::MONOCULAR_CAMERA;

  SensorPtr camera_sensor_ptr(new Sensor(camera_frame_ptr->sensor_info));
  camera_sensor_ptr->AddFrame(camera_frame_ptr);

  SensorFramePtr camera_sensor_frame_ptr(new SensorFrame());
  camera_sensor_frame_ptr->Initialize(camera_frame_ptr,
      camera_sensor_ptr->GetSensorId(), camera_sensor_ptr->GetSensorType());
  SensorObjectPtr camera_sensor_object_ptr(
      new SensorObject(base_camera_object_ptr, camera_sensor_frame_ptr));

  // camera not supported
  camera_frame_ptr->sensor_info.name = "camera_test";
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr, 0.0);
  CHECK_EQ(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  camera_frame_ptr->sensor_info.name = "camera_smartereye";
  SensorFramePtr camera_sensor_frame_normal(new SensorFrame());
  SensorPtr camera_sensor_ptr1(new Sensor(camera_frame_ptr->sensor_info));
  camera_sensor_ptr1->AddFrame(camera_frame_ptr);
  camera_sensor_frame_normal->Initialize(camera_frame_ptr,
      camera_sensor_ptr->GetSensorId(), camera_sensor_ptr->GetSensorType());
  SensorObjectPtr camera_sensor_object_ptr1(
      new SensorObject(base_camera_object_ptr, camera_sensor_frame_normal));
  // update with measurment
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr1, 0.0);
  // std::cout<< "fused dst: " << type_fusion.PrintFusedDst();
  CHECK_EQ(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::PEDESTRIAN));
  // update more times
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr1, 0.0);
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr1, 0.0);
  CHECK_EQ(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::PEDESTRIAN));
}

TEST(DstTypeFusionTest, test_update_without_measurement) {
  FLAGS_work_root = "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion";
  FLAGS_obs_sensor_intrinsic_path = "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion/params";
  FLAGS_obs_sensor_meta_path = "./conf/sensor_meta.config";
  common::SensorManager *sensor_manager =
      lib::Singleton<common::SensorManager>::get_instance();
  EXPECT_TRUE(sensor_manager != nullptr);

  EXPECT_TRUE(sensor_manager->Init());

  std::cout << "start init dst app\n";
  bool flag = DstTypeFusion::Init();
  EXPECT_TRUE(flag);
  base::SensorInfo radar_front_info;
  EXPECT_TRUE(sensor_manager->GetSensorInfo("radar", &radar_front_info));
  SensorPtr radar_front_sensor(new Sensor(radar_front_info));
  // radar front frame
  base::ObjectPtr base_obj(new base::Object());
  base::FramePtr base_frame(new base::Frame());
  base_obj->center = Eigen::Vector3d(2.3, 2.3, 0.0);
  base_obj->anchor_point = base_obj->center;
  base_obj->velocity = Eigen::Vector3f(5, 0, 0);
  base_obj->direction = Eigen::Vector3f(1, 0, 0);
  base_obj->radar_supplement.range = 10;
  base_obj->radar_supplement.angle = 10;
  base_frame->objects.emplace_back(base_obj);
  base_frame->sensor_info = radar_front_info;
  SensorFramePtr radar_frame(new SensorFrame());
  radar_frame->Initialize(base_frame,
      radar_front_sensor->GetSensorId(), radar_front_sensor->GetSensorType());

  // create a lidar track
  base::ObjectPtr base_lidar_object_ptr(new base::Object());
  base_lidar_object_ptr->type = base::ObjectType::VEHICLE;
  base_lidar_object_ptr->type_probs.resize(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.0);
  base_lidar_object_ptr->type_probs = std::vector<float>(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.1);
  base_lidar_object_ptr
      ->type_probs[static_cast<size_t>(base::ObjectType::VEHICLE)] = 0.6;
  base_lidar_object_ptr->center = Eigen::Vector3d(0, 0, 10);

  base::FramePtr lidar_frame_ptr(new base::Frame());
  lidar_frame_ptr->sensor_info.name = "velodyne_64";
  lidar_frame_ptr->sensor_info.type = base::SensorType::VELODYNE_64;

  SensorPtr lidar_sensor_ptr(new Sensor(lidar_frame_ptr->sensor_info));
  lidar_sensor_ptr->AddFrame(lidar_frame_ptr);

  SensorFramePtr lidar_sensor_frame_ptr(new SensorFrame());
  lidar_sensor_frame_ptr->Initialize(lidar_frame_ptr,
      lidar_sensor_ptr->GetSensorId(), lidar_sensor_ptr->GetSensorType());
  SensorObjectPtr lidar_sensor_object_ptr(
      new SensorObject(base_lidar_object_ptr, lidar_sensor_frame_ptr));
  TrackPtr track1(new Track());
  EXPECT_TRUE(track1->Initialize(lidar_sensor_object_ptr));

  EXPECT_TRUE(DstTypeFusion::Init());
  DstTypeFusion type_fusion1(track1);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  base::FramePtr camera_frame_ptr(new base::Frame());
  camera_frame_ptr->sensor_info.name = "camera_smartereye";
  camera_frame_ptr->sensor_info.type = base::SensorType::MONOCULAR_CAMERA;
  camera_frame_ptr->timestamp = 151192277.124567989;
  Eigen::Affine3d pose(Eigen::Affine3d::Identity());
  camera_frame_ptr->sensor2world_pose = pose;

  SensorPtr camera_sensor_ptr(new Sensor(camera_frame_ptr->sensor_info));
  camera_sensor_ptr->AddFrame(camera_frame_ptr);
  SensorDataManager *sensor_date_manager =
      lib::Singleton<SensorDataManager>::get_instance();
  sensor_date_manager->AddSensorMeasurements(camera_frame_ptr);

  // update without measurment
  std::string sensor_id = "camera_smartereye";
  // std::cout<< "fused dst: " << type_fusion1.PrintFusedDst();
  type_fusion1.UpdateWithoutMeasurement("velodyne_64", 151192277.124567989,
                                        151192277.124567989, 0.9);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                        151192277.124567989, 0.9);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));
  type_fusion1.UpdateWithoutMeasurement("camera_test", 151192277.124567989,
                                        151192277.124567989, 0.9);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  // update more times
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                        151192277.124567989, 0.0);
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                        151192277.124567989, 0.0);
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 0, 0, 0.0);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  TrackPtr track2(new Track());
  track2->Initialize(radar_frame->GetForegroundObjects()[0]);
  DstTypeFusion type_fusion_2(track2);
  type_fusion_2.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                         151192277.124567989, 0.0);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  base::ObjectPtr base_camera_object_ptr(new base::Object());

  SensorFramePtr camera_sensor_frame_ptr(new SensorFrame());
  camera_sensor_frame_ptr->Initialize(camera_frame_ptr,
      camera_sensor_ptr->GetSensorId(), camera_sensor_ptr->GetSensorType());
  SensorObjectPtr camera_sensor_object_ptr(
      new SensorObject(base_camera_object_ptr, camera_sensor_frame_ptr));
  TrackPtr track3(new Track());
  track3->Initialize(camera_sensor_object_ptr);
  DstTypeFusion type_fusion_3(track3);
  type_fusion_3.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                         151192277.124567989, 0.0);
  CHECK_EQ(static_cast<size_t>(track3->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));
}

}  // namespace fusion
}  // namespace perception
}  // namespace apollo
<commit_msg>Fixed test failure after 54314083e16c41082a81520ec44c7aaae44989c7.<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "modules/perception/fusion/lib/data_fusion/type_fusion/dst_type_fusion/dst_type_fusion.h"

#include <algorithm>
#include <functional>
#include <map>
#include <string>
#include <vector>

#include "boost/format.hpp"
#include "gtest/gtest.h"
#include "modules/perception/fusion/base/sensor_data_manager.h"
#include "modules/perception/fusion/common/camera_util.h"

namespace apollo {
namespace perception {
namespace fusion {

TEST(DstTypeFusionTest, test_update_with_measurement) {
  FLAGS_work_root =
      "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion";
  FLAGS_obs_sensor_intrinsic_path =
      "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion/params";
  FLAGS_obs_sensor_meta_path = "./data/sensor_meta.pt";
  common::SensorManager *sensor_manager =
      lib::Singleton<common::SensorManager>::get_instance();
  EXPECT_TRUE(sensor_manager != nullptr);

  EXPECT_TRUE(sensor_manager->Init());

  std::cout << "start init dst app\n";
  bool flag = DstTypeFusion::Init();
  EXPECT_TRUE(flag);
  // create a track
  base::ObjectPtr base_lidar_object_ptr(new base::Object());
  base_lidar_object_ptr->type = base::ObjectType::VEHICLE;
  base_lidar_object_ptr->type_probs.resize(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.0);
  base_lidar_object_ptr->type_probs = std::vector<float>(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.1);
  base_lidar_object_ptr
      ->type_probs[static_cast<size_t>(base::ObjectType::VEHICLE)] = 0.6;

  base::FramePtr lidar_frame_ptr(new base::Frame());
  lidar_frame_ptr->sensor_info.name = "velodyne_64";
  lidar_frame_ptr->sensor_info.type = base::SensorType::VELODYNE_64;

  SensorPtr lidar_sensor_ptr(new Sensor(lidar_frame_ptr->sensor_info));
  lidar_sensor_ptr->AddFrame(lidar_frame_ptr);

  SensorFramePtr lidar_sensor_frame_ptr(new SensorFrame());
  lidar_sensor_frame_ptr->Initialize(lidar_frame_ptr,
                                     lidar_sensor_ptr->GetSensorId(),
                                     lidar_sensor_ptr->GetSensorType());
  SensorObjectPtr lidar_sensor_object_ptr(
      new SensorObject(base_lidar_object_ptr, lidar_sensor_frame_ptr));
  TrackPtr track(new Track());
  EXPECT_TRUE(DstTypeFusion::Init());

  EXPECT_TRUE(track->Initialize(lidar_sensor_object_ptr));

  DstTypeFusion type_fusion(track);
  CHECK_EQ(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  // create a camera measurement
  base::ObjectPtr base_camera_object_ptr(new base::Object());
  base_camera_object_ptr->type = base::ObjectType::PEDESTRIAN;
  base_camera_object_ptr->type_probs.resize(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE));
  base_camera_object_ptr->type_probs = std::vector<float>(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.1);
  base_camera_object_ptr
      ->type_probs[static_cast<size_t>(base::ObjectType::PEDESTRIAN)] = 0.4;

  base::FramePtr camera_frame_ptr(new base::Frame());
  camera_frame_ptr->sensor_info.type = base::SensorType::MONOCULAR_CAMERA;

  SensorPtr camera_sensor_ptr(new Sensor(camera_frame_ptr->sensor_info));
  camera_sensor_ptr->AddFrame(camera_frame_ptr);

  SensorFramePtr camera_sensor_frame_ptr(new SensorFrame());
  camera_sensor_frame_ptr->Initialize(camera_frame_ptr,
                                      camera_sensor_ptr->GetSensorId(),
                                      camera_sensor_ptr->GetSensorType());
  SensorObjectPtr camera_sensor_object_ptr(
      new SensorObject(base_camera_object_ptr, camera_sensor_frame_ptr));

  // camera not supported
  camera_frame_ptr->sensor_info.name = "camera_test";
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr, 0.0);
  CHECK_EQ(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  camera_frame_ptr->sensor_info.name = "camera_smartereye";
  SensorFramePtr camera_sensor_frame_normal(new SensorFrame());
  SensorPtr camera_sensor_ptr1(new Sensor(camera_frame_ptr->sensor_info));
  camera_sensor_ptr1->AddFrame(camera_frame_ptr);
  camera_sensor_frame_normal->Initialize(camera_frame_ptr,
                                         camera_sensor_ptr->GetSensorId(),
                                         camera_sensor_ptr->GetSensorType());
  SensorObjectPtr camera_sensor_object_ptr1(
      new SensorObject(base_camera_object_ptr, camera_sensor_frame_normal));
  // update with measurment
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr1, 0.0);
  // std::cout<< "fused dst: " << type_fusion.PrintFusedDst();
  CHECK_NE(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::PEDESTRIAN));
  // update more times
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr1, 0.0);
  type_fusion.UpdateWithMeasurement(camera_sensor_object_ptr1, 0.0);
  CHECK_NE(static_cast<size_t>(track->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::PEDESTRIAN));
}

TEST(DstTypeFusionTest, test_update_without_measurement) {
  FLAGS_work_root =
      "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion";
  FLAGS_obs_sensor_intrinsic_path =
      "/apollo/modules/perception/testdata/"
      "fusion/dst_type_fusion/params";
  FLAGS_obs_sensor_meta_path = "./conf/sensor_meta.config";
  common::SensorManager *sensor_manager =
      lib::Singleton<common::SensorManager>::get_instance();
  EXPECT_TRUE(sensor_manager != nullptr);

  EXPECT_TRUE(sensor_manager->Init());

  std::cout << "start init dst app\n";
  bool flag = DstTypeFusion::Init();
  EXPECT_TRUE(flag);
  base::SensorInfo radar_front_info;
  EXPECT_TRUE(sensor_manager->GetSensorInfo("radar", &radar_front_info));
  SensorPtr radar_front_sensor(new Sensor(radar_front_info));
  // radar front frame
  base::ObjectPtr base_obj(new base::Object());
  base::FramePtr base_frame(new base::Frame());
  base_obj->center = Eigen::Vector3d(2.3, 2.3, 0.0);
  base_obj->anchor_point = base_obj->center;
  base_obj->velocity = Eigen::Vector3f(5, 0, 0);
  base_obj->direction = Eigen::Vector3f(1, 0, 0);
  base_obj->radar_supplement.range = 10;
  base_obj->radar_supplement.angle = 10;
  base_frame->objects.emplace_back(base_obj);
  base_frame->sensor_info = radar_front_info;
  SensorFramePtr radar_frame(new SensorFrame());
  radar_frame->Initialize(base_frame, radar_front_sensor->GetSensorId(),
                          radar_front_sensor->GetSensorType());

  // create a lidar track
  base::ObjectPtr base_lidar_object_ptr(new base::Object());
  base_lidar_object_ptr->type = base::ObjectType::VEHICLE;
  base_lidar_object_ptr->type_probs.resize(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.0);
  base_lidar_object_ptr->type_probs = std::vector<float>(
      static_cast<size_t>(base::ObjectType::MAX_OBJECT_TYPE), 0.1);
  base_lidar_object_ptr
      ->type_probs[static_cast<size_t>(base::ObjectType::VEHICLE)] = 0.6;
  base_lidar_object_ptr->center = Eigen::Vector3d(0, 0, 10);

  base::FramePtr lidar_frame_ptr(new base::Frame());
  lidar_frame_ptr->sensor_info.name = "velodyne_64";
  lidar_frame_ptr->sensor_info.type = base::SensorType::VELODYNE_64;

  SensorPtr lidar_sensor_ptr(new Sensor(lidar_frame_ptr->sensor_info));
  lidar_sensor_ptr->AddFrame(lidar_frame_ptr);

  SensorFramePtr lidar_sensor_frame_ptr(new SensorFrame());
  lidar_sensor_frame_ptr->Initialize(lidar_frame_ptr,
                                     lidar_sensor_ptr->GetSensorId(),
                                     lidar_sensor_ptr->GetSensorType());
  SensorObjectPtr lidar_sensor_object_ptr(
      new SensorObject(base_lidar_object_ptr, lidar_sensor_frame_ptr));
  TrackPtr track1(new Track());
  EXPECT_TRUE(track1->Initialize(lidar_sensor_object_ptr));

  EXPECT_TRUE(DstTypeFusion::Init());
  DstTypeFusion type_fusion1(track1);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  base::FramePtr camera_frame_ptr(new base::Frame());
  camera_frame_ptr->sensor_info.name = "camera_smartereye";
  camera_frame_ptr->sensor_info.type = base::SensorType::MONOCULAR_CAMERA;
  camera_frame_ptr->timestamp = 151192277.124567989;
  Eigen::Affine3d pose(Eigen::Affine3d::Identity());
  camera_frame_ptr->sensor2world_pose = pose;

  SensorPtr camera_sensor_ptr(new Sensor(camera_frame_ptr->sensor_info));
  camera_sensor_ptr->AddFrame(camera_frame_ptr);
  SensorDataManager *sensor_date_manager =
      lib::Singleton<SensorDataManager>::get_instance();
  sensor_date_manager->AddSensorMeasurements(camera_frame_ptr);

  // update without measurment
  std::string sensor_id = "camera_smartereye";
  // std::cout<< "fused dst: " << type_fusion1.PrintFusedDst();
  type_fusion1.UpdateWithoutMeasurement("velodyne_64", 151192277.124567989,
                                        151192277.124567989, 0.9);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                        151192277.124567989, 0.9);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));
  type_fusion1.UpdateWithoutMeasurement("camera_test", 151192277.124567989,
                                        151192277.124567989, 0.9);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  // update more times
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                        151192277.124567989, 0.0);
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                        151192277.124567989, 0.0);
  type_fusion1.UpdateWithoutMeasurement(sensor_id, 0, 0, 0.0);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  TrackPtr track2(new Track());
  track2->Initialize(radar_frame->GetForegroundObjects()[0]);
  DstTypeFusion type_fusion_2(track2);
  type_fusion_2.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                         151192277.124567989, 0.0);
  CHECK_EQ(static_cast<size_t>(track1->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));

  base::ObjectPtr base_camera_object_ptr(new base::Object());

  SensorFramePtr camera_sensor_frame_ptr(new SensorFrame());
  camera_sensor_frame_ptr->Initialize(camera_frame_ptr,
                                      camera_sensor_ptr->GetSensorId(),
                                      camera_sensor_ptr->GetSensorType());
  SensorObjectPtr camera_sensor_object_ptr(
      new SensorObject(base_camera_object_ptr, camera_sensor_frame_ptr));
  TrackPtr track3(new Track());
  track3->Initialize(camera_sensor_object_ptr);
  DstTypeFusion type_fusion_3(track3);
  type_fusion_3.UpdateWithoutMeasurement(sensor_id, 151192277.124567989,
                                         151192277.124567989, 0.0);
  CHECK_EQ(static_cast<size_t>(track3->GetFusedObject()->GetBaseObject()->type),
           static_cast<size_t>(base::ObjectType::UNKNOWN));
}

}  // namespace fusion
}  // namespace perception
}  // namespace apollo
<|endoftext|>


<commit_before>
// Qt includes
#include <QTextStream>
#include <QCoreApplication>
#include <QProcess>

// STD includes
#include <iostream>
#include <cstdlib>

  /*
/pieper/ctk/latest/CTK-superbuild/CMakeExternals/Install/bin/dcmqrscp -c /pieper/ctk/latest/CTK-superbuild/CTK-build/Testing/Temporary/dcmqrscp.cfg -d -v

storescu -aec COMMONTK -aet CTK_AE localhost 11112 /data/pieper-face-2005-05-11/1.2.840.113619.2.135.3596.6358736.5118.1115807980.182.UID/000001.SER/000001.IMA

findscu -aet CTK_AE -aec COMMONTK -P -k 0010,0010=\* localhost 11112 patqry.dcm

./CTK-build/bin/ctkDICOMQuery test.db CTK_AE COMMONTK localhost 11112

./CTK-build/bin/ctkDICOMRetrieve 1.2.840.113619.2.135.3596.6358736.5118.1115807980.182  /tmp/hoot CTK_AE 11113 CTK_AE localhost 11112 CTK_CLIENT_AE
*/

int ctkDICOMApplicationTest1(int argc, char * argv []) {

  QCoreApplication app(argc, argv);
  QTextStream out(stdout);

  if ( argc < 10 )
  {
    out << "ERROR: invalid arguments.  Should be:\n";
    out << " ctkDICOMApplicationTest1 <dcmqrscp> <configfile> <dicomData1> <dcmData2> <storescu> <ctkDICOMQuery> <ctkDICOMRetrieve> <retrieveDirectory>\n";
    return EXIT_FAILURE;
  }

  QString dcmqrscp_exe  (argv[1]);
  QString dcmqrscp_cfg  (argv[2]);
  QString dicomData1  (argv[3]);
  QString dicomData2  (argv[4]);
  QString storescu_exe  (argv[5]);
  QString ctkDICOMQuery_exe  (argv[6]);
  QString ctkDICOMQuery_db_file  (argv[7]);
  QString ctkDICOMRetrieve_exe  (argv[8]);
  QString ctkDICOMRetrieve_directory  (argv[9]);

  //
  // first, start the server process
  //

  QProcess *dcmqrscp = new QProcess(0);
  QStringList dcmqrscp_args;
  dcmqrscp_args << "--config" << dcmqrscp_cfg;
  dcmqrscp_args << "--debug" << "--verbose";
  dcmqrscp_args << "11112";

  try
  {
    out << "starting server" << dcmqrscp_exe << "\n";
    out << "with args " << dcmqrscp_args.join(" ") << "\n";
    dcmqrscp->start(dcmqrscp_exe, dcmqrscp_args);
    dcmqrscp->waitForStarted();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start server" << e.what();
    return EXIT_FAILURE;
  }

  //
  // now push some dicom data in using storescp
  //

  QProcess *storescu = new QProcess(0);
  QStringList storescu_args;
  storescu_args << "-aec" << "CTK_AE";
  storescu_args << "-aet" << "CTK_AE";
  storescu_args << "localhost" << "11112";
  storescu_args << dicomData1;
  storescu_args << dicomData2;

  try
  {
    out << "running client" << storescu_exe << "\n";
    out << "with args" << storescu_args.join(" ") << "\n";
    storescu->start(storescu_exe, storescu_args);
    storescu->waitForFinished();
    out << "storescu Finished.\n";
    out << "Standard Output:\n";
    out << storescu->readAllStandardOutput();
    out << "Standard Error:\n";
    out << storescu->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  //
  // now query the server to see if the data arrived okay
  // - our database file will be updated with metadata from the query
  //

  QProcess *ctkDICOMQuery = new QProcess(0);
  QStringList ctkDICOMQuery_args;
  ctkDICOMQuery_args << ctkDICOMQuery_db_file;
  ctkDICOMQuery_args << "CTK_AE" << "CTK_AE";
  ctkDICOMQuery_args << "localhost" << "11112";

  try
  {
    out << "running client" << ctkDICOMQuery_exe << "\n";
    out << "with args" << ctkDICOMQuery_args.join(" ") << "\n";
    ctkDICOMQuery->start(ctkDICOMQuery_exe, ctkDICOMQuery_args);
    ctkDICOMQuery->waitForFinished();
    out << "ctkDICOMQuery Finished.\n";
    out << "Standard Output:\n";
    out << ctkDICOMQuery->readAllStandardOutput();
    out << "Standard Error:\n";
    out << ctkDICOMQuery->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  //
  // now do a retrieve into our download directory
  //

  // this is the study id of the dicom files we load from CTKData
  QString studyUID("1.2.840.113619.2.135.3596.6358736.5118.1115807980.182");

  QProcess *ctkDICOMRetrieve = new QProcess(0);
  QStringList ctkDICOMRetrieve_args;
  ctkDICOMRetrieve_args << studyUID;
  ctkDICOMRetrieve_args << ctkDICOMRetrieve_directory;
  ctkDICOMRetrieve_args << "CTK_AE" << "11113";
  ctkDICOMRetrieve_args << "CTK_AE";
  ctkDICOMRetrieve_args << "localhost" << "11112" << "CTK_CLIENT_AE";

  try
  {
    out << "running client" << ctkDICOMRetrieve_exe << "\n";
    out << "with args" << ctkDICOMRetrieve_args.join(" ") << "\n";
    ctkDICOMRetrieve->start(ctkDICOMRetrieve_exe, ctkDICOMRetrieve_args);
    ctkDICOMRetrieve->waitForFinished();
    out << "ctkDICOMRetrieve Finished.\n";
    out << "Standard Output:\n";
    out << ctkDICOMRetrieve->readAllStandardOutput();
    out << "Standard Error:\n";
    out << ctkDICOMRetrieve->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  //
  // clients are finished, not kill server and print output
  //
  try
  {
    dcmqrscp->kill();
    dcmqrscp->waitForFinished();
    out << "dcmqrscp Finished.\n";
    out << "Standard Output:\n";
    out << dcmqrscp->readAllStandardOutput();
    out << "Standard Error:\n";
    out << dcmqrscp->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

<commit_msg>STYLE: cleaned up comments in dicom qr testing code<commit_after>
// Qt includes
#include <QTextStream>
#include <QCoreApplication>
#include <QProcess>

// STD includes
#include <iostream>
#include <cstdlib>

  /*
   * This test performs a full dicom store, query, and retrieve.
   *
   * To recreate the parts, from the CTK-superbuild directory, run:
   *

./CMakeExternals/Install/bin/dcmqrscp -c ./CTK-build/Testing/Temporary/dcmqrscp.cfg -d -v

./CMakeExternals/Install/bin/storescu -aec CTK_AE -aet CTK_AE localhost 11112 ./CMakeExternals/Source/CTKData/Data/DICOM/MRHEAD/*.IMA

./CMakeExternals/Install/bin/findscu -aet CTK_AE -aec COMMONTK -P -k 0010,0010=\* localhost 11112 patqry.dcm

./CTK-build/bin/ctkDICOMQuery /tmp/test.db CTK_AE CTK_AE localhost 11112

./CTK-build/bin/ctkDICOMRetrieve 1.2.840.113619.2.135.3596.6358736.5118.1115807980.182  /tmp/hoot CTK_AE 11113 CTK_AE localhost 11112 CTK_CLIENT_AE
*/

int ctkDICOMApplicationTest1(int argc, char * argv []) {

  QCoreApplication app(argc, argv);
  QTextStream out(stdout);

  if ( argc < 10 )
  {
    out << "ERROR: invalid arguments.  Should be:\n";
    out << " ctkDICOMApplicationTest1 <dcmqrscp> <configfile> <dicomData1> <dcmData2> <storescu> <ctkDICOMQuery> <ctkDICOMRetrieve> <retrieveDirectory>\n";
    return EXIT_FAILURE;
  }

  QString dcmqrscp_exe  (argv[1]);
  QString dcmqrscp_cfg  (argv[2]);
  QString dicomData1  (argv[3]);
  QString dicomData2  (argv[4]);
  QString storescu_exe  (argv[5]);
  QString ctkDICOMQuery_exe  (argv[6]);
  QString ctkDICOMQuery_db_file  (argv[7]);
  QString ctkDICOMRetrieve_exe  (argv[8]);
  QString ctkDICOMRetrieve_directory  (argv[9]);

  //
  // first, start the server process
  //

  QProcess *dcmqrscp = new QProcess(0);
  QStringList dcmqrscp_args;
  dcmqrscp_args << "--config" << dcmqrscp_cfg;
  dcmqrscp_args << "--debug" << "--verbose";
  dcmqrscp_args << "11112";

  try
  {
    out << "starting server" << dcmqrscp_exe << "\n";
    out << "with args " << dcmqrscp_args.join(" ") << "\n";
    dcmqrscp->start(dcmqrscp_exe, dcmqrscp_args);
    dcmqrscp->waitForStarted();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start server" << e.what();
    return EXIT_FAILURE;
  }

  //
  // now push some dicom data in using storescp
  //

  QProcess *storescu = new QProcess(0);
  QStringList storescu_args;
  storescu_args << "-aec" << "CTK_AE";
  storescu_args << "-aet" << "CTK_AE";
  storescu_args << "localhost" << "11112";
  storescu_args << dicomData1;
  storescu_args << dicomData2;

  try
  {
    out << "running client" << storescu_exe << "\n";
    out << "with args" << storescu_args.join(" ") << "\n";
    storescu->start(storescu_exe, storescu_args);
    storescu->waitForFinished();
    out << "storescu Finished.\n";
    out << "Standard Output:\n";
    out << storescu->readAllStandardOutput();
    out << "Standard Error:\n";
    out << storescu->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  //
  // now query the server to see if the data arrived okay
  // - our database file will be updated with metadata from the query
  //

  QProcess *ctkDICOMQuery = new QProcess(0);
  QStringList ctkDICOMQuery_args;
  ctkDICOMQuery_args << ctkDICOMQuery_db_file;
  ctkDICOMQuery_args << "CTK_AE" << "CTK_AE";
  ctkDICOMQuery_args << "localhost" << "11112";

  try
  {
    out << "running client" << ctkDICOMQuery_exe << "\n";
    out << "with args" << ctkDICOMQuery_args.join(" ") << "\n";
    ctkDICOMQuery->start(ctkDICOMQuery_exe, ctkDICOMQuery_args);
    ctkDICOMQuery->waitForFinished();
    out << "ctkDICOMQuery Finished.\n";
    out << "Standard Output:\n";
    out << ctkDICOMQuery->readAllStandardOutput();
    out << "Standard Error:\n";
    out << ctkDICOMQuery->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  //
  // now do a retrieve into our download directory
  //

  // this is the study id of the dicom files we load from CTKData
  QString studyUID("1.2.840.113619.2.135.3596.6358736.5118.1115807980.182");

  QProcess *ctkDICOMRetrieve = new QProcess(0);
  QStringList ctkDICOMRetrieve_args;
  ctkDICOMRetrieve_args << studyUID;
  ctkDICOMRetrieve_args << ctkDICOMRetrieve_directory;
  ctkDICOMRetrieve_args << "CTK_AE" << "11113";
  ctkDICOMRetrieve_args << "CTK_AE";
  ctkDICOMRetrieve_args << "localhost" << "11112" << "CTK_CLIENT_AE";

  try
  {
    out << "running client" << ctkDICOMRetrieve_exe << "\n";
    out << "with args" << ctkDICOMRetrieve_args.join(" ") << "\n";
    ctkDICOMRetrieve->start(ctkDICOMRetrieve_exe, ctkDICOMRetrieve_args);
    ctkDICOMRetrieve->waitForFinished();
    out << "ctkDICOMRetrieve Finished.\n";
    out << "Standard Output:\n";
    out << ctkDICOMRetrieve->readAllStandardOutput();
    out << "Standard Error:\n";
    out << ctkDICOMRetrieve->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  //
  // clients are finished, not kill server and print output
  //
  try
  {
    dcmqrscp->kill();
    dcmqrscp->waitForFinished();
    out << "dcmqrscp Finished.\n";
    out << "Standard Output:\n";
    out << dcmqrscp->readAllStandardOutput();
    out << "Standard Error:\n";
    out << dcmqrscp->readAllStandardError();
  }
  catch (std::exception e)
  {
    out << "ERROR: could not start client" << e.what();
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

<|endoftext|>