i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraK4A: use mkv framerate if camera rate is -1. Preferences/Source: fixed imu filter parameters not reset on restore defaults

Browse Source
This commit is contained in:
matlabbe
2019-07-11 18:13:20 -04:00
parent 0973f48b2c
commit 89f1e809e6
4 changed files with 116 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/camera/CameraK4A.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap
{
@@ -72,6 +73,8 @@ private:
int deviceId_;
std::string fileName_;
bool ir_;
double previousStamp_;
UTimer timer_;
#endif
};

42
corelib/src/camera/CameraK4A.cpp
View File

@@ -57,7 +57,8 @@ CameraK4A::CameraK4A(
,playbackHandle_(NULL),
transformationHandle_(NULL),
deviceId_(deviceId),
ir_(false)
ir_(false),
previousStamp_(0.0)
#endif
{
UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
@@ -73,7 +74,8 @@ CameraK4A::CameraK4A(
transformationHandle_(NULL),
deviceId_(-1),
fileName_(fileName),
ir_(false)
ir_(false),
previousStamp_(0.0)
#endif
{
}
@@ -388,6 +390,42 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
{
data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
data.setIMU(imu);
// Frame rate
if (this->getImageRate() < 0.0f)
{
if (stamp == 0)
{
UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
}
else if (previousStamp_ > 0)
{
float ratio = -this->getImageRate();
int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
if (sleepTime > 10000)
{
UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
sleepTime / 1000, previousStamp_, stamp);
sleepTime = 10000;
}
if (sleepTime > 2)
{
uSleep(sleepTime - 2);
}
// Add precision at the cost of a small overhead
while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
{
//
}
double slept = timer_.getElapsedTime();
timer_.start();
UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
}
previousStamp_ = stamp;
}
break;
}
}