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

Tango: updated how RGB and depth images are synchronized (to minimize the dt between them). Added time offset to get stamps in epoch time. Trajectory Mode: fixed top button not selected.

Browse Source
This commit is contained in:
matlabbe
2017-01-30 12:29:34 -05:00
parent 819db9538f
commit 7b5761106d
5 changed files with 55 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
app/android/AndroidManifest.xml.in
View File

@@ -2,7 +2,7 @@
<!-- BEGIN_INCLUDE(manifest) --> <!-- BEGIN_INCLUDE(manifest) -->
<manifest xmlns:android="http://schemas.android.com/apk/res/android" <manifest xmlns:android="http://schemas.android.com/apk/res/android"
package="com.introlab.rtabmap" package="com.introlab.rtabmap"
android:versionCode="32" android:versionCode="34"
android:versionName="@RTABMAP_VERSION@"> android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" /> <uses-permission android:name="android.permission.CAMERA" />

81
app/android/jni/CameraTango.cpp
View File

@@ -98,6 +98,7 @@ CameraTango::CameraTango(int decimation, bool autoExposure) :
Camera(0), Camera(0),
tango_config_(0), tango_config_(0),
firstFrame_(true), firstFrame_(true),
stampEpochOffset_(0.0),
decimation_(decimation), decimation_(decimation),
autoExposure_(autoExposure), autoExposure_(autoExposure),
cloudStamp_(0), cloudStamp_(0),
@@ -337,45 +338,53 @@ void CameraTango::close()
void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
{ {
if(this->isRunning()) if(this->isRunning() && !cloud.empty())
{ {
//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
UASSERT(cloud.type() == CV_32FC4); UASSERT(cloud.type() == CV_32FC4);
boost::mutex::scoped_lock lock(dataMutex_); boost::mutex::scoped_lock lock(dataMutex_);
bool notify = cloud_.empty(); // From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
cloud_ = cloud.clone(); // "In the current version of Project Tango Tablet RGB IR camera
cloudStamp_ = timestamp; // is used for both depth and color images and it can only do one
LOGD("Depth received! (%d points)", cloud.cols); // or the other for each frame. So in the stream we get 4 RGB frames
if(!tangoColor_.empty() && notify) // followed by 1 Depth frame resulting in the pattern you observed. This
// is more of a hardware limitation."
//
// So, synchronize with the last RGB frame before the Depth is acquired
if(!tangoColor_.empty())
{ {
LOGD("Cloud: Release semaphore"); double dt = fabs(timestamp - tangoColorStamp_);
dataReady_.release();
//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
if(dt >= 0.0 && dt < 0.5)
{
bool notify = cloud_.empty();
cloud_ = cloud.clone();
cloudStamp_ = timestamp;
if(notify)
{
//LOGD("Cloud: Release semaphore");
dataReady_.release();
}
}
} }
} }
} }
void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp) void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
{ {
if(this->isRunning()) if(this->isRunning() && !tangoImage.empty())
{ {
//LOGD("RGB received! %fs", timestamp);
boost::mutex::scoped_lock lock(dataMutex_); boost::mutex::scoped_lock lock(dataMutex_);
if(!cloud_.empty()) tangoColor_ = tangoImage.clone();
{ tangoColorStamp_ = timestamp;
if(!tangoImage.empty()) tangoColorType_ = type;
{
bool notify = tangoColor_.empty();
tangoColor_ = tangoImage.clone();
tangoColorStamp_ = timestamp;
tangoColorType_ = type;
LOGD("RGB received!");
if(!cloud_.empty() && notify)
{
LOGD("RGB: Release semaphore");
dataReady_.release();
}
}
}
} }
} }
@@ -458,7 +467,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
SensorData CameraTango::captureImage(CameraInfo * info) SensorData CameraTango::captureImage(CameraInfo * info)
{ {
LOGI("Capturing image..."); //LOGI("Capturing image...");
SensorData data; SensorData data;
if(!dataReady_.acquire(1, 2000)) if(!dataReady_.acquire(1, 2000))
@@ -567,7 +576,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
{ {
// The Color Camera frame at timestamp t0 with respect to Depth // The Color Camera frame at timestamp t0 with respect to Depth
// Camera frame at timestamp t1. // Camera frame at timestamp t1.
LOGI("colorToDepth=%s", colorToDepth.prettyPrint().c_str()); //LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
int pixelsSet = 0; int pixelsSet = 0;
depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
@@ -607,7 +616,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
{ {
scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone(); scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
} }
LOGI("pixels depth set= %d", pixelsSet); //LOGD("pixels depth set= %d", pixelsSet);
} }
else else
{ {
@@ -620,15 +629,15 @@ SensorData CameraTango::captureImage(CameraInfo * info)
Transform poseDevice = getPoseAtTimestamp(rgbStamp); Transform poseDevice = getPoseAtTimestamp(rgbStamp);
LOGD("Local = %s", model.localTransform().prettyPrint().c_str()); //LOGD("Local = %s", model.localTransform().prettyPrint().c_str());
LOGD("tango = %s", poseDevice.prettyPrint().c_str()); //LOGD("tango = %s", poseDevice.prettyPrint().c_str());
LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str()); //LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
//Rotate in RTAB-Map's coordinate //Rotate in RTAB-Map's coordinate
Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device; Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
LOGD("rtabmap = %s", odom.prettyPrint().c_str()); //LOGD("rtabmap = %s", odom.prettyPrint().c_str());
LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str()); //LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp); data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
data.setGroundTruth(odom); data.setGroundTruth(odom);
@@ -661,6 +670,12 @@ void CameraTango::mainLoop()
{ {
rtabmap::Transform pose = data.groundTruth(); rtabmap::Transform pose = data.groundTruth();
data.setGroundTruth(Transform()); data.setGroundTruth(Transform());
// convert stamp to epoch
if(firstFrame_)
{
stampEpochOffset_ = UTimer::now()-data.stamp();
}
data.setStamp(stampEpochOffset_ + data.stamp());
LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001); LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001)); this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
firstFrame_ = false; firstFrame_ = false;

1
app/android/jni/CameraTango.h
View File

@@ -100,6 +100,7 @@ private:
void * tango_config_; void * tango_config_;
bool firstFrame_; bool firstFrame_;
UTimer cameraStartedTime_; UTimer cameraStartedTime_;
double stampEpochOffset_;
int decimation_; int decimation_;
bool autoExposure_; bool autoExposure_;
cv::Mat cloud_; cv::Mat cloud_;

8
app/android/jni/RTABMapApp.cpp
View File

@@ -191,6 +191,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
if(camera_) if(camera_)
{ {
delete camera_; delete camera_;
camera_ = 0;
} }
if(rtabmapThread_) if(rtabmapThread_)
{ {
@@ -407,6 +408,8 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
// OpenGL thread // OpenGL thread
int RTABMapApp::Render() int RTABMapApp::Render()
{ {
UASSERT(camera_!=0 && rtabmap_!=0);
UTimer fpsTime; UTimer fpsTime;
boost::mutex::scoped_lock lock(renderingMutex_); boost::mutex::scoped_lock lock(renderingMutex_);
@@ -983,10 +986,6 @@ void RTABMapApp::setLocalizationMode(bool enabled)
} }
void RTABMapApp::setTrajectoryMode(bool enabled) void RTABMapApp::setTrajectoryMode(bool enabled)
{ {
if(trajectoryMode_ != enabled)
{
main_scene_.SetCameraType(enabled?tango_gl::GestureCamera::kTopDown:tango_gl::GestureCamera::kThirdPersonFollow);
}
trajectoryMode_ = enabled; trajectoryMode_ = enabled;
this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_))); this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
} }
@@ -994,6 +993,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
void RTABMapApp::setGraphOptimization(bool enabled) void RTABMapApp::setGraphOptimization(bool enabled)
{ {
graphOptimization_ = enabled; graphOptimization_ = enabled;
UASSERT(camera_ != 0 && rtabmap_!=0 && rtabmap_->getMemory()!=0);
if(!camera_->isRunning() && rtabmap_->getMemory()->getLastWorkingSignature()!=0) if(!camera_->isRunning() && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
{ {
std::map<int, rtabmap::Transform> poses; std::map<int, rtabmap::Transform> poses;

1
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
View File

@@ -1013,6 +1013,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{ {
item.setChecked(!item.isChecked()); item.setChecked(!item.isChecked());
RTABMapLib.setTrajectoryMode(item.isChecked()); RTABMapLib.setTrajectoryMode(item.isChecked());
setCamera(item.isChecked()?2:1);
} }
else if(itemId == R.id.graph_optimization) else if(itemId == R.id.graph_optimization)
{ {