m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat(slam): add rtabmap_ros

Browse Source
This commit is contained in:
X-lanni
2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/utilite/include/rtabmap/utilite/Posix/UThreadC.h
+411
View File
@@ -0,0 +1,411 @@
/////////////////////////////////////////////////////////////////////
// Written by Phillip Sitbon
// Copyright 2003
//
// Modified by Mathieu Labbe
//
// Posix/Thread.h
// - Posix thread
//
/////////////////////////////////////////////////////////////////////
#ifndef _U_Thread_Posix_
#define _U_Thread_Posix_
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/utilite/UMutex.h"
#include <pthread.h>
/**
* Calling thread sleeps for some milliseconds.
*/
inline void uSleep(unsigned int ms)
{
struct timespec req;
struct timespec rem;
req.tv_sec = ms / 1000;
req.tv_nsec = (ms - req.tv_sec * 1000) * 1000 * 1000;
nanosleep (&req, &rem);
}
/**
* Calling thread sleeps for some microseconds.
*/
inline void uSleepMicro(unsigned int us)
{
struct timespec req;
struct timespec rem;
req.tv_sec = us / 1000000;
req.tv_nsec = (us - req.tv_sec * 1000000) * 1000;
nanosleep (&req, &rem);
}
/**
* Calling thread sleeps for some nanoseconds.
*/
inline void uSleepNano(unsigned int ns)
{
struct timespec req;
struct timespec rem;
req.tv_sec = ns / 1000000000;
req.tv_nsec = (ns - req.tv_sec * 1000000000);
nanosleep (&req, &rem);
}
#define InvalidHandle 0
#define THREAD_HANDLE pthread_t
typedef void *( * pthread_fn )( void * );
template
<
typename Thread_T
>
class UThreadC
{
private:
struct Instance;
public:
typedef Thread_T & Thread_R;
typedef const Thread_T & Thread_C_R;
typedef THREAD_HANDLE Handle;
typedef void ( *Handler)( Thread_R );
virtual ~UThreadC() {}
protected:
UThreadC() {}
virtual void ThreadMain( Thread_R ) = 0;
static void Exit()
{ pthread_exit(0); }
#ifndef ANDROID
static void TestCancel()
{ pthread_testcancel(); }
#endif
static Handle Self()
{ return (Handle)pthread_self(); }
public:
static int Create(
const Handler & Function,
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
)
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(Param,0,Function,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
int Create(
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
) const
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(Param,const_cast<UThreadC *>(this),0,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
static int Join( Handle H )
{ return pthread_join(H,0); }
#ifndef ANDROID
static int Kill( Handle H )
{ return pthread_cancel(H); }
#endif
static int Detach( Handle H )
{ return pthread_detach(H); }
private:
static const UMutex &M_Create() { static UMutex M; return M; }
static USemaphore &S_Create() { static USemaphore S; return S; }
static void *ThreadMainHandler( Instance *Param )
{
Instance I(*Param);
Thread_T Data(I.Data);
S_Create().release();
#ifndef ANDROID
if ( I.Flags & 1 /*CancelEnable*/ )
{
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
if ( I.Flags & 2 /*CancelAsync*/ )
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
else
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
}
else
{
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);
}
#endif
if ( I.Owner )
I.Owner->ThreadMain(Data);
else
I.pFN(Data);
return 0;
}
struct Instance
{
Instance( Thread_C_R P, UThreadC<Thread_T> *const &O, const UThreadC<Thread_T>::Handler &pH = 0, const bool &CE=false, const bool &CA=false )
: Data(P), Owner(O), pFN(pH), Flags(0) { if ( CE ) Flags|=1; if ( CA ) Flags|=2; }
Thread_C_R Data;
UThreadC<Thread_T> * Owner;
Handler pFN;
unsigned char Flags;
};
};
/////////////////////////////////////////////////////////////////////
// Explicit specialization, no thread parameters
//
template<>
class UThreadC<void>
{
private:
struct Instance;
public:
typedef THREAD_HANDLE Handle;
typedef void ( *Handler)();
virtual ~UThreadC() {}
protected:
UThreadC() {}
virtual void ThreadMain() = 0;
static void Exit()
{ pthread_exit(0); }
#ifndef ANDROID
static void TestCancel()
{ pthread_testcancel(); }
#endif
static Handle Self()
{ return (Handle)pthread_self(); }
public:
static int Create(
const Handler & Function,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
)
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(0,Function,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
int Create(
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
) const
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(const_cast<UThreadC *>(this),0,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
int Create(
unsigned long & ThreadId,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
) const
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(const_cast<UThreadC *>(this),0,CancelEnable,CancelAsync);
*H = InvalidHandle;
int R = pthread_create((pthread_t *)&(*H),&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
ThreadId = (unsigned long)*H;
pthread_attr_destroy(&attr);
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
static int Join( Handle H )
{ return pthread_join(H,0); }
#ifndef ANDROID
static int Kill( Handle H )
{ return pthread_cancel(H); }
#endif
static int Detach( Handle H )
{ return pthread_detach(H); }
private:
static const UMutex &M_Create() { static UMutex M; return M; }
static USemaphore &S_Create() { static USemaphore S; return S; }
static void *ThreadMainHandler( Instance *Param )
{
Instance I(*Param);
S_Create().release();
#ifndef ANDROID
if ( I.Flags & 1 /*CancelEnable*/ )
{
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
if ( I.Flags & 2 /*CancelAsync*/ )
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
else
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
}
else
{
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);
}
#endif
if ( I.Owner )
I.Owner->ThreadMain();
else
I.pFN();
return 0;
}
struct Instance
{
Instance( UThreadC<void> *const &O, const UThreadC<void>::Handler &pH = 0, const bool &CE=false, const bool &CA=false )
: pFN(pH), Owner(O), Flags(0) { if ( CE ) Flags|=1; if ( CA ) Flags|=2; }
UThreadC<void>::Handler pFN;
UThreadC<void> * Owner;
unsigned char Flags;
};
};
#endif // !_U_Thread_Posix_
rtabmap/utilite/include/rtabmap/utilite/UConversion.h
+290
View File
@@ -0,0 +1,290 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UCONVERSION_H
#define UCONVERSION_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
#include <vector>
#include <stdarg.h>
/**
* \file UConversion.h
* \brief Some conversion functions
*
* This contains functions to do some convenient conversion like
* uNumber2str(), uBytes2Hex() or uHex2Bytes().
*/
/**
* Replace old characters in a string to new ones.
* Example :
* @code
* std::string str = "Hello";
* uReplaceChar(str, 'l', 'p');
* // The results is str = "Heppo";
* @endcode
*
* @param str the string
* @param before the character to be replaced by the new one
* @param after the new character replacing the old one
* @return the modified string
*/
std::string UTILITE_EXPORT uReplaceChar(const std::string & str, char before, char after);
/**
* Replace old characters in a string with the specified string.
* Example :
* @code
* std::string str = "Hello";
* uReplaceChar(str, 'o', "oween");
* // The results is str = "Helloween";
* @endcode
*
* @param str the string
* @param before the character to be replaced by the new one
* @param after the new string replacing the old character
* @return the modified string
*/
std::string UTILITE_EXPORT uReplaceChar(const std::string & str, char before, const std::string & after);
/**
* Transform characters from a string to upper case.
* Example :
* @code
* std::string str = "hello!";
* str = uToUpperCase(str);
* //str is now equal to "HELLO!"
* @endcode
* @param str the string
* @return the modified string
*/
std::string UTILITE_EXPORT uToUpperCase(const std::string & str);
/**
* Transform characters from a string to lower case.
* Example :
* @code
* std::string str = "HELLO!";
* str = uToLowerCase(str, false);
* //str is now equal to "hello!"
* @endcode
* @param str the string
* @return the modified string
*/
std::string UTILITE_EXPORT uToLowerCase(const std::string & str);
/**
* Convert a number (unsigned int) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(unsigned int number);
/**
* Convert a number (int) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(int number);
/**
* Convert a number (float) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(float number, int precision=6, bool fixed = false);
/**
* Convert a number (double) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(double number, int precision=6, bool fixed = false);
/**
* Convert a string to an integer.
* @param the string
* @return the number
*/
int UTILITE_EXPORT uStr2Int(const std::string & str);
/**
* Convert a string to a float independent of the locale (comma/dot).
* @param the string
* @return the number
*/
float UTILITE_EXPORT uStr2Float(const std::string & str);
/**
* Convert a string to a double independent of the locale (comma/dot).
* @param the string
* @return the number
*/
double UTILITE_EXPORT uStr2Double(const std::string & str);
/**
* Convert a bool to a string.
* The format used is "true" and "false".
* @param boolean the boolean to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uBool2Str(bool boolean);
/**
* Convert a string to a boolean.
* The format used is :
* "false", "FALSE" or "0" give false. All others give true.
* @param str the string to convert in a boolean
* @return the boolean
*/
bool UTILITE_EXPORT uStr2Bool(const char * str);
bool UTILITE_EXPORT uStr2Bool(const std::string & str);
/**
* Convert a string to an array of bytes including the null character ('\0').
* @param str the string
* @return the array of bytes
*/
std::vector<unsigned char> UTILITE_EXPORT uStr2Bytes(const std::string & str);
/**
* Convert an array of bytes to string, the array of bytes must end with the null character ('\0').
* @param bytes the array of bytes
* @return the string
*/
std::string UTILITE_EXPORT uBytes2Str(const std::vector<unsigned char> & bytes);
/**
* Convert a bytes array to an hexadecimal string.
* The resulting string is twice the size of the bytes array. The hexadecimal
* Characters are in upper case.
* Example :
* @code
* char bytes[] = {0x3F};
* std::string hex = uBytes2Hex(bytes, 1);
* // The string constains "3F".
* @endcode
*
* @param bytes the bytes array
* @param bytesLen the length of the bytes array
* @return the hexadecimal string
*/
std::string UTILITE_EXPORT uBytes2Hex(const char * bytes, unsigned int bytesLen);
/**
* Convert an hexadecimal string to a bytes array.
* The string must be pair length. The hexadecimal
* Characters can be in upper or lower case.
* Example :
* @code
* std::string hex = "1f3B";
* std::vector<char> bytes = uHex2Bytes(hex);
* // The array contains {0x1F, 0x3B}.
* @endcode
*
* @param hex the hexadecimal string
* @return the bytes array
*/
std::vector<char> UTILITE_EXPORT uHex2Bytes(const std::string & hex);
/**
* Convert an hexadecimal string to a bytes array.
* The string must be pair length. The hexadecimal
* Characters can be in upper or lower case.
* Example :
* @code
* std::vector<char> bytes = uHex2Bytes("1f3B", 4);
* // The array contains {0x1F, 0x3B}.
* @endcode
*
* @param hex the hexadecimal string
* @param bytesLen the hexadecimal string length
* @return the bytes array
*/
std::vector<char> UTILITE_EXPORT uHex2Bytes(const char * hex, int hexLen);
/**
* Convert an hexadecimal string to an ascii string. A convenient way
* when using only strings.
* The hexadecimal str MUST not contains any null values 0x00 ("00").
* Think to use of hex2bytes() to handle 0x00 values.
* Characters can be in upper or lower case.
* Example :
* @code
* std::string str = uHex2Str("48656C6C4F21");
* // The string contains "Hello!".
* @endcode
*
* @see hex2bytes
* @param hex the hexadecimal string
* @return the ascii string
*/
std::string UTILITE_EXPORT uHex2Str(const std::string & hex);
/**
* Convert hexadecimal (left or right part) value to an ascii character.
* Example :
* @code
* unsigned char F = uHex2Ascii(0xFA, false);
* unsigned char A = uHex2Ascii(0xFA, true);
* @endcode
* @see ascii2hex
* @param c the hexadecimal value
* @param rightPart If we want the character corresponding to the right of left part (4 bits) of the byte value.
* @return the ascii character (in upper case)
*/
unsigned char UTILITE_EXPORT uHex2Ascii(const unsigned char & c, bool rightPart);
/**
* Convert an ascii character to an hexadecimal value (right 4 bits).
* Characters can be in upper or lower case.
* Example :
* @code
* unsigned char hex = uAscii2Hex('F');
* // The results is hex = 0x0F;
* @endcode
* @see hex2ascii
* @param c the ascii character
* @return the hexadecimal value
*/
unsigned char UTILITE_EXPORT uAscii2Hex(const unsigned char & c);
/**
* Format a string like printf, and return it as a std::string
*/
std::string UTILITE_EXPORT uFormatv (const char *fmt, va_list ap);
/**
* Format a string like printf, and return it as a std::string
*/
std::string UTILITE_EXPORT uFormat (const char *fmt, ...);
#ifdef _WIN32
/**
* Convert multi-byte string to unicode (wide-char) string.
* Note that returned whar_t * must be deleted : delete [] wText;
*/
UTILITE_EXPORT wchar_t * createWCharFromChar(const char * text);
/**
* Convert unicode (wide-char) string to multi-byte string.
* Note that returned char * must be deleted : delete [] text;
*/
UTILITE_EXPORT char * createCharFromWChar(const wchar_t * wText);
#endif
#endif /* UCONVERSION_H */
rtabmap/utilite/include/rtabmap/utilite/UDestroyer.h
+75
View File
@@ -0,0 +1,75 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UDESTROYER_H
#define UDESTROYER_H
/**
* This class is used to delete a dynamically created
* objects. It was mainly designed to remove dynamically created Singleton.
* Created on the stack of a Singleton, when the
* application is finished, his destructor make sure that the
* Singleton is deleted.
*
*/
template <class T>
class UDestroyer
{
public:
/**
* The constructor. Set the doomed object (take ownership of the object). The object is deleted
* when this object is deleted.
*/
UDestroyer(T* doomed = 0) : doomed_(doomed) {}
~UDestroyer()
{
if(doomed_)
{
delete doomed_;
doomed_ = 0;
}
}
/**
* Set the doomed object. If a doomed object is already set, the function returns false.
* @param doomed the doomed object
* @return false if an object is already set and the new object is not null, otherwise true
*/
bool setDoomed(T* doomed)
{
if(doomed_ && doomed)
{
return false;
}
doomed_ = doomed;
return true;
}
private:
// Prevent users from making copies of a
// Destroyer to avoid double deletion:
UDestroyer(const UDestroyer<T>&);
void operator=(const UDestroyer<T>&);
private:
T* doomed_;
};
#endif // UDESTROYER_H
rtabmap/utilite/include/rtabmap/utilite/UDirectory.h
+143
View File
@@ -0,0 +1,143 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UDIRECTORY_H
#define UDIRECTORY_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
#include <vector>
#include <list>
/**
* Class UDirectory.
*
* This class can be used to get file names in a directory.
*/
class UTILITE_EXPORT UDirectory
{
public:
/**
* Check if a directory exists.
* @param dirPath the directory path
* @return true if the directory exists
*/
static bool exists(const std::string & dirPath);
/**
* Get the directory path of a file path.
* @param filePath the file path
* @return the directory path of the file
*/
static std::string getDir(const std::string & filePath);
/**
* Get the current directory.
* @param trailingSeparator If true, a '/' is added to the path.
* @return the current directory
*/
static std::string currentDir(bool trailingSeparator = false);
/**
* Make a directory.
* @param dirPath the directory path
* @return true on success, false otherwise.
*/
static bool makeDir(const std::string & dirPath);
/**
* Remove a directory.
* @param dirPath the directory path
* @return true on success, false otherwise.
*/
static bool removeDir(const std::string & dirPath);
/**
* Return the "home" directory.
* @return the directory path.
*/
static std::string homeDir();
/**
* Return \ (Win32) or / (Unix) depending of the platform.
*/
static std::string separator();
public:
/**
* Create a UDirectory object with path initialized to an existing "path" and with filter "extensions".
* @param path the path to an existing directory
* @param extensions filter to get only file names with the extensions specified, format is a
* list of extensions separated by a space: "jpg bmp" get only file names finishing by jpg or bmp.
*/
UDirectory(const std::string & path = "", const std::string & extensions = "");
UDirectory(const UDirectory & dir);
UDirectory & operator=(const UDirectory & dir);
~UDirectory();
/**
* Set path of the directory.
* @param path the new directory path.
*/
void setPath(const std::string & path, const std::string & extensions = "");
/**
* Update indexed file names (if the directory changed).
*/
void update();
/**
* Check is the directory exists.
* @return if directory exists.
*/
bool isValid();
/**
* Get the next file name.
* @return the next file name
*/
std::string getNextFileName();
/**
* Get the next file path.
* @return the next file path
*/
std::string getNextFilePath();
/**
* Get all file names.
* @see UDirectory()
* @return all the file names in directory matching the set extensions.
*/
const std::list<std::string> & getFileNames() const {return fileNames_;}
/**
* Return the pointer of file names to beginning.
*/
void rewind();
private:
std::string path_;
std::vector<std::string> extensions_;
std::list<std::string> fileNames_;
std::list<std::string>::iterator iFileName_;
};
#endif /* UDIRECTORY_H */
rtabmap/utilite/include/rtabmap/utilite/UEvent.h
+87
View File
@@ -0,0 +1,87 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEVENT_H
#define UEVENT_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
class UEventsHandler;
/**
* This is the base class for all events used
* with the UEventsManager. Inherited classes
* must redefined the virtual method getClassName()
* to return their class name.
*
* Example:
* @code
* class MyEvent : public UEvent
* {
* public:
* MyEvent() {}
* virtual ~MyEvent() {}
* std::string getClassName() const {return "MyEvent";}
* };
*
* int main(int argc, char * argv[])
* {
* ...
* UEventsManager::post(new MyEvent()); // UEventsManager take ownership of the event (deleted by UEventsManager).
* ...
* }
* @endcode
*
* @see UEventsManager
* @see UEventsHandler
* @see getClassName()
*/
class UTILITE_EXPORT UEvent{
public:
virtual ~UEvent() {}
/**
* This method is used to get the class name
* of the event. For example, if a class MouseEvent
* inherits from UEvent, it must return the
* "MouseEvent" string.
* @return the class name
*/
virtual std::string getClassName() const = 0; // TODO : macro?
/**
* Get event's code.
* @return the code
*/
int getCode() const {return code_;}
protected:
/**
* @param code the event code.
* TODO : Remove the code, not required for most of all implemented events
*/
UEvent(int code = 0) : code_(code) {}
private:
int code_; /**< The event's code. */
};
#endif // UEVENT_H
rtabmap/utilite/include/rtabmap/utilite/UEventsHandler.h
+188
View File
@@ -0,0 +1,188 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEVENTSHANDLER_H
#define UEVENTSHANDLER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UEventsSender.h"
class UEvent;
/**
* The class UEventsHandler is an abstract class for
* handling events.
*
* Inherited classes must implement handleEvent() function, which
* is called by the UEventsManager when an event is dispatched. Once the handler is
* created, it must be added to events manager with UEventsManager::addHandler() function.
* Note that it is not safe to automatically add the handler to UEventsManager in the handler's constructor.
*
* Note for multi-threading: the handleEvent() method is called
* inside the UEventsManager thread. If the inherited class also inherits
* from UThreadNode, handleEvent() is done as well outside the thread's main loop, so
* be careful to protect private data of the thread used in its main loop.
*
* Example for a useful combination of an UEventsHandler and a UThreadNode, with safe data
* modification while not blocking the handleEvent() call on a mutex:
* @code
* #include "utilite/UThreadNode.h"
* #include "utilite/UEventsHandler.h"
* #include "utilite/UEventsManager.h"
* #include "utilite/UEvent.h"
*
* // Implement a simple event
* class ResetEvent : public UEvent {
* public:
* ResetEvent() {}
* virtual ~ResetEvent() {}
* virtual std::string getClassName() const {return "ResetEvent";} // Must be implemented
* };
*
* // There is the thread counting indefinitely, the count can be reseted by sending a ResetEvent.
* class CounterThread : public UThreadNode, public UEventsHandler {
* public:
* CounterThread() : state_(0), count_(0) {}
* virtual ~CounterThread() {this->join(true);}
*
* protected:
* virtual void mainLoop() {
* if(state_ == 1) {
* state_ = 0;
* // Do a long initialization, reset memory or other special long works... here
* // we reset the count. This could be done in the handleEvent() but
* // with many objects, it is more safe to do it here (in the thread's loop). A safe
* // way could be also to use a UMutex to protect this initialization in
* // the handleEvent(), but it is not recommended to do long works in handleEvent()
* // because this will add latency in the UEventsManager dispatching events loop.
* count_ = 0; // Reset the count
* printf("Reset!\n");
* }
*
* // Do some works...
* printf("count=%d\n", count_++);
* uSleep(100); // wait 100 ms
* }
* virtual void handleEvent(UEvent * event) {
* if(event->getClassName().compare("ResetEvent") == 0) {
* state_ = 1;
* }
* }
* private:
* int state_;
* int count_;
* };
*
* int main(int argc, char * argv[])
* {
* CounterThread counter;
* counter.start();
* UEventsManager::addHandler(&counter);
*
* uSleep(500); // wait 500 ms before sending a reset event
* UEventsManager::post(new ResetEvent());
* uSleep(500); // wait 500 ms before termination
*
* UEventsManager::removeHandler(&counter);
* counter.join(true); // Kill and wait to finish
* return 0;
* }
* @endcode
*
* The output is:
* @code
* count=0
* count=1
* count=2
* count=3
* count=4
* Reset!
* count=0
* count=1
* count=2
* count=3
* count=4
* @endcode
*
* @see UEventsManager
* @see UEvent
* @see UThreadNode
*
*/
class UTILITE_EXPORT UEventsHandler : public UEventsSender {
public:
void registerToEventsManager();
void unregisterFromEventsManager();
protected:
/**
* Only the UEventsManager has access
* to the handleEvent() method.
*/
friend class UEventsManager;
/**
* Method called by the UEventsManager
* to handle an event. Important : this method
* must do a minimum of work because the faster
* the dispatching loop is done; the faster the
* events are received. If a handling function
* takes too much time, the events list can grow
* faster than it is emptied. The event can be
* modified.
* @return "true" to notify UEventsManager that this handler took ownership of the
* event (meaning it must delete it). The event will
* not be dispatched to next handlers.
* @return "false" to let event be dispatched to next handlers (default behavior). UEventsManager
* will take care of deleting the event.
*
*/
virtual bool handleEvent(UEvent * event) = 0;
protected:
/**
* UEventsHandler constructor.
*
* Note : You can call EventsManager::addHandler(this) at
* the end of the constructor of the inherited class where the virtual
* method handleEvent(...) is defined. If so, the UEventsHandler doesn't
* need to be manually added to the EventsManager where the handler
* is instantiated. We decided to not include UEventsManager::addHandler(this)
* in this abstract class constructor because an event can be handled (calling
* the pure virtual method) while the concrete class is constructed.
*/
UEventsHandler() {}
/**
* UEventsHandler destructor.
*
* By default, it removes the handler reference from the UEventsManager. To be thread-safe,
* the inherited class must remove itself from the UEventsManager before it is deleted because
* an event can be handled (calling the pure virtual method handleEvent()) after the concrete class
* is deleted.
*/
virtual ~UEventsHandler();
private:
};
#endif // UEVENTSHANDLER_H
rtabmap/utilite/include/rtabmap/utilite/UEventsManager.h
+253
View File
@@ -0,0 +1,253 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEVENTSMANAGER_H
#define UEVENTSMANAGER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UDestroyer.h"
#include <list>
#include <map>
// TODO Not implemented... for multithreading event handling
class UEventDispatcher : public UThread
{
public:
virtual ~UEventDispatcher();
protected:
friend class UEventsManager;
UEventDispatcher();
virtual void mainLoop();
private:
virtual void killCleanup();
private:
UEvent * _event;
std::vector<UEventsHandler*> _handlers;
};
/**
* This class is used to post events between threads
* in the application. It is Thread-Safe and the events are sent
* to receivers in the same order they are posted (FIFO). It works
* like the design pattern Mediator. It is also a Singleton, so
* it can be used anywhere in the application.
*
* To send an event, use UEventsManager::post().
* Events are automatically deleted after they are posted.
*
* The EventsManager have a list of handlers to which
* it sends posted events. To add an handler, use
* UEventsManager::addHandler(). To remove, use
* UEventsManager::removeHandler().
*
* @code
* // Anywhere in the code:
* UEventsManager::post(new MyEvent()); // where MyEvent is an implemented UEvent
* @endcode
*
* @see UEvent
* @see UEventsHandler
* @see post()
* @see addHandler()
* @see removeHandler()
*/
class UTILITE_EXPORT UEventsManager : public UThread{
public:
/**
* This method is used to add an events
* handler to the list of handlers.
*
* @param handler the handler to be added.
*/
static void addHandler(UEventsHandler* handler);
/**
* This method is used to remove an events
* handler from the list of handlers.
*
* @param handler the handler to be removed.
*/
static void removeHandler(UEventsHandler* handler);
/**
* This method is used to post an event to
* handlers.
*
* Event can be posted asynchronously or not. In the first case,
* the event is dispatched by the UEventsManager's thread. In the
* second case, the event is handled immediately by event's
* receivers, thus in the sender thread.
*
* @param event the event to be posted.
* @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
*/
static void post(UEvent * event, bool async = true, const UEventsSender * sender = 0);
static void createPipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
static void removePipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
static void removeAllPipes(const UEventsSender * sender);
static void removeNullPipes(const UEventsSender * sender);
protected:
/*
* This method is used to have a reference on the
* EventsManager. When no EventsManager exists, one is
* created. There is only one instance in the application.
* See the Singleton pattern further explanation.
*
* @return the reference on the EventsManager
*/
static UEventsManager* getInstance();
/*
* Called only once in getInstance(). It can't be instantiated
* by the user.
*
*/
UEventsManager();
/*
* Only called by a Destroyer.
*/
virtual ~UEventsManager();
/*
* A Destroyer is used to remove a dynamically created
* Singleton. It is friend here to have access to the
* destructor.
*
*/
friend class UDestroyer<UEventsManager>;
/**
* The UEventsManager's main loop.
*/
virtual void mainLoop();
private:
/**
* Reimplemented to wake up UEventsManager on termination.
*/
virtual void mainLoopKill();
/*
* This method dispatches asynchronized events to all handlers.
* FIFO (first in first out) dispatching is used.
*/
virtual void dispatchEvents();
/*
* This method dispatches an event to all handlers.
*/
virtual bool dispatchEvent(UEvent * event, const UEventsSender * sender);
/*
* This method is used to add an events
* handler to the list of handlers.
*
* @param handler the handler to be added.
*/
void _addHandler(UEventsHandler* handler);
/*
* This method is used to remove an events
* handler from the list of handlers.
*
* @param handler the handler to be removed.
*/
void _removeHandler(UEventsHandler* handler);
/*
* This method is used to post an event to
* handlers.
*
* Event can be posted asynchronously or not. In the first case,
* the event is dispatched by the UEventsManager's thread. In the
* second case, the event is handled immediately by event's
* receivers, thus in the sender thread.
*
* @param event the event to be posted.
* @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
*/
void _postEvent(UEvent * event, bool async = true, const UEventsSender * sender = 0);
std::list<UEventsHandler*> getPipes(
const UEventsSender * sender,
const std::string & eventName);
void _createPipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
void _removePipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
void _removeAllPipes(const UEventsSender * sender);
void _removeNullPipes(const UEventsSender * sender);
private:
class Pipe
{
public:
Pipe(const UEventsSender * sender, const UEventsHandler * receiver, const std::string & eventName) :
sender_(sender),
receiver_(receiver),
eventName_(eventName)
{}
const UEventsSender * sender_;
const UEventsHandler * receiver_;
const std::string eventName_;
};
static UEventsManager* instance_; /* The EventsManager instance pointer. */
static UDestroyer<UEventsManager> destroyer_; /* The EventsManager's destroyer. */
std::list<std::pair<UEvent*, const UEventsSender * > > events_; /* The events list. */
std::list<UEventsHandler*> handlers_; /* The handlers list. */
UMutex eventsMutex_; /* The mutex of the events list, */
UMutex handlersMutex_; /* The mutex of the handlers list. */
USemaphore postEventSem_; /* Semaphore used to signal when an events is posted. */
std::list<Pipe> pipes_;
UMutex pipesMutex_;
};
#endif // UEVENTSMANAGER_H
rtabmap/utilite/include/rtabmap/utilite/UEventsSender.h
+31
View File
@@ -0,0 +1,31 @@
/*
* UEventsSender.h
*
* Created on: 2013-10-14
* Author: Mathieu
*/
#ifndef UEVENTSSENDER_H_
#define UEVENTSSENDER_H_
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
class UEvent;
class UTILITE_EXPORT UEventsSender
{
public:
UEventsSender(){}
virtual ~UEventsSender();
protected:
/**
* For convenience to post an event. This is the same than calling UEventsManager::post()
* with the sender reference.
*/
void post(UEvent * event, bool async = true) const;
};
#endif /* UEVENTSSENDER_H_ */
rtabmap/utilite/include/rtabmap/utilite/UException.h
+33
View File
@@ -0,0 +1,33 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEXCEPTION_H_
#define UEXCEPTION_H_
#include <stdexcept>
class UException: public std::runtime_error {
public:
UException(const std::string & what) :
std::runtime_error(what)
{}
};
#endif /* UEXCEPTION_H_ */
rtabmap/utilite/include/rtabmap/utilite/UFile.h
+152
View File
@@ -0,0 +1,152 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef FILE_H
#define FILE_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UDirectory.h"
#include <string>
/**
* Class UFile.
*
* This class can be used to modify/erase files on hard drive.
*/
class UTILITE_EXPORT UFile
{
public:
/**
* Check if a file exists.
* @param filePath the file path
* @return true if the file exists, otherwise false.
*/
static bool exists(const std::string &filePath);
/**
* Get the file length.
* @param filePath the file path
* @return long the length of the file in bytes. Return -1 if the file doesn't exist.
*/
static long length(const std::string &filePath);
/**
* Erase a file.
* @param filePath the file path
* @return 0 if success.
*/
static int erase(const std::string &filePath);
/**
* Rename a file.
* @param oldFilePath the old file path
* @param newFilePath the new file path
* @return 0 if success.
*/
static int rename(const std::string &oldFilePath,
const std::string &newFilePath);
/**
* Get the file name from a file path (with extension).
* @param filePath the file path
* @return the file name.
*/
static std::string getName(const std::string & filePath);
/**
* Get the file extension.
* @return the file extension
*/
static std::string getExtension(const std::string &filePath);
/**
* Copy a file.
* @param from the file path
* @param to destination file path
*/
static void copy(const std::string & from, const std::string & to);
public:
/**
* Create a UFile object with path initialized to an existing file .
* @param path the path to an existing file
*/
UFile(const std::string & path) : path_(path) {}
~UFile() {}
/**
* Check if the file exists. Same as exists().
* @return true if the path exits
*/
bool isValid() {return exists(path_);}
/**
* Check if the file exists.
* @return true if the path exits
*/
bool exists() {return exists(path_);}
/**
* Get the length of the file.
* @return long the length of the file in bytes. Return -1 if the file doesn't exist.
*/
long length() {return length(path_);}
/**
* Rename the file name. The path stays the same.
* @param the new name
*/
int rename(const std::string &newName)
{
std::string ext = this->getExtension();
std::string newPath = UDirectory::getDir(path_) + std::string("/") + newName;
if(ext.size())
{
newPath += std::string(".") + getExtension(path_);
}
int result = rename(path_, newPath);
if(result == 0)
{
path_ = newPath;
}
return result;
}
/**
* Get the file name without the path.
* @return the file name
*/
std::string getName() {return getName(path_);}
/**
* Get the file extension.
* @return the file extension
*/
std::string getExtension() {return getExtension(path_);}
/**
* Copy a file.
* @param to destination file path
*/
void copy(const std::string & to) {copy(path_, to);}
private:
std::string path_;
};
#endif
rtabmap/utilite/include/rtabmap/utilite/ULogger.h
+575
View File
@@ -0,0 +1,575 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef ULOGGER_H
#define ULOGGER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/UDestroyer.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UException.h"
#include <stdio.h>
#include <time.h>
#include <string>
#include <vector>
#include <map>
#include <set>
#include <stdarg.h>
/**
* \file ULogger.h
* \brief ULogger class and convenient macros
*
* This contains macros useful for logging a message anywhere in the
* application. Once the ULogger is set, use these macros like a printf to
* print debug messages.
*/
/*
* Convenient macros for logging...
*/
#define ULOGGER_LOG(level, ...) ULogger::write(level, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__)
#define ULOGGER_DEBUG(...) ULOGGER_LOG(ULogger::kDebug, __VA_ARGS__)
#define ULOGGER_INFO(...) ULOGGER_LOG(ULogger::kInfo, __VA_ARGS__)
#define ULOGGER_WARN(...) ULOGGER_LOG(ULogger::kWarning, __VA_ARGS__)
#define ULOGGER_ERROR(...) ULOGGER_LOG(ULogger::kError, __VA_ARGS__)
#define ULOGGER_FATAL(...) ULOGGER_LOG(ULogger::kFatal, __VA_ARGS__) // Throw UException
#define UDEBUG(...) ULOGGER_DEBUG(__VA_ARGS__)
#define UINFO(...) ULOGGER_INFO(__VA_ARGS__)
#define UWARN(...) ULOGGER_WARN(__VA_ARGS__)
#define UERROR(...) ULOGGER_ERROR(__VA_ARGS__)
#define UFATAL(...) ULOGGER_FATAL(__VA_ARGS__) // Throw UException
// Throw UException
#define UASSERT(condition) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met!", #condition)
#define UASSERT_MSG(condition, msg_str) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met! [%s]", #condition, msg_str)
/**
* \def UDEBUG(...)
* Print a debug level message in the logger. Format is the same as a printf:
* @code
* UDEBUG("This is a debug message with the number %d", 42);
* @endcode
*/
/**
* \def UINFO(...)
* Print a information level message in the logger. Format is the same as a printf:
* @code
* UINFO("This is a information message with the number %d", 42);
* @endcode
*/
/**
* \def UWARN(...)
* Print a warning level message in the logger. Format is the same as a printf:
* @code
* UWARN("This is a warning message with the number %d", 42);
* @endcode
*/
/**
* \def UERROR(...)
* Print an error level message in the logger. Format is the same as a printf:
* @code
* UERROR("This is an error message with the number %d", 42);
* @endcode
*/
/**
* \def UFATAL(...)
* Print a fatal error level message in the logger. The application will exit on
* fatal error. Format is the same as a printf:
* @code
* UFATAL("This is a fatal error message with the number %d", 42);
* @endcode
*/
/**
* \def UASSERT(condition, ...)
* Print a fatal error level message in the logger if condition is not met. The application will exit on
* fatal error. Format is the same as a printf:
* @code
* UASSERT(a!=42, "This is a fatal error message with the number %d", 42);
* @endcode
*/
/**
* This class is used by the ULogger to send logged messages like events. Messages with level
* over the event level set in ULogger::setEventLevel() are sent like ULogEvent with the message and its level.
* The default event level of ULogger is kFatal (see ULogger::Level).
*/
class ULogEvent : public UEvent
{
public:
/**
* ULogEvent constructor. Note that to retrieve the message level, use UEvent::getCode().
* @param msg the message already formatted to a full string.
* @param level the severity of the message, @see ULogger::Level.
*/
ULogEvent(const std::string & msg, int level) :
UEvent(level),
msg_(msg)
{}
virtual ~ULogEvent() {}
/**
* Get the message from the event.
*/
const std::string & getMsg() const {return msg_;}
/**
* @return string "ULogEvent"
*/
virtual std::string getClassName() const {return "ULogEvent";}
private:
std::string msg_;
};
/**
* This class is used to log messages with time on a console, in a file
* and/or with an event. At the start of the application, call
* ULogger::setType() with the type of the logger you want (see ULogger::Type, the type of the output
* can be changed at the run-time.). To use it,
* simply call the convenient macros UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL() depending of
* the severity of the message. You can disable some messages by setting the logger
* level ULogger::setLevel() to severity you want, defined by ULogger::Level. A fatal message
* will make the application to exit, printing the message on console (whatever the logger type) and
* posting a ULogEvent (synchronously... see UEventsManager::post()) before exiting.
*
* The display of the logged messages can be modified:
* - If you don't want the level label, set ULogger::setPrintLevel() to false.
* - If you don't want the time label, set ULogger::setPrintTime() to false.
* - If you don't want the end of line added, set ULogger::setPrintEndline() to false.
* - If you don't the full path of the message, set ULogger::setPrintWhereFullPath() to false.
* - If you don't the path of the message, set ULogger::setPrintWhere() to false.
*
* When using a file logger (kTypeLogger), it can be useful in some
* application to buffer messages before writing them to hard drive (avoiding
* hard drive latencies). You can set ULogger::setBuffered() to true to do that. When the
* buffered messages will be written to file on appllciation exit (ULogger destructor) or when
* ULogger::flush() is called.
*
* If you want the application to exit on a lower severity level than kFatal,
* you can set ULogger::setExitLevel() to any ULogger::Type you want.
*
* Example:
* @code
* #include <utilite/ULogger.h>
* int main(int argc, char * argv[])
* {
* // Set the logger type. The choices are kTypeConsole,
* // kTypeFile or kTypeNoLog (nothing is logged).
* ULogger::setType(ULogger::kTypeConsole);
*
* // Set the logger severity level (kDebug, kInfo, kWarning, kError, kFatal).
* // All log entries under the severity level are not logged. Here,
* // only debug messages are not logged.
* ULogger::setLevel(ULogger::kInfo);
*
* // Use a predefined Macro to easy logging. It can be
* // called anywhere in the application as the logger is
* // a Singleton.
* UDEBUG("This message won't be logged because the "
* "severity level of the logger is set to kInfo.");
*
* UINFO("This message is logged.");
*
* UWARN("A warning message...");
*
* UERROR("An error message with code %d.", 42);
*
* return 0;
* }
* @endcode
* Output:
* @code
* [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() This message is logged.
* [ WARN] (2010-09-25 18:08:20) main.cpp:20::main() A warning message...
* [ERROR] (2010-09-25 18:08:20) main.cpp:22::main() An error message with code 42.
* @endcode
*
* Another useful form of the ULogger is to use it with the UTimer class. Here an example:
* @code
* #include <utilite/ULogger.h>
* #include <utilite/UTimer.h>
* ...
* UTimer timer; // automatically starts
* // do some works for part A
* UINFO("Time for part A = %f s", timer.ticks());
* // do some works for part B
* UINFO("Time for part B = %f s", timer.ticks());
* // do some works for part C
* UINFO("Time for part C = %f s", timer.ticks());
* ...
* @endcode
*
* @see setType()
* @see setLevel()
* @see UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL()
*
*/
class UTILITE_EXPORT ULogger
{
public:
/**
* The default log file name.
*/
static const std::string kDefaultLogFileName;
/**
* Loggers available:
* @code
* kTypeNoLog, kTypeConsole, kTypeFile
* @endcode
*/
enum Type{kTypeNoLog, kTypeConsole, kTypeFile};
/**
* Logger levels, from lowest severity to highest:
* @code
* kDebug, kInfo, kWarning, kError, kFatal
* @endcode
*/
enum Level{kDebug, kInfo, kWarning, kError, kFatal};
/**
* Set the type of the logger. When using kTypeFile, the parameter "fileName" would be
* changed (default is "./ULog.txt"), and optionally "append" if we want the
* logger to append messages to file or to overwrite the file.
* @param type the ULogger::Type of the logger.
* @param fileName file name used with a file logger type.
* @param append if true, the file isn't overwritten, otherwise it is.
*
* TODO : Can it be useful to have 2 or more types at the same time ? Print
* in console and file at the same time.
*/
static void setType(Type type, const std::string &fileName = kDefaultLogFileName, bool append = true);
static Type type() {return type_;}
// Setters
/**
* Print time: default true.
* @param printTime true to print time, otherwise set to false.
*/
static void setPrintTime(bool printTime) {printTime_ = printTime;}
static bool isPrintTime() {return printTime_;}
/**
* Print level: default true.
* @param printLevel true to print level, otherwise set to false.
*/
static void setPrintLevel(bool printLevel) {printLevel_ = printLevel;}
static bool isPrintLevel() {return printLevel_;}
/**
* Print end of line: default true.
* @param printLevel true to print end of line, otherwise set to false.
*/
static void setPrintEndline(bool printEndline) {printEndline_ = printEndline;}
static bool isPrintEndLine() {return printEndline_;}
/**
* Print text with color: default true.
* Dark green for Debug, white for Info, yellow for Warning, red for Error and Fatal.
* @param printColored true to print text with color, otherwise set to false.
*/
static void setPrintColored(bool printColored) {printColored_ = printColored;}
static bool isPrintColored() {return printColored_;}
/**
* Print where is this message in source code: default true.
* @param printWhere true to print where, otherwise set to false.
*/
static void setPrintWhere(bool printWhere) {printWhere_ = printWhere;}
static bool isPrintWhere() {return printWhere_;}
/**
* Print thread ID: default false.
* @param printThreadId true to print where, otherwise set to false.
*/
static void setPrintThreadId(bool printThreadId) {printThreadID_ = printThreadId;}
static bool isPrintThreadId() {return printThreadID_;}
/**
* Print the full path: default true. ULogger::setPrintWhere() must be true to have path printed.
* @param printWhereFullPath true to print the full path, otherwise set to false.
*/
static void setPrintWhereFullPath(bool printWhereFullPath) {printWhereFullPath_ = printWhereFullPath;}
static bool isPrintWhereFullPath() {return printWhereFullPath_;}
/**
* Set is the logger buffers messages, default false. When true, the messages are
* buffered until the application is closed or ULogger::flush() is called.
* @see ULogger::flush()
* @param buffered true to buffer messages, otherwise set to false.
*/
static void setBuffered(bool buffered);
static bool isBuffered() {return buffered_;}
/**
* Set logger level: default kInfo. All messages over the severity set
* are printed, other are ignored. The severity is from the lowest to
* highest:
* - kDebug
* - kInfo
* - kWarning
* - kError
* - kFatal
* @param level the minimum level of the messages printed.
*/
static void setLevel(ULogger::Level level) {level_ = level;}
static ULogger::Level level() {return level_;}
/**
* An ULogEvent is sent on each message logged at the specified level.
* Note : On message with level >= exitLevel, the event is sent synchronously (see UEventsManager::post()).
* @see ULogEvent
* @see setExitLevel()
*/
static void setEventLevel(ULogger::Level eventSentLevel) {eventLevel_ = eventSentLevel;}
static ULogger::Level eventLevel() {return eventLevel_;}
/**
* If not empty, only show log messages from threads included in this list.
*/
static void setTreadIdFilter(const std::set<unsigned long> & ids) {threadIdFilter_ = ids;}
static void setTreadIdFilter(const std::vector<std::string> & ids);
static const std::set<unsigned long> & getTreadIdFilter() {return threadIdFilter_;}
/**
* Threads can register to this list. If name is empty, it will
* clear the thread in the list. Should be called from the thread itself.
*/
static void registerCurrentThread(const std::string & name);
static void unregisterCurrentThread();
static std::map<std::string, unsigned long> getRegisteredThreads();
/**
* Reset to default parameters.
*/
static void reset();
/**
* Flush buffered messages.
* @see setBuffered()
*/
static void flush();
/**
* Write a message directly to logger without level handling.
* @param msg the message to write.
* @param ... the variable arguments
* @deprecated use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL()
*/
static void write(const char* msg, ...);
/*
* Write a message to logger: use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL() instead.
* @param level the log level of this message
* @param file the file path
* @param line the line in the file
* @param function the function name in which the message is logged
* @param msg the message to write
* @param ... the variable arguments
*/
static void write(ULogger::Level level,
const char * file,
int line,
const char *function,
const char* msg,
...);
/**
* Get the time in the format "2008-7-13 12:23:44".
* @param timeStr string were the time will be copied.
* @return the number of characters written, or 0 if an error occurred.
*/
static int getTime(std::string &timeStr);
protected:
/*
* This method is used to have a reference on the
* Logger. When no Logger exists, one is
* created. There is only one instance in the application.
* Must be protected by loggerMutex_.
* See the Singleton pattern for further explanation.
*
* @return the reference on the Logger
*/
static ULogger* getInstance();
/*
* Called only once in getInstance(). It can't be instanciated
* by the user.
*
* @see getInstance()
*/
ULogger() {}
/*
* Only called by a Destroyer.
* @see Destroyer
*/
virtual ~ULogger();
/*
* Flush buffered messages
*/
void _flush();
/*
* A Destroyer is used to remove a dynamicaly created
* Singleton. It is friend here to have access to the
* destructor.
*
* @see Destroyer
*/
friend class UDestroyer<ULogger>;
/*
* The log file name.
*/
static std::string logFileName_;
/*
* Default true, it doesn't overwrite the file.
*/
static bool append_;
private:
/*
* Create an instance according to type. See the Abstract factory
* pattern for further explanation.
* @see type_
* @return the reference on the new logger
*/
static ULogger* createInstance();
/*
* Write a message on the output with the format :
* "A message". Inherited class
* must override this method to output the message. It
* does nothing by default.
* @param msg the message to write.
* @param arg the variable arguments
*/
virtual void _write(const char*, va_list) {} // Do nothing by default
virtual void _writeStr(const char*) {} // Do nothing by default
private:
/*
* The Logger instance pointer.
*/
static ULogger* instance_;
/*
* The Logger's destroyer
*/
static UDestroyer<ULogger> destroyer_;
/*
* If the logger prints the time for each message.
* Default is true.
*/
static bool printTime_;
/*
* If the logger prints the level for each message.
* Default is true.
*/
static bool printLevel_;
/*
* If the logger prints the end line for each message.
* Default is true.
*/
static bool printEndline_;
/*
* If the logger prints text with color.
* Default is true.
*/
static bool printColored_;
/*
* If the logger prints where the message is logged (fileName::function():line).
* Default is true.
*/
static bool printWhere_;
/*
* If the logger prints the full path of the source file
* where the message is written. Only works when
* "printWhere_" is true.
* Default is false.
*/
static bool printWhereFullPath_;
/*
* If the logger prints the thread ID.
* Default is false.
*/
static bool printThreadID_;
/*
* If the logger limit the size of the "where" path to
* characters. If the path is over 8 characters, a "~"
* is added. Only works when "printWhereFullPath_" is false.
* Default is false.
*/
static bool limitWhereLength_;
/*
* The type of the logger.
*/
static Type type_;
/*
* The severity of the log.
*/
static Level level_;
/*
* The severity at which the message is also sent in a ULogEvent.
*/
static Level eventLevel_;
static const char * levelName_[5];
/*
* Mutex used when accessing public functions.
*/
static UMutex loggerMutex_;
/*
* If the logger prints messages only when ULogger::flush() is called.
* Default is false.
*/
static bool buffered_;
static std::string bufferedMsgs_;
static std::set<unsigned long> threadIdFilter_;
static std::map<std::string, unsigned long> registeredThreads_;
};
#endif // ULOGGER_H
rtabmap/utilite/include/rtabmap/utilite/UMath.h
+937
View File
@@ -0,0 +1,937 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UMATH_H
#define UMATH_H
/** \file UMath.h
\brief Basic mathematics functions
*/
#include <cmath>
#include <list>
#include <vector>
#if _MSC_VER
#undef min
#undef max
#endif
/**
* Return true if the number is NAN.
*/
template<class T>
inline bool uIsNan(const T & value)
{
#if _MSC_VER
return _isnan(value) != 0;
#else
return std::isnan(value);
#endif
}
/**
* Return true if the number is finite.
*/
template<class T>
inline bool uIsFinite(const T & value)
{
#if _MSC_VER
return _finite(value) != 0;
#else
return std::isfinite(value);
#endif
}
/**
* Get the minimum of the 3 values.
* @return the minimum value
*/
template<class T>
inline T uMin3( const T& a, const T& b, const T& c)
{
float m=a<b?a:b;
return m<c?m:c;
}
/**
* Get the maximum of the 3 values.
* @return the maximum value
*/
template<class T>
inline T uMax3( const T& a, const T& b, const T& c)
{
float m=a>b?a:b;
return m>c?m:c;
}
/**
* Get the maximum of a vector.
* @param v the array
* @param size the size of the array
* @param index the index of the maximum value in the vector.
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const T * v, unsigned int size, unsigned int & index)
{
T max = 0;
index = 0;
if(!v || size == 0)
{
return max;
}
max = v[0];
for(unsigned int i=1; i<size; ++i)
{
if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
{
max = v[i];
index = i;
}
}
return max;
}
/**
* Get the maximum of a vector.
* @param v the array
* @param index the index of the maximum value in the vector.
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const std::vector<T> & v, unsigned int & index)
{
return uMax(v.data(), v->size(), index);
}
/**
* Get the maximum of a vector.
* @param v the array
* @param size the size of the array
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const T * v, unsigned int size)
{
unsigned int index;
return uMax(v, size, index);
}
/**
* Get the maximum of a vector.
* @param v the array
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const std::vector<T> & v)
{
return uMax(v.data(), v.size());
}
/**
* Get the minimum of a vector.
* @param v the array
* @param size the size of the array
* @param index the index of the minimum value in the vector.
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const T * v, unsigned int size, unsigned int & index)
{
T min = 0;
index = 0;
if(!v || size == 0)
{
return min;
}
min = v[0];
for(unsigned int i=1; i<size; ++i)
{
if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
{
min = v[i];
index = i;
}
}
return min;
}
/**
* Get the minimum of a vector.
* @param v the array
* @param index the index of the minimum value in the vector.
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const std::vector<T> & v, unsigned int & index)
{
return uMin(v.data(), v.size(), index);
}
/**
* Get the minimum of a vector.
* @param v the array
* @param size the size of the array
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const T * v, unsigned int size)
{
unsigned int index;
return uMin(v, size, index);
}
/**
* Get the minimum of a vector.
* @param v the array
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const std::vector<T> & v)
{
return uMin(v.data(), v.size());
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param size the size of the array
* @param min reference to output minimum
* @param max reference to output maximum
* @param min reference to output minimum index
* @param max reference to output maximum index
*/
template<class T>
inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
{
min = 0;
max = 0;
indexMin = 0;
indexMax = 0;
if(!v || size == 0)
{
return;
}
min = v[0];
max = v[0];
for(unsigned int i=1; i<size; ++i)
{
if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
{
min = v[i];
indexMin = i;
}
if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
{
max = v[i];
indexMax = i;
}
}
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param min reference to output minimum
* @param max reference to output maximum
* @param min reference to output minimum index
* @param max reference to output maximum index
*/
template<class T>
inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
{
uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param size the size of the array
* @param min reference to output minimum
* @param max reference to output maximum
*/
template<class T>
inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
{
unsigned int indexMin;
unsigned int indexMax;
uMinMax(v, size, min, max, indexMin, indexMax);
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param min reference to output minimum
* @param max reference to output maximum
*/
template<class T>
inline void uMinMax(const std::vector<T> & v, T & min, T & max)
{
uMinMax(v.data(), v.size(), min, max);
}
/**
* Get the sign of value.
* @param v the value
* @return -1 if v<0, otherwise 1
*/
template<class T>
inline int uSign(const T & v)
{
if(v < 0)
{
return -1;
}
else
{
return 1;
}
}
/**
* Get the sum of all values contained in a list. Provided for convenience.
* @param list the list
* @return the sum of values of the list
*/
template<class T>
inline T uSum(const std::list<T> & list)
{
T sum = 0;
for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
{
sum += *i;
}
return sum;
}
/**
* Get the sum of all values contained in an array: sum(x).
* @param v the array
* @param size the size of the array
* @return the sum of values of the array
*/
template<class T>
inline T uSum(const T * v, unsigned int size)
{
T sum = 0;
if(v && size)
{
for(unsigned int i=0; i<size; ++i)
{
sum += v[i];
}
}
return sum;
}
/**
* Get the sum of all values contained in a vector. Provided for convenience.
* @param v the vector
* @return the sum of values of the vector
*/
template<class T>
inline T uSum(const std::vector<T> & v)
{
return uSum(v.data(), (int)v.size());
}
/**
* Get the sum of all squared values contained in an array: sum(x.^2).
* @param v the array
* @param size the size of the array
* @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
* @return the sum of values of the array
*/
template<class T>
inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
{
T sum = 0;
if(v && size)
{
for(unsigned int i=0; i<size; ++i)
{
sum += (v[i]-subtract)*(v[i]-subtract);
}
}
return sum;
}
/**
* Get the sum of all squared values contained in an array: sum(x.^2).
* @param v the array
* @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
* @return the sum of values of the array
*/
template<class T>
inline T uSumSquared(const std::vector<T> & v, T subtract = T())
{
return uSumSquared(v.data(), v.size(), subtract);
}
/**
* Compute the mean of an array.
* @param v the array
* @param size the size of the array
* @return the mean
*/
template<class T>
inline T uMean(const T * v, unsigned int size)
{
T buf = 0;
if(v && size)
{
for(unsigned int i=0; i<size; ++i)
{
buf += v[i];
}
buf /= size;
}
return buf;
}
/**
* Get the mean of a list. Provided for convenience.
* @param list the list
* @return the mean
*/
template<class T>
inline T uMean(const std::list<T> & list)
{
T m = 0;
if(list.size())
{
for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
{
m += *i;
}
m /= list.size();
}
return m;
}
/**
* Get the mean of a vector. Provided for convenience.
* @param v the vector
* @return the mean
*/
template<class T>
inline T uMean(const std::vector<T> & v)
{
return uMean(v.data(), v.size());
}
/**
* Compute mean squared error between two arrays: mean((x-y).^2).
* @param x the first array
* @param sizeX the size of the array x
* @param y the second array
* @param sizeY the size of the array y (must be same size as x)
* @return the mean squared error (return -1 if error cannot be computed)
*/
template<class T>
inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
{
T sum = 0;
if(x && y && sizeX == sizeY)
{
for(unsigned int i=0; i<sizeX; ++i)
{
T diff = x[i]-y[i];
sum += diff*diff;
}
return sum/(T)sizeX;
}
return (T)-1;
}
/**
* Compute mean squared error between two arrays: mean((x-y).^2).
* @param x the first array
* @param y the second array (must be same size as x)
* @return the mean squared error (return -1 if error cannot be computed)
*/
template<class T>
inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
{
return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
}
/**
* Compute the variance of an array.
* @param v the array
* @param size the size of the array
* @param meanV the mean of the array
* @return the variance
* @see mean()
*/
template<class T>
inline T uVariance(const T * v, unsigned int size, T meanV)
{
T buf = 0;
if(v && size>1)
{
float sum = 0;
for(unsigned int i=0; i<size; ++i)
{
sum += (v[i]-meanV)*(v[i]-meanV);
}
buf = sum/(size-1);
}
return buf;
}
/**
* Get the variance of a list. Provided for convenience.
* @param list the list
* @param m the mean of the list
* @return the variance
* @see mean()
*/
template<class T>
inline T uVariance(const std::list<T> & list, const T & m)
{
T buf = 0;
if(list.size()>1)
{
float sum = 0;
for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
{
sum += (*i-m)*(*i-m);
}
buf = sum/(list.size()-1);
}
return buf;
}
/**
* Compute the variance of an array.
* @param v the array
* @param size the size of the array
* @return the variance
*/
template<class T>
inline T uVariance(const T * v, unsigned int size)
{
T m = uMean(v, size);
return uVariance(v, size, m);
}
/**
* Get the variance of a vector. Provided for convenience.
* @param v the vector
* @param m the mean of the vector
* @return the variance
* @see mean()
*/
template<class T>
inline T uVariance(const std::vector<T> & v, const T & m)
{
return uVariance(v.data(), v.size(), m);
}
/**
* Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3 + ...
* @return the squared norm of the vector
*/
template<class T>
inline T uNormSquared(const std::vector<T> & v)
{
float sum = 0.0f;
for(unsigned int i=0; i<v.size(); ++i)
{
sum += v[i]*v[i];
}
return sum;
}
/**
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
* @return the norm of the vector
*/
template<class T>
inline T uNorm(const std::vector<T> & v)
{
return std::sqrt(uNormSquared(v));
}
/**
* Get the squared norm of the vector : return x1*x1 + x2*x2
* @return the squared norm of the vector
*/
template<class T>
inline T uNormSquared(const T & x1, const T & x2)
{
return x1*x1 + x2*x2;
}
/**
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
* @return the norm of the vector
*/
template<class T>
inline T uNorm(const T & x1, const T & x2)
{
return std::sqrt(uNormSquared(x1, x2));
}
/**
* Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3
* @return the squared norm of the vector
*/
template<class T>
inline T uNormSquared(const T & x1, const T & x2, const T & x3)
{
return x1*x1 + x2*x2 + x3*x3;
}
/**
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
* @return the norm of the vector
*/
template<class T>
inline T uNorm(const T & x1, const T & x2, const T & x3)
{
return std::sqrt(uNormSquared(x1, x2, x3));
}
/**
* Normalize the vector : [x1 x2 x3 ...] ./ uNorm([x1 x2 x3 ...])
* @return the vector normalized
*/
template<class T>
inline std::vector<T> uNormalize(const std::vector<T> & v)
{
float norm = uNorm(v);
if(norm == 0)
{
return v;
}
else
{
std::vector<T> r(v.size());
for(unsigned int i=0; i<v.size(); ++i)
{
r[i] = v[i]/norm;
}
return r;
}
}
/**
* Find all local maxima.
*/
template<class T>
inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
{
std::list<unsigned int> maxima;
if(size)
{
for(unsigned int i=0; i<size; ++i)
{
if(i == 0)
{
// first item
if((i+1 < size && v[i] > v[i+1]) ||
i+1 >= size)
{
maxima.push_back(i);
}
}
else if(i == size - 1)
{
//last item
if((i >= 1 && v[i] > v[i-1]) ||
i == 0)
{
maxima.push_back(i);
}
}
else
{
//all others, check previous and next
if(v[i] > v[i-1] && v[i] > v[i+1])
{
maxima.push_back(i);
}
}
}
}
return maxima;
}
/**
* Find all local maxima.
*/
template<class T>
inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
{
return uLocalMaxima(v.data(), v.size());
}
/**
* Enum of cross matching methods (cross-correlation, cross-covariance) :
* UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff.
*/
enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};
/**
* Do a full cross-correlation or cross-covariance between 2 arrays.
* @param vA the first array
* @param vB the second array
* @param sizeA the size of the first array
* @param sizeB the size of the second array
* @param method see UXMatchMethod
* @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
*/
template<class T>
inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
{
if(!vA || !vB || sizeA == 0 || sizeB == 0)
{
return std::vector<T>();
}
std::vector<T> result(sizeA + sizeB - 1);
T meanA = 0;
T meanB = 0;
if(method > UXCorrCoeff)
{
meanA = uMean(vA, sizeA);
meanB = uMean(vB, sizeB);
}
T den = 1;
if(method == UXCorrCoeff || method == UXCovCoeff)
{
den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
}
else if(method == UXCorrBiased || method == UXCovBiased)
{
den = (T)std::max(sizeA, sizeB);
}
if(sizeA == sizeB)
{
T resultA;
T resultB;
int posA;
int posB;
unsigned int j;
// Optimization, filling two results at once
for(unsigned int i=0; i<sizeA; ++i)
{
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
den = 0;
}
posA = sizeA - i - 1;
posB = sizeB - i - 1;
resultA = 0;
resultB = 0;
for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
{
resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
result[i] = resultA / den;
result[result.size()-1 -i] = resultB / den;
}
}
else
{
for(unsigned int i=0; i<result.size(); ++i)
{
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
den = 0;
}
int posB = sizeB - i - 1;
T r = 0;
if(posB >= 0)
{
for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
{
r += (vA[j] - meanA) * (vB[j + posB] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
else
{
int posA = posB*-1;
for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
{
r += (vA[i+posA] - meanA) * (vB[i] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
result[i] = r / den;
}
}
return result;
}
/**
* Do a full cross-correlation or cross-covariance between 2 arrays.
* @param vA the first array
* @param vB the second array
* @param method see UXMatchMethod
* @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
*/
template<class T>
inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
{
return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
}
/**
* Do a cross correlation between 2 arrays at a specified index.
* @param vA the first array
* @param vB the second array
* @param sizeA the size of the first array
* @param sizeB the size of the second array
* @param index the index to correlate
* @param method see UXMatchMethod
* @return the resulting correlation value
*/
template<class T>
inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
{
T result = 0;
if(!vA || !vB || sizeA == 0 || sizeB == 0)
{
return result;
}
T meanA = 0;
T meanB = 0;
if(method > UXCorrCoeff)
{
meanA = uMean(vA, sizeA);
meanB = uMean(vB, sizeB);
}
unsigned int size = sizeA + sizeB - 1;
T den = 1;
if(method == UXCorrCoeff || method == UXCovCoeff)
{
den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
}
else if(method == UXCorrBiased || method == UXCovBiased)
{
den = (T)std::max(sizeA, sizeB);
}
else if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
den = 0;
}
if(index < size)
{
int posB = sizeB - index - 1;
unsigned int i;
if(posB >= 0)
{
for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
{
result += (vA[i] - meanA) * (vB[i + posB] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
else
{
int posA = posB*-1;
for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
{
result += (vA[i+posA] - meanA) * (vB[i] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
}
return result / den;
}
/**
* Do a cross correlation between 2 arrays at a specified index.
* @param vA the first array
* @param vB the second array
* @param sizeA the size of the first array
* @param sizeB the size of the second array
* @param index the index to correlate
* @param method see UXMatchMethod
* @return the resulting correlation value
*/
template<class T>
inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
{
return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
}
/**
* Return Hamming window of length L.
* @param L the window length
* @return the Hamming window (values are between 0 and 1)
*/
inline std::vector<float> uHamming(unsigned int L)
{
std::vector<float> w(L);
unsigned int N = L-1;
float pi = 3.14159265f;
for(unsigned int n=0; n<N; ++n)
{
w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
}
return w;
}
template <typename T>
bool uIsInBounds(const T& value, const T& low, const T& high)
{
return uIsFinite(value) && !(value < low) && !(value >= high);
}
#endif // UMATH_H
rtabmap/utilite/include/rtabmap/utilite/UMutex.h
+179
View File
@@ -0,0 +1,179 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UMUTEX_H
#define UMUTEX_H
#include <errno.h>
#ifdef _WIN32
#include "rtabmap/utilite/Win32/UWin32.h"
#else
#include <pthread.h>
#endif
/**
* A mutex class.
*
* On a lock() call, the calling thread is blocked if the
* UMutex was previously locked by another thread. It is unblocked when unlock() is called.
*
* On Unix (not yet tested on Windows), UMutex is recursive: the same thread can
* call multiple times lock() without being blocked.
*
* Example:
* @code
* UMutex m; // Mutex shared with another thread(s).
* ...
* m.lock();
* // Data is protected here from the second thread
* //(assuming the second one protects also with the same mutex the same data).
* m.unlock();
*
* @endcode
*
* @see USemaphore
*/
class UMutex
{
public:
/**
* The constructor.
*/
UMutex()
{
#ifdef _WIN32
InitializeCriticalSection(&C);
#else
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr,PTHREAD_MUTEX_RECURSIVE);
pthread_mutex_init(&M,&attr);
pthread_mutexattr_destroy(&attr);
#endif
}
virtual ~UMutex()
{
#ifdef _WIN32
DeleteCriticalSection(&C);
#else
pthread_mutex_unlock(&M); pthread_mutex_destroy(&M);
#endif
}
/**
* Lock the mutex.
*/
int lock() const
{
#ifdef _WIN32
EnterCriticalSection(&C); return 0;
#else
return pthread_mutex_lock(&M);
#endif
}
#ifdef _WIN32
#if(_WIN32_WINNT >= 0x0400)
int lockTry() const
{
return (TryEnterCriticalSection(&C)?0:EBUSY);
}
#endif
#else
int lockTry() const
{
return pthread_mutex_trylock(&M);
}
#endif
/**
* Unlock the mutex.
*/
int unlock() const
{
#ifdef _WIN32
LeaveCriticalSection(&C); return 0;
#else
return pthread_mutex_unlock(&M);
#endif
}
private:
#ifdef _WIN32
mutable CRITICAL_SECTION C;
#else
mutable pthread_mutex_t M;
#endif
void operator=(UMutex &) {}
UMutex( const UMutex & ) {}
};
/**
* Automatically lock the referenced mutex on constructor and unlock mutex on destructor.
*
* Example:
* @code
* UMutex m; // Mutex shared with another thread(s).
* ...
* int myMethod()
* {
* UScopeMutex sm(m); // automatically lock the mutex m
* if(cond1)
* {
* return 1; // automatically unlock the mutex m
* }
* else if(cond2)
* {
* return 2; // automatically unlock the mutex m
* }
* return 0; // automatically unlock the mutex m
* }
*
* @endcode
*
* @see UMutex
*/
class UScopeMutex
{
public:
UScopeMutex(const UMutex & mutex) :
mutex_(mutex)
{
mutex_.lock();
}
// backward compatibility
UScopeMutex(UMutex * mutex) :
mutex_(*mutex)
{
mutex_.lock();
}
~UScopeMutex()
{
mutex_.unlock();
}
private:
const UMutex & mutex_;
};
#endif // UMUTEX_H
rtabmap/utilite/include/rtabmap/utilite/UObjDeletionThread.h
+129
View File
@@ -0,0 +1,129 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UOBJDELETIONTHREAD_H
#define UOBJDELETIONTHREAD_H
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UEventsManager.h"
/**
* Event used by UObjDeletionThread to notify when its object is deleted. It contains
* the object id used for deletion (can be retrieved by UEvent::getCode()).
*/
class UObjDeletedEvent : public UEvent
{
public:
UObjDeletedEvent(int objDeletionThreadId) : UEvent(objDeletionThreadId) {}
virtual ~UObjDeletedEvent() {}
/**
* @return string "UObjDeletedEvent"
*/
virtual std::string getClassName() const {return std::string("UObjDeletedEvent");}
};
/**
* This class can be used to delete a dynamically created object in another thread. Give the
* dynamic reference to object to it and it will notify with a UObjDeletedEvent when the object is deleted.
* The deletion can be delayed on startDeletion(), the thread will wait the time given before deleting the object.
*/
template<class T>
class UObjDeletionThread : public UThread
{
public:
/**
* The constructor.
* @param obj the object to delete
* @param id the custom id which will be sent in a event UObjDeletedEvent after the object is deleted
*/
UObjDeletionThread(T * obj, int id=0) :
obj_(obj),
id_(id),
waitMs_(0) {}
/**
* The destructor. If this thread is not started but with an object set, the
* object is deleted. If the thread has not finished to delete the object, the
* calling thread will wait (on a UThreadNode::join()) until the object is deleted.
* @param obj the object to delete
* @param id the custom id which will be sent in a event UObjDeletedEvent after the object is deleted
*/
virtual ~UObjDeletionThread()
{
join(true);
if(obj_)
{
delete obj_;
}
}
/**
* Start the thread after optional delay.
* @param waitMs the delay before deletion
*/
void startDeletion(int waitMs = 0) {waitMs_ = waitMs; this->start();}
/**
* Get id of the deleted object.
* @return the id
*/
int id() const {return id_;}
/**
* Set a new object, if one was already set, the old one is deleted.
* @param obj the object to delete
*/
void setObj(T * obj)
{
join();
if(obj_)
{
delete obj_;
obj_ = 0;
}
obj_ = obj;
}
private:
/**
* Thread main loop...
*/
virtual void mainLoop()
{
if(waitMs_)
{
uSleep(waitMs_);
}
if(obj_)
{
delete obj_;
obj_ = 0;
}
this->kill();
UEventsManager::post(new UObjDeletedEvent(id_), false);
}
private:
T * obj_;
int id_;
int waitMs_;
};
#endif /* UOBJDELETIONTHREAD_H */
rtabmap/utilite/include/rtabmap/utilite/UProcessInfo.h
+41
View File
@@ -0,0 +1,41 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UPROCESSINFO_H
#define UPROCESSINFO_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
/**
* This class is used to get some informations
* about the current process.
*/
class UTILITE_EXPORT UProcessInfo {
public:
UProcessInfo();
virtual ~UProcessInfo();
/**
* Get the memory used by the current process.
* @return the number of bytes used by the current process.
*/
static long int getMemoryUsage();
};
#endif /* UPROCESSINFO_H */
rtabmap/utilite/include/rtabmap/utilite/USemaphore.h
+223
View File
@@ -0,0 +1,223 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Originally written by Phillip Sitbon
* Copyright 2003
*/
#ifndef USEMAPHORE_H
#define USEMAPHORE_H
#include <errno.h>
#ifdef _WIN32
#include "rtabmap/utilite/Win32/UWin32.h"
#define SEM_VALUE_MAX ((int) ((~0u) >> 1))
#else
#include <pthread.h>
#include <sys/time.h>
#endif
/**
* A semaphore class.
*
* On an acquire() call, the calling thread is blocked if the
* USemaphore's value is <= 0. It is unblocked when release() is called.
* The function acquire() decreases by 1 (default) the
* semaphore's value and release() increases it by 1 (default).
*
* Example:
* @code
* USemaphore s;
* s.acquire(); // Will wait until s.release() is called by another thread.
* @endcode
*
* @see UMutex
*/
class USemaphore
{
public:
/**
* The constructor. The semaphore waits on acquire() when its value is <= 0.
* @param n number to initialize
*/
USemaphore( int initValue = 0 )
{
#ifdef _WIN32
S = CreateSemaphore(0,initValue,SEM_VALUE_MAX,0);
#else
_available = initValue;
pthread_mutex_init(&_waitMutex, NULL);
pthread_cond_init(&_cond, NULL);
#endif
}
virtual ~USemaphore()
{
#ifdef _WIN32
CloseHandle(S);
#else
pthread_cond_destroy(&_cond);
pthread_mutex_destroy(&_waitMutex);
#endif
}
/**
* Acquire the semaphore. If semaphore's value is <=0, the
* calling thread will wait until the count acquired is released.
* @see release()
* @param n number to acquire
* @param t time to wait (ms), a value <=0 means infinite
* @return true on success, false on error/timeout
*/
#ifdef _WIN32
bool acquire(int n = 1, int ms = 0) const
{
int rt = 0;
while(n-- > 0 && rt==0)
{
rt = WaitForSingleObject((HANDLE)S, ms<=0?INFINITE:ms);
}
return rt == 0;
}
#else
bool acquire(int n = 1, int ms = 0)
{
int rt = 0;
pthread_mutex_lock(&_waitMutex);
while (n > _available && rt == 0)
{
if(ms > 0)
{
struct timespec timeToWait;
struct timeval now;
gettimeofday(&now,NULL);
timeToWait.tv_sec = now.tv_sec + ms/1000;
timeToWait.tv_nsec = (now.tv_usec+1000UL*(ms%1000))*1000UL;
rt = pthread_cond_timedwait(&_cond, &_waitMutex, &timeToWait);
}
else
{
rt = pthread_cond_wait(&_cond, &_waitMutex);
}
}
if(rt == 0)
{
// only remove them if waiting did not fail
_available -= n;
}
pthread_mutex_unlock(&_waitMutex);
return rt == 0;
}
#endif
/*
* Try to acquire the semaphore, not a blocking call.
* @return false if the semaphore can't be taken without waiting (value <= 0), true otherwise
*/
#ifdef _WIN32
int acquireTry() const
{
return ((WaitForSingleObject((HANDLE)S,INFINITE)==WAIT_OBJECT_0)?0:EAGAIN);
}
#else
int acquireTry(int n)
{
pthread_mutex_lock(&_waitMutex);
if(n > _available)
{
pthread_mutex_unlock(&_waitMutex);
return false;
}
_available -= n;
pthread_mutex_unlock(&_waitMutex);
return true;
}
#endif
/**
* Release the semaphore, increasing its value by 1 and
* signaling waiting threads (which called acquire()).
*/
#ifdef _WIN32
int release(int n = 1) const
{
return (ReleaseSemaphore((HANDLE)S,n,0)?0:ERANGE);
}
#else
void release(int n = 1)
{
pthread_mutex_lock(&_waitMutex);
_available += n;
pthread_cond_broadcast(&_cond);
pthread_mutex_unlock(&_waitMutex);
}
#endif
/**
* Get the USempahore's value.
* @return the semaphore's value
*/
#ifdef _WIN32
int value() const
{
LONG V = -1; ReleaseSemaphore((HANDLE)S,0,&V); return V;
}
#else
int value()
{
int value = 0;
pthread_mutex_lock(&_waitMutex);
value = _available;
pthread_mutex_unlock(&_waitMutex);
return value;
}
#endif
#ifdef _WIN32
/*
* Reset the semaphore count.
* @param init the initial value
* TODO implement on posix ?
*/
void reset( int init = 0 )
{
CloseHandle(S);
S = CreateSemaphore(0,init,SEM_VALUE_MAX,0);
}
#endif
private:
void operator=(const USemaphore &){}
#ifdef _WIN32
USemaphore(const USemaphore &S){}
HANDLE S;
#else
USemaphore(const USemaphore &):_available(0){}
pthread_mutex_t _waitMutex;
pthread_cond_t _cond;
int _available;
#endif
};
#endif // USEMAPHORE_H
rtabmap/utilite/include/rtabmap/utilite/UStl.h
+822
View File
@@ -0,0 +1,822 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef USTL_H
#define USTL_H
#include <list>
#include <map>
#include <set>
#include <vector>
#include <string>
#include <algorithm>
#include <stdlib.h>
/**
* \file UStl.h
* \brief Wrappers of STL for convenient functions.
*
* All functions you will find here are here
* for the use of STL in a more convenient way.
*/
/**
* Get unique keys from a std::multimap.
* @param mm the multimap
* @return the list which contains unique keys
*/
template<class K, class V>
inline std::list<K> uUniqueKeys(const std::multimap<K, V> & mm)
{
std::list<K> l;
typename std::list<K>::reverse_iterator lastValue;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
if(iter == mm.begin() || (iter != mm.begin() && *lastValue != iter->first))
{
l.push_back(iter->first);
lastValue = l.rbegin();
}
}
return l;
}
/**
* Get all keys from a std::multimap.
* @param mm the multimap
* @return the vector which contains all keys (may contains duplicated keys)
*/
template<class K, class V>
inline std::vector<K> uKeys(const std::multimap<K, V> & mm)
{
std::vector<K> v(mm.size());
int i=0;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
v[i++] = iter->first;
}
return v;
}
/**
* Get all keys from a std::multimap.
* @param mm the multimap
* @return the list which contains all keys (may contains duplicated keys)
*/
template<class K, class V>
inline std::list<K> uKeysList(const std::multimap<K, V> & mm)
{
std::list<K> l;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
l.push_back(iter->first);
}
return l;
}
/**
* Get all values from a std::multimap.
* @param mm the multimap
* @return the vector which contains all values (contains values from duplicated keys)
*/
template<class K, class V>
inline std::vector<V> uValues(const std::multimap<K, V> & mm)
{
std::vector<V> v(mm.size());
int i=0;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
v[i++] = iter->second;
}
return v;
}
/**
* Get all values from a std::multimap.
* @param mm the multimap
* @return the list which contains all values (contains values from duplicated keys)
*/
template<class K, class V>
inline std::list<V> uValuesList(const std::multimap<K, V> & mm)
{
std::list<V> l;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
l.push_back(iter->second);
}
return l;
}
/**
* Get values for a specified key from a std::multimap.
* @param mm the multimap
* @param key the key
* @return the list which contains the values of the key
*/
template<class K, class V>
inline std::list<V> uValues(const std::multimap<K, V> & mm, const K & key)
{
std::list<V> l;
std::pair<typename std::multimap<K, V>::const_iterator, typename std::multimap<K, V>::const_iterator> range;
range = mm.equal_range(key);
for(typename std::multimap<K, V>::const_iterator iter = range.first; iter!=range.second; ++iter)
{
l.push_back(iter->second);
}
return l;
}
/**
* Get all keys from a std::map.
* @param m the map
* @return the vector of keys
*/
template<class K, class V>
inline std::vector<K> uKeys(const std::map<K, V> & m)
{
std::vector<K> v(m.size());
int i=0;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
v[i] = iter->first;
++i;
}
return v;
}
/**
* Get all keys from a std::map.
* @param m the map
* @return the list of keys
*/
template<class K, class V>
inline std::list<K> uKeysList(const std::map<K, V> & m)
{
std::list<K> l;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
l.push_back(iter->first);
}
return l;
}
/**
* Get all keys from a std::map.
* @param m the map
* @return the set of keys
*/
template<class K, class V>
inline std::set<K> uKeysSet(const std::map<K, V> & m)
{
std::set<K> s;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
s.insert(s.end(), iter->first);
}
return s;
}
/**
* Get all values from a std::map.
* @param m the map
* @return the vector of values
*/
template<class K, class V>
inline std::vector<V> uValues(const std::map<K, V> & m)
{
std::vector<V> v(m.size());
int i=0;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
v[i] = iter->second;
++i;
}
return v;
}
/**
* Get all values from a std::map.
* @param m the map
* @return the list of values
*/
template<class K, class V>
inline std::list<V> uValuesList(const std::map<K, V> & m)
{
std::list<V> l;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
l.push_back(iter->second);
}
return l;
}
/**
* Get the value of a specified key from a std::map.
* @param m the map
* @param key the key
* @param defaultValue the default value used if the key is not found
* @return the value
*/
template<class K, class V>
inline V uValue(const std::map<K, V> & m, const K & key, const V & defaultValue = V())
{
V v = defaultValue;
typename std::map<K, V>::const_iterator i = m.find(key);
if(i != m.end())
{
v = i->second;
}
return v;
}
/**
* Get the value of a specified key from a std::map. This will
* remove the value from the map;
* @param m the map
* @param key the key
* @param defaultValue the default value used if the key is not found
* @return the value
*/
template<class K, class V>
inline V uTake(std::map<K, V> & m, const K & key, const V & defaultValue = V())
{
V v;
typename std::map<K, V>::iterator i = m.find(key);
if(i != m.end())
{
v = i->second;
m.erase(i);
}
else
{
v = defaultValue;
}
return v;
}
/**
* Get the iterator at a specified position in a std::list. If the position
* is out of range, the result is the end iterator of the list.
* @param list the list
* @param pos the index position in the list
* @return the iterator at the specified index
*/
template<class V>
inline typename std::list<V>::iterator uIteratorAt(std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::iterator iter = list.begin();
for(unsigned int i = 0; i<pos && iter != list.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::list. If the position
* is out of range, the result is the end iterator of the list.
* @param list the list
* @param pos the index position in the list
* @return the iterator at the specified index
*/
template<class V>
inline typename std::list<V>::const_iterator uIteratorAt(const std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::const_iterator iter = list.begin();
for(unsigned int i = 0; i<pos && iter != list.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::set. If the position
* is out of range, the result is the end iterator of the set.
* @param set the set
* @param pos the index position in the set
* @return the iterator at the specified index
*/
template<class V>
inline typename std::set<V>::iterator uIteratorAt(std::set<V> & set, const unsigned int & pos)
{
typename std::set<V>::iterator iter = set.begin();
for(unsigned int i = 0; i<pos && iter != set.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::set. If the position
* is out of range, the result is the end iterator of the set.
* @param set the set
* @param pos the index position in the set
* @return the iterator at the specified index
*/
template<class V>
inline typename std::set<V>::const_iterator uIteratorAt(const std::set<V> & set, const unsigned int & pos)
{
typename std::set<V>::const_iterator iter = set.begin();
for(unsigned int i = 0; i<pos && iter != set.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::vector. If the position
* is out of range, the result is the end iterator of the vector.
* @param v the vector
* @param pos the index position in the vector
* @return the iterator at the specified index
*/
template<class V>
inline typename std::vector<V>::iterator uIteratorAt(std::vector<V> & v, const unsigned int & pos)
{
return v.begin() + pos;
}
/**
* Get the iterator at a specified position in a std::vector. If the position
* is out of range, the result is the end iterator of the vector.
* @param v the vector
* @param pos the index position in the vector
* @return the iterator at the specified index
*/
template<class V>
inline typename std::vector<V>::const_iterator uIteratorAt(const std::vector<V> & v, const unsigned int & pos)
{
return v.begin() + pos;
}
/**
* Get the value at a specified position in a std::list. If the position
* is out of range, the result is undefined.
* @param list the list
* @param pos the index position in the list
* @return the value at the specified index
*/
template<class V>
inline V & uValueAt(std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::iterator iter = uIteratorAt(list, pos);
return *iter;
}
/**
* Get the value at a specified position in a std::list. If the position
* is out of range, the result is undefined.
* @param list the list
* @param pos the index position in the list
* @return the value at the specified index
*/
template<class V>
inline const V & uValueAt(const std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::const_iterator iter = uIteratorAt(list, pos);
return *iter;
}
/**
* Check if the list contains the specified value.
* @param list the list
* @param value the value
* @return true if the value is found in the list, otherwise false
*/
template<class V>
inline bool uContains(const std::list<V> & list, const V & value)
{
return std::find(list.begin(), list.end(), value) != list.end();
}
/**
* Check if the map contains the specified key.
* @param map the map
* @param key the key
* @return true if the value is found in the map, otherwise false
*/
template<class K, class V>
inline bool uContains(const std::map<K, V> & map, const K & key)
{
return map.find(key) != map.end();
}
/**
* Check if the multimap contains the specified key.
* @param map the map
* @param key the key
* @return true if the value is found in the map, otherwise false
*/
template<class K, class V>
inline bool uContains(const std::multimap<K, V> & map, const K & key)
{
return map.find(key) != map.end();
}
/**
* Insert an item in the map. Contrary to the insert in the STL,
* if the key already exists, the value will be replaced by the new one.
*/
template<class K, class V>
inline void uInsert(std::map<K, V> & map, const std::pair<K, V> & pair)
{
std::pair<typename std::map<K, V>::iterator, bool> inserted = map.insert(pair);
if(inserted.second == false)
{
inserted.first->second = pair.second;
}
}
/**
* Insert items in the map. Contrary to the insert in the STL,
* if the key already exists, the value will be replaced by the new one.
*/
template<class K, class V>
inline void uInsert(std::map<K, V> & map, const std::map<K, V> & items)
{
for(typename std::map<K, V>::const_iterator iter=items.begin(); iter!=items.end(); ++iter)
{
std::pair<typename std::map<K, V>::iterator, bool> inserted = map.insert(*iter);
if(inserted.second == false)
{
inserted.first->second = iter->second;
}
}
}
/**
* Convert a std::list to a std::vector.
* @param list the list
* @return the vector
*/
template<class V>
inline std::vector<V> uListToVector(const std::list<V> & list)
{
return std::vector<V>(list.begin(), list.end());
}
/**
* Convert a std::vector to a std::list.
* @param v the vector
* @return the list
*/
template<class V>
inline std::list<V> uVectorToList(const std::vector<V> & v)
{
return std::list<V>(v.begin(), v.end());
}
/**
* Convert a std::multimap to a std::map
* @see uMultimapToMapUnique to keep only unique keys
*/
template<class K, class V>
inline std::map<K, V> uMultimapToMap(const std::multimap<K, V> & m)
{
return std::map<K, V>(m.begin(), m.end());
}
/**
* Convert a std::multimap to a std::map, keeping only unique keys!
*/
template<class K, class V>
inline std::map<K, V> uMultimapToMapUnique(const std::multimap<K, V> & m)
{
std::map<K, V> mapOut;
std::list<K> uniqueKeys = uUniqueKeys(m);
for(typename std::list<K>::const_iterator iter = uniqueKeys.begin(); iter!=uniqueKeys.end(); ++iter)
{
if(m.count(*iter) == 1)
{
typename std::multimap<K, V>::const_iterator jter=m.find(*iter);
mapOut.insert(mapOut.end(), std::pair<K,V>(jter->first, jter->second));
}
}
return mapOut;
}
/**
* Append a list to another list.
* @param list the list on which the other list will be appended
* @param newItems the list of items to be appended
*/
template<class V>
inline void uAppend(std::list<V> & list, const std::list<V> & newItems)
{
list.insert(list.end(), newItems.begin(), newItems.end());
}
/**
* Get the index in the list of the specified value. S negative index is returned
* if the value is not found.
* @param list the list
* @param value the value
* @return the index of the value in the list
*/
template<class V>
inline int uIndexOf(const std::vector<V> & list, const V & value)
{
int index=-1;
int i=0;
for(typename std::vector<V>::const_iterator iter = list.begin(); iter!=list.end(); ++iter)
{
if(*iter == value)
{
index = i;
break;
}
++i;
}
return index;
}
/**
* Split a string into multiple string around the specified separator.
* Example:
* @code
* std::list<std::string> v = split("Hello the world!", ' ');
* @endcode
* The list v will contain {"Hello", "the", "world!"}
* @param str the string
* @param separator the separator character
* @return the list of strings
*/
inline std::list<std::string> uSplit(const std::string & str, char separator = ' ')
{
std::list<std::string> v;
std::string buf;
for(unsigned int i=0; i<str.size(); ++i)
{
if(str[i] != separator)
{
buf += str[i];
}
else if(buf.size())
{
v.push_back(buf);
buf = "";
}
}
if(buf.size())
{
v.push_back(buf);
}
return v;
}
/**
* Join multiple strings into one string with optional separator.
* Example:
* @code
* std::list<std::string> v;
* v.push_back("Hello");
* v.push_back("world!");
* std::string joined = split(v, " ");
* @endcode
* The output string is "Hello world!"
* @param strings a list of strings
* @param separator the separator string
* @return the joined string
*/
inline std::string uJoin(const std::list<std::string> & strings, const std::string & separator = "")
{
std::string out;
for(std::list<std::string>::const_iterator iter = strings.begin(); iter!=strings.end(); ++iter)
{
if(iter!=strings.begin() && !separator.empty())
{
out += separator;
}
out+=*iter;
}
return out;
}
/**
* Check if a character is a digit.
* @param c the character
* @return true if the character is a digit (if c >= '0' && c <= '9')
*/
inline bool uIsDigit(const char c)
{
return c >= '0' && c <= '9';
}
/**
* Check if a string is a integer number.
* @param str the string
* @return true if the string is a integer number
*/
inline bool uIsInteger(const std::string & str, bool checkForSign = true)
{
bool isInteger = str.size()!=0;
for(unsigned int i=0; i<str.size() && isInteger; ++i)
{
isInteger = (checkForSign && i==0 && str[i]=='-') || uIsDigit(str[i]);
}
return isInteger;
}
/**
* Check if a string is a number (integer or float).
* @param str the string
* @return true if the string is a number
*/
inline bool uIsNumber(const std::string & str)
{
std::list<std::string> list = uSplit(str, '.');
if(list.size() == 1)
{
return uIsInteger(str);
}
else if(list.size() == 2)
{
return uIsInteger(list.front()) && uIsInteger(list.back(), false);
}
return false;
}
/**
* Split a string into number and character strings.
* Example:
* @code
* std::list<std::string> v = uSplit("Hello 03 my 65 world!");
* @endcode
* The list v will contain {"Hello ", "03", " my ", "65", " world!"}
* @param str the string
* @return the list of strings
*/
inline std::list<std::string> uSplitNumChar(const std::string & str)
{
std::list<std::string> list;
std::string buf;
bool num = false;
for(unsigned int i=0; i<str.size(); ++i)
{
if(uIsDigit(str[i]))
{
if(!num && buf.size())
{
list.push_back(buf);
buf.clear();
}
buf += str[i];
num = true;
}
else
{
if(num)
{
list.push_back(buf);
buf.clear();
}
buf += str[i];
num = false;
}
}
if(buf.size())
{
list.push_back(buf);
}
return list;
}
/**
* Compare two alphanumeric strings. Useful to sort filenames (human-like sorting).
* Example:
* @code
* std::string a = "Image9.jpg";
* std::string b = "Image10.jpg";
* int r = uStrNumCmp(a, b); // r returns -1 (a is smaller than b). In contrast, std::strcmp(a, b) would return 1.
* @endcode
* @param a the first string
* @param b the second string
* @return -1 if a<b, 0 if a=b and 1 if a>b
*/
inline int uStrNumCmp(const std::string & a, const std::string & b)
{
std::vector<std::string> listA;
std::vector<std::string> listB;
listA = uListToVector(uSplitNumChar(a));
listB = uListToVector(uSplitNumChar(b));
unsigned int i;
int result = 0;
for(i=0; i<listA.size() && i<listB.size(); ++i)
{
if(uIsDigit(listA[i].at(0)) && uIsDigit(listB[i].at(0)))
{
//padding if zeros at the beginning
if(listA[i].at(0) == '0' && listB[i].size() < listA[i].size())
{
while(listB[i].size() < listA[i].size())
{
listB[i] += '0';
}
}
else if(listB[i].at(0) == '0' && listA[i].size() < listB[i].size())
{
while(listA[i].size() < listB[i].size())
{
listA[i] += '0';
}
}
if(listB[i].size() < listA[i].size())
{
result = 1;
}
else if(listB[i].size() > listA[i].size())
{
result = -1;
}
else
{
result = listA[i].compare(listB[i]);
}
}
else if(uIsDigit(listA[i].at(0)))
{
result = -1;
}
else if(uIsDigit(listB[i].at(0)))
{
result = 1;
}
else
{
result = listA[i].compare(listB[i]);
}
if(result != 0)
{
break;
}
}
return result;
}
/**
* Check if a string contains a specified substring.
*/
inline bool uStrContains(const std::string & string, const std::string & substring)
{
return string.find(substring) != std::string::npos;
}
inline int uCompareVersion(const std::string & version, int major, int minor=-1, int patch=-1)
{
std::vector<std::string> v = uListToVector(uSplit(version, '.'));
if(v.size() == 3)
{
int vMajor = atoi(v[0].c_str());
int vMinor = atoi(v[1].c_str());
int vPatch = atoi(v[2].c_str());
if(vMajor > major ||
(vMajor == major && minor!=-1 && vMinor > minor) ||
(vMajor == major && minor!=-1 && vMinor == minor && patch!=-1 && vPatch > patch))
{
return 1;
}
else if(vMajor == major && (minor == -1 || (vMinor == minor && (patch == -1 || vPatch == patch))))
{
return 0;
}
}
return -1;
}
inline std::string uPad(const std::string & title, int padding = 20)
{
int emptySize = padding - (int)title.size();
if(emptySize>0)
{
return title + std::string(emptySize, ' ');
}
return title;
}
#endif /* USTL_H */
rtabmap/utilite/include/rtabmap/utilite/UThread.h
+270
View File
@@ -0,0 +1,270 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTHREADNODE_H
#define UTHREADNODE_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UThreadC.h"
/**
* The class UThread is an abstract class for creating thread objects.
* A UThread provides methods to create threads as an object-style fashion.
*
* For most of inherited classes, only mainLoop() needs to be implemented, then only start() needs
* to be called from the outside.
* The main loop is called until the thread itself calls kill() or another thread
* calls kill() or join() (with parameter to true) on this thread. Unlike kill(), join() is a blocking call:
* the calling thread will wait until this thread has finished, thus join() must not be
* called inside the mainLoop().
*
* If inside the mainLoop(), at some time, the thread needs to wait on a mutex/semaphore
* (like for the acquisition of a resource), the function mainLoopKill() should be
* implemented to release the mutex/semaphore when the thread is killed, to avoid a deadlock.
* The function killCleanup() is called after the thread's state is set to kSKilled.
* After the mutex/semaphore is released in killCleanup(), on wake up, the thread can know if
* it needs to stop by calling isKilled().
*
* To do an initialization process (executed by the worker thread) just one time before
* entering the mainLoop(), mainLoopBegin() can be implemented.
*
* Example:
* @code
* #include "utilite/UThread.h"
* class SimpleThread : public UThread
* {
* public:
* SimpleThread() {}
* virtual ~SimpleThread() {
* // The calling thread will wait until this thread has finished.
* this->join(true);
* }
*
* protected:
* virtual void mainLoop() {
* // Do some works...
*
* // This will stop the thread, otherwise the mainLoop() is recalled.
* this->kill();
* }
* };
*
* int main(int argc, char * argv[])
* {
* SimpleThread t;
* t.start();
* t.join(); // Wait until the thread has finished.
* return 0;
* }
* @endcode
*
* @see start()
* @see kill()
* @see join()
* @see mainLoopBegin()
* @see mainLoopKill()
* @see mainLoop()
*
*/
class UTILITE_EXPORT UThread : public UThreadC<void>
{
public:
/**
* Enum of priorities : kPLow, kPBelowNormal, kPNormal, kPAboveNormal, kPRealTime.
*/
enum Priority{kPLow, kPBelowNormal, kPNormal, kPAboveNormal, kPRealTime};
public:
//return caller thread id
static unsigned long currentThreadId() {return (unsigned long)UThreadC<void>::Self();}
public:
/**
* The constructor.
* @see Priority
* @param priority the thread priority
*/
UThread(Priority priority = kPNormal);
/**
* The destructor. Inherited classes must call join(true) inside their destructor
* to avoid memory leaks where the underlying c-thread is still running.
*
* Note: not safe to delete a thread while other threads are joining it.
*/
virtual ~UThread();
/**
* Start the thread. Once the thread is started, subsequent calls
* to start() are ignored until the thread is killed.
* @see kill()
*/
void start();
/**
* Kill the thread.
* This functions does nothing if the thread is not started or is killed.
*
* Note : not a blocking call
*/
void kill();
/**
* The caller thread will wait until the thread has finished.
*
* Note : blocking call
* @param killFirst if you want kill() to be called before joining (default false), otherwise not.
*/
void join(bool killFirst = false);
/**
* Set the thread priority.
* @param priority the priority
*/
void setPriority(Priority priority);
/**
* Set the thread affinity. This is applied during start of the thread.
*
* MAC OS X : http://developer.apple.com/library/mac/#releasenotes/Performance/RN-AffinityAPI/_index.html.
* @param cpu the cpu id (start at 1), 0 means no affinity (default).
*/
void setAffinity(int cpu = 0);
/**
* @return if the state of the thread is kSCreating (after start() is called but before entering the mainLoop()).
*/
bool isCreating() const;
/**
* @return if the state of the thread is kSRunning (it is executing the mainLoop()) or kSCreating.
*/
bool isRunning() const;
/**
* @return if the state of the thread is kSIdle (before start() is called and after the thread is totally killed (or after join(true))).
*/
bool isIdle() const;
/**
* @return if the state of the thread is kSKilled (after kill() is called and before the thread is totally killed).
*/
bool isKilled() const;
Handle getThreadHandle() const {return handle_;}
unsigned long getThreadId() const {return threadId_;}
protected:
private:
/**
* Virtual method mainLoopBegin().
* User can implement this function to add a behavior
* before the main loop is started. It is
* called once (before entering mainLoop()).
*/
virtual void mainLoopBegin() {}
/**
* Pure virtual method mainLoop().
* The inner loop of the thread. This method is called repetitively
* until the thread is killed. Note that if kill() is called in mainLoopBegin(),
* mainLoop() is not called, terminating immediately the thread.
*
* @see mainLoop()
* @see kill()
*/
virtual void mainLoop() = 0;
/**
* Virtual method mainLoopKill().
* User can implement this function to add a behavior
* before the thread is killed. When this
* function is called, the state of the thread is set to kSKilled. It is useful to
* wake up a sleeping thread to finish his loop and to avoid a deadlock.
*/
virtual void mainLoopKill() {}
/**
* Virtual method mainLoopEnd().
* User can implement this function to add a behavior
* after the thread is killed (after exiting the mainLoop(), work is
* still done in the thread before exiting).
*/
virtual void mainLoopEnd() {}
/*
* Inherited method ThreadMain() from Thread.
* @see Thread<void>
*/
void ThreadMain();
/*
* Apply thread priority. This is called when starting the thread.
* *@todo : Support pthread
*/
void applyPriority();
/*
* Apply cpu affinity. This is called when starting the thread.
* *@todo : Support Windows
*/
void applyAffinity();
/*
* Inherited method Create() from Thread.
* Here we force this function to be private so the
* inherited class can't have access to it.
* @see Thread<void>
*/
int Create(
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const;
//Methods from UThread<void> class hided
static int Join( Handle H )
{ return UThreadC<void>::Join(H); }
#ifndef ANDROID
static int Kill( Handle H )
{ return UThreadC<void>::Kill(H); }
#endif
static int Detach( Handle H )
{ return UThreadC<void>::Detach(H); }
private:
void operator=(UThread &) {}
UThread( const UThread &) : state_(kSIdle) {}
private:
enum State{kSIdle, kSCreating, kSRunning, kSKilled}; /* Enum of states. */
State state_; /* The thread state. */
Priority priority_; /* The thread priority. */
Handle handle_; /* The thread handle. */
unsigned long threadId_; /* The thread id. */
int cpuAffinity_; /* The cpu affinity. */
UMutex killSafelyMutex_; /* Mutex used to protect the kill() method. */
UMutex runningMutex_; /* Mutex used to notify the join method when the thread has finished. */
};
#endif // UTHREADNODE_H
rtabmap/utilite/include/rtabmap/utilite/UThreadC.h
+36
View File
@@ -0,0 +1,36 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTHREADC_H
#define UTHREADC_H
#include <errno.h>
/*
* Use of StateThread is safer. The Thread class in his
* base form is not supported.
* @see StateThread
*/
#ifdef _WIN32
#include "rtabmap/utilite/Win32/UThreadC.h"
#else
#include "rtabmap/utilite/Posix/UThreadC.h"
#endif
#endif // UTHREADC_H
rtabmap/utilite/include/rtabmap/utilite/UThreadNode.h
+29
View File
@@ -0,0 +1,29 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTHREADNODE_H_
#define UTHREADNODE_H_
#include "rtabmap/utilite/UThread.h"
//For backward compatibility
typedef UThread UThreadNode;
#endif /* UTHREADNODE_H_ */
rtabmap/utilite/include/rtabmap/utilite/UTimer.h
+109
View File
@@ -0,0 +1,109 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTIMER_H
#define UTIMER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#ifdef _WIN32
#include <windows.h>
#else
#include <sys/time.h>
#include <time.h>
#endif
/**
* This class is used to time some codes (in seconds).
* On Unix, the resolution is up to microseconds (see gettimeofday()).
* On Windows, the performance counter is used (see QueryPerformanceCounter() and QueryPerformanceFrequency()).
* Example:
* @code
* UTimer timer;
* timer.start();
* ... (do some work)
* timer.stop();
* double seconds = timer.getInterval();
* ...
* @endcode
*/
class UTILITE_EXPORT UTimer
{
public:
UTimer();
~UTimer();
/**
* This method is used to get
* the time of the system right now.
* @return double the time in seconds.
*/
static double now();
/**
* This method starts the timer.
*/
void start();
/**
* This method stops the timer.
*/
void stop();
/**
* This method is used to get the elapsed time
* between now and the start(). If timer is stopped, the interval time
* between stop() and the start() is returned.
* @return double the interval in seconds.
*/
double elapsed() {return getElapsedTime();}
double getElapsedTime();
/**
* This method is used to get the interval time
* between stop() and the start().
* @return double the interval in seconds.
* @deprecated use elapsed() instead.
*/
UTILITE_DEPRECATED double getInterval();
/**
* This method is used to get the interval of
* the timer while it is running. It's automatically
* stop the timer, get the interval and restart
* the timer. It's the same of calling stop(),
* elapsed() and start(). Method restart() does the same thing, for convenience.
* @return double the interval in seconds.
*/
double restart() {return ticks();}
double ticks();
private:
#ifdef _WIN32
LARGE_INTEGER startTimeRecorded_; /* When we start the timer, timeRecorded is copied over lastTimeRecorded.*/
LARGE_INTEGER stopTimeRecorded_; /* When we stop the timer. */
LARGE_INTEGER frequency_; /* Keep the frequency of the counter */
#else
struct timeval startTimeRecorded_; /* When we start the timer, timeRecorded is copied over lastTimeRecorded.*/
struct timeval stopTimeRecorded_; /* When we stop the timer. */
#endif
};
#endif //UTIMER_H
rtabmap/utilite/include/rtabmap/utilite/UVariant.h
+124
View File
@@ -0,0 +1,124 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UVARIANT_H
#define UVARIANT_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
#include <vector>
/**
* Experimental class...
*/
class UTILITE_EXPORT UVariant
{
public:
enum Type{
kBool,
kChar,
kUChar,
kShort,
kUShort,
kInt,
kUInt,
kFloat,
kDouble,
kStr,
kCharArray,
kUCharArray,
kShortArray,
kUShortArray,
kIntArray,
kUIntArray,
kFloatArray,
kDoubleArray,
kUndef
};
public:
UVariant();
UVariant(const bool & value);
UVariant(const signed char & value);
UVariant(const unsigned char & value);
UVariant(const short & value);
UVariant(const unsigned short & value);
UVariant(const int & value);
UVariant(const unsigned int & value);
UVariant(const float & value);
UVariant(const double & value);
UVariant(const char * value);
UVariant(const std::string & value);
UVariant(const std::vector<signed char> & value);
UVariant(const std::vector<unsigned char> & value);
UVariant(const std::vector<short> & value);
UVariant(const std::vector<unsigned short> & value);
UVariant(const std::vector<int> & value);
UVariant(const std::vector<unsigned int> & value);
UVariant(const std::vector<float> & value);
UVariant(const std::vector<double> & value);
Type type() const {return type_;}
bool isUndef() const {return type_ == kUndef;}
bool isBool() const {return type_ == kBool;}
bool isChar() const {return type_ == kChar;}
bool isUChar() const {return type_ == kUChar;}
bool isShort() const {return type_ == kShort;}
bool isUShort() const {return type_ == kUShort;}
bool isInt() const {return type_ == kInt;}
bool isUInt() const {return type_ == kUInt;}
bool isFloat() const {return type_ == kFloat;}
bool isDouble() const {return type_ == kDouble;}
bool isStr() const {return type_ == kStr;}
bool isCharArray() const {return type_ == kCharArray;}
bool isUCharArray() const {return type_ == kUCharArray;}
bool isShortArray() const {return type_ == kShortArray;}
bool isUShortArray() const {return type_ == kUShortArray;}
bool isIntArray() const {return type_ == kIntArray;}
bool isUIntArray() const {return type_ == kUIntArray;}
bool isFloatArray() const {return type_ == kFloatArray;}
bool isDoubleArray() const {return type_ == kDoubleArray;}
bool toBool() const;
signed char toChar(bool * ok = 0) const;
unsigned char toUChar(bool * ok = 0) const;
short toShort(bool * ok = 0) const;
unsigned short toUShort(bool * ok = 0) const;
int toInt(bool * ok = 0) const;
unsigned int toUInt(bool * ok = 0) const;
float toFloat(bool * ok = 0) const;
double toDouble(bool * ok = 0) const;
std::string toStr(bool * ok = 0) const;
std::vector<signed char> toCharArray(bool * ok = 0) const;
std::vector<unsigned char> toUCharArray(bool * ok = 0) const;
std::vector<short> toShortArray(bool * ok = 0) const;
std::vector<unsigned short> toUShortArray(bool * ok = 0) const;
std::vector<int> toIntArray(bool * ok = 0) const;
std::vector<unsigned int> toUIntArray(bool * ok = 0) const;
std::vector<float> toFloatArray(bool * ok = 0) const;
std::vector<double> toDoubleArray(bool * ok = 0) const;
virtual ~UVariant() {}
private:
Type type_;
std::vector<unsigned char> data_;
};
#endif /* UVARIANT_H */
rtabmap/utilite/include/rtabmap/utilite/UtiLite.h
+253
View File
@@ -0,0 +1,253 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTILITE_H
#define UTILITE_H
/** \mainpage UtiLite
*
* \section intro Introduction
* <a href="http://utilite.googlecode.com">UtiLite</a> is a simple library to create small cross-platform
* applications using <b>threads</b>, <b>events-based communication</b> and a powerful <b>logger</b>. The first three
* sections show the core classes of the library, then last sections show some useful functions added through time.
*
* UtiLite provides a utility application called \ref uResourceGeneratorPage "uResourceGenerator" to generate resources to include in an executable. For example:
* @code
* $ ./uresourcegenerator DatabaseSchema.sql
* @endcode
* This will generate a HEX file "DatabaseSchema_sql.h" which can be included in source files.
* Data of the file is global and can be accessed by the generated const char * DATABASESCHEMA_SQL.
* @code
* #include "DatabaseSchema_sql.h"
* ...
* std::string hex = DATABASESCHEMA_SQL;
* // Assuming there are only ASCII characters, we can directly convert to a string:
* std::string schema = uHex2Str(hex);
* // For binary data:
* std::vector<char> bytes = uHex2Bytes(hex);
* @endcode
*
* A generated \ref findUtilitePage "FindUtiLite.cmake" is also provided for easy linking with the library.
*
*
* \section logger ULogger
* The ULogger can be used anywhere in the application to log messages (formated like a printf). The
* logger can be set (ULogger::setType()) to print in a file or in the console (with colors depending on the severity). Convenient
* macros are given, working like a printf(), as UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL(), UASSERT(). Small example:
* @code
* ...
* UINFO("A simple message with number %d", 42);
* UDEBUG("A debug message");
* ...
* @endcode
* This will print: [Severity] (Time) File:Line:Function() "The message"
* @code
* [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() A simple message with number 42
* [DEBUG] (2010-09-25 18:08:20) main.cpp:18::main() A debug message
* @endcode
*
* \section eventsmanager UEventsManager, UEventsHandler, UEvent
* The events-based communication framework helps to communicate between objects/threads.
* The UEventsManager is a singleton with which we can post events anywhere in the
* application by calling UEventsManager::post(). All UEventsHandler will then receive the
* event with their protected function UEventsHandler::handleEvent(). Handlers are added to UEventsManager by
* calling UEventsManager::addHandler(). The UEvent provides an abstract class to implement any event
* implementations. The only requirement is that the event must implements UEvent::getClassName() to know the event's type.
* @code
* ...
* MyHandler handler; // MyHandler is an implementation of UEventsHandler
* UEventsManager::addHandler(&handler);
* UEventsManager::post(new MyEvent()); // MyEvent is an implementation of UEvent
* // The UEventsHandler::handleEvent() of "handler" will then be called by the UEventsManager's events dispatching thread.
* ...
* @endcode
* Look at the <b>full example</b> in page of UEventsHandler on how communication works with threads (UThread).
*
* \section thread UThread, UMutex, USemaphore
* The multi-threading framework use a UThread as an abstract class to implement a
* thread in object-style. Reimplement UThread::mainLoop() then call UThread::start() to
* start the main loop of the thread. Threads can be joined by calling UThread::join() and
* killed by calling UThread::kill(). Classes UMutex and USemaphore provide blocking mechanisms to
* protect data between threads.
* @code
* ...
* MyThread t; // MyThread is an implementation of UThread
* t.start();
* t.join(); // Wait the thread to finish
* ...
* @endcode
*
* \section timer UTimer
* A useful class to compute processing time:
* - UTimer::start(),
* - UTimer::stop(),
* - UTimer::restart(),
* - UTimer::elapsed(),
* - UTimer::now().
*
* \section filedir UDirectory, UFile
* For files and directories manipulations :
* - UFile::exists(),
* - UFile::length(),
* - UFile::rename(),
* - UFile::erase(),
* - UDirectory::exists(),
* - UDirectory::getFileNames(),
* - UDirectory::makeDir(),
* - UDirectory::removeDir(),
* - UDirectory::currentDir(),
* - UDirectory::homeDir(),
*
* \section stl Convenient use of STL
* The library provides some simple wrappers of the STL like:
* - uUniqueKeys() to get unique keys from a std::multimap,
* - uKeys() to get all keys of a std::multimap or std::map,
* - uValues() to get all values of a std::multimap or std::map,
* - uValue() to get the value of a key (with a default argument if the key is not found),
* - uTake() to take the value of a key (with a default argument if the key is not found),
* - uIteratorAt() to get iterator at a specified position in a std::list,
* - uValueAt() to get value at a specified position in a std::list,
* - uContains() to know if a key/value exists in a std::multimap, std::map, std::list,
* - uInsert() to insert a value in a std::map and overwriting the value if the key already exists,
* - uListToVector(),
* - uVectorToList(),
* - uAppend() to append a list to another list,
* - uIndexOf() to get index of a value in a std::list,
* - uSplit() to split a string into a std::list of strings on the specified separator.
*
*
* \section math Basic mathematic operations
* A library of basic array manipulations:
* - uMax(),
* - uSign(),
* - uSum(),
* - uMean(),
* - uStdDev(),
* - uNorm(),
* - uNormalize(),
* - uXMatch().
*
* \section conversion Conversion
* A library of convenient functions to convert some data into another like:
* - uReplaceChar(),
* - uToUpperCase(),
* - uToLowerCase(),
* - uNumber2Str(),
* - uBool2Str(),
* - uStr2Bool(),
* - uBytes2Hex(),
* - uHex2Bytes(),
* - uHex2Bytes(),
* - uHex2Str(),
* - uHex2Ascii(),
* - uAscii2Hex(),
* - uFormatv(),
* - uFormat().
*
* \section processinfo UProcessInfo
* This class can be used to get the process memory usage: UProcessInfo::getMemoryUsage().
*
* \section qtLib Qt Widgets (libutilite_qt.so : OPTIONAL)
* If Qt is found on the system, the UtiLite Qt library (libutilite_qt.so, libutilite_qt.dll) with
* useful widgets is built. Use class UPlot to create a plot like MATLAB, and incrementally add
* new values like a scope. USpectrogram is used to
* show audio frequency frames.
* - UPlot,
* - USpectrogram,
* - UImageView.
* @image html UPlot.gif
* @image html USpectrogram.png
*
* \section audioLib Audio stuff (libutilite_audio.so : OPTIONAL)
* If FMOD is found on the system, the UtiLite audio
* library is built (libutilite_audio.so, libutilite_audio.dll). It is a wrapper
* of FMOD methods with a convenient interface to extract audio frames.
* - UAudioCapture,
* - UAudioCaptureFile,
* - UAudioCaptureMic,
* - UAudioCaptureFFT,
* - UAudioPlayer,
* - UAudioPlayerTone,
* - UWav,
* - UMp3Encoder (only if Lame is also found on the system).
*
* \section cvLib OpenCV stuff (libutilite_cv.so : OPTIONAL)
* If OpenCV is found on the system, the UtiLite cv
* library is built (libutilite_cv.so, libutilite_cv.dll). It provides
* image capture classes used to read from a webcam, a video file
* or a directory of images. If UtiLite is also built with Qt, a
* convenient function uCvMat2QImage() can be used to convert a cv::Mat
* image to a QImage.
* - UVideoCapture,
* - UImageFolderCapture,
* - UColorTable,
* - uCvMat2QImage() (only if Qt is also found on the system).
*/
/*! \page uResourceGeneratorPage uResourceGenerator
* UtiLite provides a utility application called \ref uResourceGeneratorPage "uResourceGenerator" to generate resources to include in an executable. For example:
* @code
* $ ./uresourcegenerator DatabaseSchema.sql
* @endcode
* This will generate a HEX file "DatabaseSchema_sql.h" which can be included in source files.
* Data of the file is global and can be accessed by the generated const char * DATABASESCHEMA_SQL.
* @code
* #include "DatabaseSchema_sql.h"
* ...
* std::string hex = DATABASESCHEMA_SQL;
* // Assuming there are only ASCII characters, we can directly convert to a string:
* std::string schema = uHex2Str(hex);
* // For binary data:
* std::vector<char> bytes = uHex2Bytes(hex);
* @endcode
*
* The generator can be automated in a CMake build like:
* @code
* ADD_CUSTOM_COMMAND(
* OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
* COMMAND ${URESOURCEGENERATOR_EXEC} -n my_namespace -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/DatabaseSchema.sql
* COMMENT "[Creating database resource]"
* DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/DatabaseSchema.sql
* )
* SET(RESOURCES
* ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
* )
* ADD_LIBRARY(mylib ${SRC_FILES} ${RESOURCES})
* ADD_EXECUTABLE(myexecutable ${SRC_FILES} ${RESOURCES})
* @endcode
* The variable URESOURCEGENERATOR_EXEC is set when FIND_PACKAGE(UtiLite) is done, you would need to add \ref findUtilitePage "FindUtiLite.cmake".
*/
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UEventsManager.h"
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UProcessInfo.h"
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UVariant.h"
#include "rtabmap/utilite/UMath.h"
#endif /* UTILITE_H */
rtabmap/utilite/include/rtabmap/utilite/Win32/UThreadC.h
+385
View File
@@ -0,0 +1,385 @@
/////////////////////////////////////////////////////////////////////
// Written by Phillip Sitbon
// Copyright 2003
//
// Modified by Mathieu Labbe
//
// Win32/Thread.h
// - Windows thread
//
// - From CreateThread Platform SDK Documentation:
//
// "A thread that uses functions from the static C run-time
// libraries should use the beginthread and endthread C run-time
// functions for thread management rather than CreateThread and
// ExitThread. Failure to do so results in small memory leaks
// when ExitThread is called. Note that this is not a problem
// with the C run-time in a DLL."
//
// With regards to this, I have decided to use the CreateThread
// API, unless you define _CRT_ in which case there are two
// possibilities:
//
// 1. Define _USE_BEGINTHREAD: Uses _beginthread/_endthread
// (said to be *unreliable* in the SDK docs)
//
// 2. Don't - Uses _beginthreaded/_endthreadex
//
// A note about _endthread:
//
// It will call CloseHandle() on exit, and if it was already
// closed then you will get an exception. To prevent this, I
// removed the CloseHandle() functionality - this means that
// a Join() WILL wait on a Detach()'ed thread.
//
/////////////////////////////////////////////////////////////////////
#ifndef _U_Thread_Win32_
#define _U_Thread_Win32_
#include "rtabmap/utilite/utilite_export.h"
#include "rtabmap/utilite/Win32/UWin32.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/utilite/UMutex.h"
inline void uSleep(unsigned int ms)
{
Sleep(ms);
}
#ifdef _CRT_
# include <process.h>
# ifdef _USE_BEGINTHREAD
# define THREAD_CALL __cdecl
# define THREAD_HANDLE uintptr_t
# define THREAD_RET_T void
# define CREATE_THREAD_FAILED (-1L)
# define CREATE_THREAD_ERROR (errno)
# define CREATE_THREAD(_S,_F,_P) ((Handle)_beginthread((void (__cdecl *)(void *))_F,_S,(void *)_P))
# define EXIT_THREAD _endthread()
# define CLOSE_HANDLE(x) 1
# define THREAD_RETURN(x) return
# else
# define THREAD_CALL WINAPI
# define THREAD_HANDLE HANDLE
# define THREAD_RET_T UINT
# define CREATE_THREAD_FAILED (0L)
# define CREATE_THREAD_ERROR (errno)
# define CREATE_THREAD(_S,_F,_P) ((Handle)_beginthreadex(0,_S,(UINT (WINAPI *)(void *))_F,(void *)_P,0,0))
# define EXIT_THREAD _endthreadex(0)
# define CLOSE_HANDLE(x) CloseHandle(x)
# define THREAD_RETURN(x) return(x)
# endif
#else
# define THREAD_CALL WINAPI
# define THREAD_HANDLE HANDLE
# define THREAD_RET_T DWORD
# define CREATE_THREAD_FAILED (0L)
# define CREATE_THREAD_ERROR GetLastError()
# define CREATE_THREAD(_S,_F,_P) ((Handle)CreateThread(0,_S,(DWORD (WINAPI *)(void *))_F,(void *)_P,0,0))
# define CREATE_THREAD2(_S,_F,_P,_ID) ((Handle)CreateThread(0,_S,(DWORD (WINAPI *)(void *))_F,(void *)_P,0,_ID))
# define EXIT_THREAD ExitThread(0)
# define CLOSE_HANDLE(x) CloseHandle(x)
# define THREAD_RETURN(x) return(x)
#endif
#define InvalidHandle 0
template
<
typename Thread_T
>
class UTILITE_EXPORT UThreadC
{
private:
struct Instance;
public:
typedef Thread_T & Thread_R;
typedef const Thread_T & Thread_C_R;
typedef THREAD_HANDLE Handle;
typedef void (* Handler)( Thread_R );
protected:
UThreadC() {}
virtual void ThreadMain( Thread_R ) = 0;
static void Exit()
{ EXIT_THREAD; }
static void TestCancel()
{ Sleep(0); }
static int Self()
{
Handle Hnd = InvalidHandle;
DuplicateHandle(GetCurrentProcess(),GetCurrentThread(),GetCurrentProcess(),(LPHANDLE)&Hnd,NULL,0,NULL);
return Hnd;
// only a pseudo-handle!
//return (Handle)GetCurrentThread();
}
public:
static int Create(
const Handler & Function,
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
)
{
M_Create().lock();
Instance I(Param,0,Function);
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler,&I));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
M_Create().unlock();
return CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
S_Create().Wait();
M_Create().unlock();
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
return 0;
}
int Create(
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const
{
M_Create().lock();
Instance I(Param,const_cast<UThreadC *>(this));
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler,&I));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
M_Create().unlock();
return CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
S_Create().Wait();
M_Create().unlock();
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
return 0;
}
static int Join( const Handle &H )
{
DWORD R = WaitForSingleObject((HANDLE)H,INFINITE);
if ( (R == WAIT_OBJECT_0) || (R == WAIT_ABANDONED) )
{
CLOSE_HANDLE(H);
return 0;
}
if ( R == WAIT_TIMEOUT ) return EAGAIN;
return EINVAL;
}
static int Kill( const Handle &H )
{ return TerminateThread((HANDLE)H,0) ? 0 : EINVAL; }
static int Detach( const Handle &H )
{ return (CLOSE_HANDLE(H)?0:EINVAL); }
private:
static const UMutex &M_Create() { static UMutex M; return M; }
static const USemaphore &S_Create() { static USemaphore S; return S; }
static THREAD_RET_T THREAD_CALL ThreadMainHandler( Instance *Param )
{
Instance I(*Param);
Thread_T Data(I.Data);
S_Create().Post();
if ( I.Owner )
I.Owner->ThreadMain(Data);
else
I.pFN(Data);
Exit();
THREAD_RETURN(0);
}
struct Instance
{
Instance( Thread_C_R P, UThreadC<Thread_T> *const &O, const typename UThreadC<Thread_T>::Handler &pH = 0 )
: pFN(pH), Data(P), Owner(O) {}
typename UThreadC<Thread_T>::Handler pFN;
typename UThreadC<Thread_T>::Thread_C_R Data;
UThreadC<Thread_T> * Owner;
};
};
/////////////////////////////////////////////////////////////////////
// Explicit Specialization of void
//
template<>
class UTILITE_EXPORT UThreadC<void>
{
private:
struct Instance;
public:
typedef THREAD_HANDLE Handle;
typedef void ( *Handler)();
virtual ~UThreadC<void>() {}
protected:
UThreadC<void>() {}
virtual void ThreadMain() = 0;
static void Exit()
{ EXIT_THREAD; }
static void TestCancel()
{ Sleep(0); }
static int Self()
{
return (int)GetCurrentThreadId();
//Handle Hnd = InvalidHandle;
//DuplicateHandle(GetCurrentProcess(),GetCurrentThread(),GetCurrentProcess(),(LPHANDLE)&Hnd,NULL,0,NULL);
//return Hnd;
// only a pseudo-handle!
//return (Handle)GetCurrentThread();
}
public:
static int Create(
const Handler & Function,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
)
{
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler_S,Function));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
return (int)CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
return 0;
}
int Create(
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const
{
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler,this));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
return (int)CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
Self();
return 0;
}
int Create(
unsigned long & ThreadId,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const
{
*H = InvalidHandle;
int id;
*H = CREATE_THREAD2(StackSize,ThreadMainHandler,this, (LPDWORD)&id);
ThreadId = (unsigned long)id;
if ( *H == CREATE_THREAD_FAILED )
{
*H = InvalidHandle;
return (int)CREATE_THREAD_ERROR;
}
if ( CreateDetached ) CLOSE_HANDLE(*H);
return 0;
}
static int Join( const Handle &H )
{
DWORD R = WaitForSingleObject((HANDLE)H,INFINITE);
if ( (R == WAIT_OBJECT_0) || (R == WAIT_ABANDONED) )
{
CLOSE_HANDLE(H);
return 0;
}
if ( R == WAIT_TIMEOUT ) return EAGAIN;
return EINVAL;
}
static int Kill( const Handle &H )
{ return TerminateThread((HANDLE)H,0) ? 0 : EINVAL; }
static int Detach( const Handle &H )
{ return (CLOSE_HANDLE(H)?0:EINVAL); }
private:
static THREAD_RET_T THREAD_CALL ThreadMainHandler( UThreadC<void> *Param )
{
Param->ThreadMain();
Exit();
THREAD_RETURN(0);
}
static THREAD_RET_T THREAD_CALL ThreadMainHandler_S( Handler Param )
{
Param();
Exit();
THREAD_RETURN(0);
}
};
#endif // !_U_Thread_Win32_
rtabmap/utilite/include/rtabmap/utilite/Win32/UWin32.h
+62
View File
@@ -0,0 +1,62 @@
/////////////////////////////////////////////////////////////////////
// Written by Phillip Sitbon
// Copyright 2003
//
// Win32.h
// - Windows includes
//
/////////////////////////////////////////////////////////////////////
#ifndef _U_Win32_
#define _U_Win32_
#if !defined(_WINDOWS_)
// WIN32 Excludes
#ifdef WIN32_LEAN_AND_MEAN
# define VC_EXTRALEAN
# define WIN32_LEAN_AND_MEAN
# define _PRSHT_H_
# define NOGDICAPMASKS // CC_*, LC_*, PC_*, CP_*, TC_*, RC_
# define NOVIRTUALKEYCODES // VK_*
# define NOWINMESSAGES // WM_*, EM_*, LB_*, CB_*
# define NOWINSTYLES // WS_*, CS_*, ES_*, LBS_*, SBS_*, CBS_*
# define NOSYSMETRICS // SM_*
# define NOMENUS // MF_*
# define NOICONS // IDI_*
# define NOKEYSTATES // MK_*
# define NOSYSCOMMANDS // SC_*
# define NORASTEROPS // Binary and Tertiary raster ops
# define NOSHOWWINDOW // SW_*
# define OEMRESOURCE // OEM Resource values
# define NOATOM // Atom Manager routines
# define NOCLIPBOARD // Clipboard routines
# define NOCOLOR // Screen colors
# define NOCTLMGR // Control and Dialog routines
# define NODRAWTEXT // DrawText() and DT_*
# define NOGDI // All GDI defines and routines
# define NOKERNEL // All KERNEL defines and routines
# define NOUSER // All USER defines and routines
# define NONLS // All NLS defines and routines
# define NOMB // MB_* and MessageBox()
# define NOMEMMGR // GMEM_*, LMEM_*, GHND, LHND, associated routines
# define NOMETAFILE // typedef METAFILEPICT
# define NOMINMAX // Macros min(a,b) and max(a,b)
# define NOMSG // typedef MSG and associated routines
# define NOOPENFILE // OpenFile(), OemToAnsi, AnsiToOem, and OF_*
# define NOSCROLL // SB_* and scrolling routines
# define NOSERVICE // All Service Controller routines, SERVICE_ equates, etc.
# define NOSOUND // Sound driver routines
# define NOTEXTMETRIC // typedef TEXTMETRIC and associated routines
# define NOWH // SetWindowsHook and WH_*
# define NOWINOFFSETS // GWL_*, GCL_*, associated routines
# define NOCOMM // COMM driver routines
# define NOKANJI // Kanji support stuff.
# define NOHELP // Help engine interface.
# define NOPROFILER // Profiler interface.
# define NODEFERWINDOWPOS // DeferWindowPos routines
# define NOMCX // Modem Configuration Extensions
#endif // WIN32_LEAN_AND_MEAN
//
# include <windows.h>
#endif
#endif // !_U_Win32_