i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

-Updated CameraModel and StereoCameraModel so that a stereo camera model can be initialization from know intrinsics and extrinsics

Browse Source 
-CreateSimpleCalibrationDialog updated to create mono/stereo calibration files with advanced option (setting intrinsics of each camera + extrinsics between them). Rectification R and new camera P matrices are then automatically computed.
-Added GeodeticCoords class for convience conversion between GPS values (latitude/longitude/altitude) to local coordinate (ENU).
-Added support of Malaga Urban and St Lucia ground truths.
-CameraImages: added option to debayer images. Timestamps file: added support of "sec millisec" format.
-Added UException class: exceptions are sent instead of exiting the application on UASSERT or UFATAL.
This commit is contained in:
matlabbe
2016-01-19 17:43:33 -05:00
parent 72e66605e4
commit 7bfd76e747
29 changed files with 1739 additions and 702 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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_ */
utilite/include/rtabmap/utilite/ULogger.h
+5 -26
View File
@@ -25,6 +25,7 @@
#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>
@@ -51,14 +52,15 @@
#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__)
#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__)
#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)
@@ -335,15 +337,6 @@ public:
static void setLevel(ULogger::Level level) {level_ = level;}
static ULogger::Level level() {return level_;}
/**
* Make application to exit when a log with level is written (useful for debugging). The message is printed to
* console (whatever the logger type) and an ULogEvent is sent (synchronously... see UEventsManager::post()) before exiting.
*
* Note : A kFatal level will always exit whatever the level specified here.
*/
static void setExitLevel(ULogger::Level exitLevel) {exitLevel_ = exitLevel;}
static ULogger::Level exitLevel() {return exitLevel_;}
/**
* 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()).
@@ -386,7 +379,7 @@ public:
int line,
const char *function,
const char* msg,
...);
...) throw(UException);
/**
* Get the time in the format "2008-7-13 12:23:44".
@@ -538,12 +531,6 @@ private:
*/
static Level level_;
/*
* The severity at which the application exits.
* Note : A FATAL level will always exit whatever the level specified here.
*/
static Level exitLevel_;
/*
* The severity at which the message is also sent in a ULogEvent.
*/
@@ -563,14 +550,6 @@ private:
static bool buffered_;
static std::string bufferedMsgs_;
/*
* State attribute. This state happens when an exit level
* message is received.
* Messages received during this state are not logged.
* @see exitLevel_
*/
static bool exitingState_;
};
#endif // ULOGGER_H
utilite/src/ULogger.cpp
+15 -23
View File
@@ -53,9 +53,7 @@ bool ULogger::printWhereFullPath_ = false;
bool ULogger::printThreadID_ = false;
bool ULogger::limitWhereLength_ = false;
bool ULogger::buffered_ = false;
bool ULogger::exitingState_ = false;
ULogger::Level ULogger::level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error)
ULogger::Level ULogger::exitLevel_ = kFatal;
ULogger::Level ULogger::eventLevel_ = kFatal;
const char * ULogger::levelName_[5] = {"DEBUG", " INFO", " WARN", "ERROR", "FATAL"};
ULogger* ULogger::instance_ = 0;
@@ -313,18 +311,13 @@ void ULogger::write(ULogger::Level level,
const char* msg,
...)
{
if(exitingState_)
{
// Ignore messages after a fatal exit...
return;
}
loggerMutex_.lock();
if(type_ == kTypeNoLog && level < kFatal)
{
loggerMutex_.unlock();
return;
}
if(strlen(msg) == 0 && !printWhere_ && level < exitLevel_)
if(strlen(msg) == 0 && !printWhere_ && level < kFatal)
{
loggerMutex_.unlock();
// No need to show an empty message if we don't print where.
@@ -502,11 +495,10 @@ void ULogger::write(ULogger::Level level,
va_start(args, msg);
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(level >= exitLevel_)
if(level >= kFatal)
{
// Send it synchronously, then receivers
// can do something before the code (exiting) below is executed.
exitingState_ = true;
UEventsManager::post(new ULogEvent(fullMsg, kFatal), false);
}
else
@@ -515,23 +507,23 @@ void ULogger::write(ULogger::Level level,
}
}
if(level >= exitLevel_)
if(level >= kFatal)
{
printf("\n*******\n%s message occurred! Application will now exit.\n", levelName_[level]);
if(type_ != kTypeConsole)
std::string fullMsg = uFormat("%s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(instance_)
{
printf(" %s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
vprintf(msg, args);
va_end(args);
destroyer_.setDoomed(0);
delete instance_; // If a FileLogger is used, this will close the file.
instance_ = 0;
}
printf("\n*******\n");
destroyer_.setDoomed(0);
delete instance_; // If a FileLogger is used, this will close the file.
instance_ = 0;
//========================================================================
// EXIT APPLICATION
exit(1);
// Throw exception
loggerMutex_.unlock();
throw UException(fullMsg);
//========================================================================
}
}