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

Tango: Handling app pause/resume without having to restart mapping (OpenGL context flushed issue).

Browse Source
This commit is contained in:
matlabbe
2016-09-08 11:55:44 -04:00
parent 53c3981651
commit 2eb2362da8
8 changed files with 131 additions and 65 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) -->
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
package="com.introlab.rtabmap"
android:versionCode="11"
android:versionCode="12"
android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" />

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

@@ -77,8 +77,11 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
{
CameraTango* app = static_cast<CameraTango*>(context);
app->poseReceived(app->tangoPoseToTransform(pose, true));
if(pose->status_code == TANGO_POSE_VALID)
{
CameraTango* app = static_cast<CameraTango*>(context);
app->poseReceived(app->tangoPoseToTransform(pose, true));
}
}
void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
@@ -318,6 +321,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
-1.0f, 0.0f, 0.0f, 0.0f,
0.0f, -1.0f, 0.0f, 0.0f));
cameraStartedTime_.restart();
return true;
}
@@ -634,7 +639,11 @@ SensorData CameraTango::captureImage(CameraInfo * info)
void CameraTango::mainLoopBegin()
{
uSleep(2000); // just to make sure that the camera is started
double t = cameraStartedTime_.elapsed();
if(t < 5.0)
{
uSleep((5.0-t)*1000); // just to make sure that the camera is started
}
}
void CameraTango::mainLoop()

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

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEventsSender.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEvent.h>
#include <rtabmap/utilite/UTimer.h>
#include <boost/thread/mutex.hpp>
class TangoPoseData;
@@ -97,6 +98,7 @@ private:
private:
void * tango_config_;
bool firstFrame_;
UTimer cameraStartedTime_;
int decimation_;
bool autoExposure_;
cv::Mat cloud_;

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

@@ -114,18 +114,20 @@ RTABMapApp::RTABMapApp() :
driftCorrection_(false),
localizationMode_(false),
trajectoryMode_(false),
autoExposure_(false),
autoExposure_(true),
fullResolution_(false),
maxCloudDepth_(0.0),
meshTrianglePix_(1),
meshAngleToleranceDeg_(15.0),
paused_(false),
clearSceneOnNextRender_(false),
filterPolygonsOnNextRender_(false),
gainCompensationOnNextRender_(false),
cameraJustInitialized_(false),
totalPoints_(0),
totalPolygons_(0),
lastDrawnCloudsCount_(0),
renderingFPS_(0.0f)
renderingTime_(0.0f)
{
}
@@ -158,7 +160,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
totalPoints_ = 0;
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingFPS_ = 0.0f;
renderingTime_ = 0.0f;
if(camera_)
{
@@ -249,7 +251,11 @@ bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
if(camera_->init())
{
LOGI("Start camera thread");
camera_->start();
if(!paused_)
{
camera_->start();
}
cameraJustInitialized_ = true;
return true;
}
LOGE("Failed camera initialization!");
@@ -303,6 +309,8 @@ private:
// OpenGL thread
int RTABMapApp::Render()
{
UTimer fpsTime;
bool notifyDataLoaded = false;
boost::mutex::scoped_lock lock(renderingMutex_);
// should be before clearSceneOnNextRender_ in case openDatabase is called
@@ -330,7 +338,23 @@ int RTABMapApp::Render()
totalPoints_ = 0;
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingFPS_ = 0.0f;
renderingTime_ = 0.0f;
}
// Did we lose OpenGL context? If so, recreate the context;
if(main_scene_.getAddedClouds().size() != createdMeshes_.size())
{
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(!main_scene_.hasCloud(iter->first))
{
cv::Mat compressed = iter->second.texture;
iter->second.texture = rtabmap::uncompressImage(iter->second.texture);
main_scene_.addMesh(iter->first, iter->second, iter->second.pose);
main_scene_.setCloudVisible(iter->first, iter->second.visible);
iter->second.texture = compressed;
}
}
}
// Process events
@@ -347,9 +371,28 @@ int RTABMapApp::Render()
{
// update camera pose?
main_scene_.SetCameraPose(pose);
if(!camera_->isRunning() && cameraJustInitialized_)
{
notifyDataLoaded = true;
cameraJustInitialized_ = false;
}
}
rtabmap::OdometryEvent odomEvent;
{
boost::mutex::scoped_lock lock(odomMutex_);
if(odomEvents_.size())
{
LOGI("Process odom events");
odomEvent = odomEvents_.back();
odomEvents_.clear();
if(cameraJustInitialized_)
{
notifyDataLoaded = true;
cameraJustInitialized_ = false;
}
}
}
bool notifyDataLoaded = false;
if(rtabmapEvents.size())
{
LOGI("Process rtabmap events");
@@ -422,14 +465,9 @@ int RTABMapApp::Render()
main_scene_.setCloudPose(id, iter->second);
main_scene_.setCloudVisible(id, true);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
if(meshIter!=createdMeshes_.end())
{
meshIter->second.pose = iter->second;
}
else
{
UERROR("Not found mesh %d !?!?", id);
}
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.pose = iter->second;
meshIter->second.visible = true;
}
else if(uContains(bufferedSensorData, id))
{
@@ -474,11 +512,12 @@ int RTABMapApp::Render()
inserted.first->second.height = cloud->height;
inserted.first->second.polygons = outputPolygons;
inserted.first->second.pose = iter->second;
inserted.first->second.visible = true;
inserted.first->second.texture = data.imageRaw();
main_scene_.addMesh(id, inserted.first->second, iter->second);
inserted.first->second.texture = data.imageCompressed(); // keep comrpessed
inserted.first->second.texture = data.imageCompressed(); // keep compressed
}
else
{
@@ -517,42 +556,32 @@ int RTABMapApp::Render()
if(*iter > 0 && poses.find(*iter) == poses.end())
{
main_scene_.setCloudVisible(*iter, false);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.visible = true;
}
}
}
else
{
rtabmap::OdometryEvent event;
bool set = false;
{
boost::mutex::scoped_lock lock(odomMutex_);
if(odomEvents_.size())
{
LOGI("Process odom events");
event = odomEvents_.back();
odomEvents_.clear();
set = true;
}
}
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
//just process the last one
if(set && !event.pose().isNull())
if(!odomEvent.pose().isNull())
{
if(odomCloudShown_ && !trajectoryMode_)
{
if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), 1, maxCloudDepth_);
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), 1, maxCloudDepth_);
if(cloud->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
event.data().imageRaw().cols, event.data().imageRaw().rows,
event.data().depthRaw().cols, event.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*event.pose());
odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*odomEvent.pose());
main_scene_.setCloudVisible(-1, true);
}
else
@@ -641,9 +670,11 @@ int RTABMapApp::Render()
notifyDataLoaded = true;
}
UTimer fpsTime;
lastDrawnCloudsCount_ = main_scene_.Render();
renderingFPS_ = 1.0/fpsTime.elapsed();
if(renderingTime_ < fpsTime.elapsed())
{
renderingTime_ = fpsTime.elapsed();
}
if(rtabmapEvents.size())
{
@@ -668,16 +699,19 @@ void RTABMapApp::OnTouchEvent(int touch_count,
void RTABMapApp::setPausedMapping(bool paused)
{
paused_ = paused;
if(camera_)
{
if(paused)
if(paused_)
{
LOGW("Pause!");
camera_->kill();
}
else
{
LOGW("Resume!");
UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdTriggerNewMap));
camera_->start();
}
}
@@ -1246,7 +1280,7 @@ void RTABMapApp::handleEvent(UEvent * event)
featuresExtracted,
hypothesis,
lastDrawnCloudsCount_,
renderingFPS_,
renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
rejected);
success = true;
}
@@ -1258,6 +1292,7 @@ void RTABMapApp::handleEvent(UEvent * event)
{
UERROR("Failed to call RTABMapActivity::updateStatsCallback");
}
renderingTime_ = 0.0f;
}
}

5
app/android/jni/RTABMapApp.h
View File

@@ -158,14 +158,15 @@ class RTABMapApp : public UEventsHandler {
rtabmap::ParametersMap mappingParameters_;
bool paused_;
bool clearSceneOnNextRender_;
bool filterPolygonsOnNextRender_;
bool gainCompensationOnNextRender_;
bool cameraJustInitialized_;
int totalPoints_;
int totalPolygons_;
int lastDrawnCloudsCount_;
float renderingFPS_;
float renderingTime_;
// main_scene_ includes all drawable object for visualizing Tango device's
// movement and point cloud.

5
app/android/jni/util.h
View File

@@ -140,9 +140,10 @@ struct Mesh
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // dense or organized cloud
std::vector<pcl::Vertices> polygons;
std::vector<int> denseToOrganizedIndices; // should be set if cloud is dense, used for texturing
int width; // width of the organized cloud
int height; // height of the organized cloud
unsigned int width; // width of the organized cloud
unsigned int height; // height of the organized cloud
rtabmap::Transform pose;
bool visible;
cv::Mat texture;
};

4
app/android/res/menu/optionmenu.xml
View File

@@ -22,7 +22,6 @@
</menu>
</item>
<item android:id="@+id/open" android:title="Open"/>
<item android:id="@+id/save" android:title="Save"/>
<item android:id="@+id/export" android:title="Export...">
<menu>
<group android:id="@+id/group_export">
@@ -31,6 +30,7 @@
</group>
</menu>
</item>
<item android:id="@+id/save" android:title="Save"/>
<item android:id="@+id/reset" android:title="Reset"/>
<item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
@@ -53,7 +53,7 @@
<item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
<item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
<item android:id="@+id/grid_visible" android:checked="true" android:title="Grid Visible" />
<item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
<item android:id="@+id/auto_exposure" android:checked="true" android:title="Auto Exposure" />
</group>
</menu>
</item>

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

@@ -67,6 +67,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
// Screen size for normalizing the touch input for orbiting the render camera.
private Point mScreenSize = new Point();
private boolean mPauseFirstTime = true;
private boolean mOnPause = false;
private MenuItem mItemPause;
private MenuItem mItemSave;
@@ -161,6 +162,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mLayoutDebug.setVisibility(LinearLayout.GONE);
mProgressDialog = new ProgressDialog(this);
mProgressDialog.setCanceledOnTouchOutside(false);
mRenderer.setProgressDialog(mProgressDialog);
// Check if the Tango Core is out dated.
@@ -218,6 +220,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
@Override
protected void onResume() {
super.onResume();
mProgressDialog.setTitle("");
mProgressDialog.setMessage(String.format("Hold Tight! Initializing Tango Service..."));
mProgressDialog.show();
if(mOnPause)
{
mToast.makeText(this, "Mapping is paused!", mToast.LENGTH_LONG).show();
}
else
{
mToast.makeText(this, "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\".", mToast.LENGTH_LONG).show();
}
mOnPause = false;
TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
@@ -226,16 +243,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
mGLView.onResume();
mTotalLoopClosures = 0;
if(mItemOpen != null)
{
mItemOpen.setEnabled(false);
mItemPause.setChecked(false);
mItemSave.setEnabled(false);
mItemExport.setEnabled(false);
mItemPostProcessing.setEnabled(false);
}
} else {
Log.i(TAG, String.format("Asking for motion tracking permission"));
@@ -248,14 +255,20 @@ public class RTABMapActivity extends Activity implements OnClickListener {
@Override
protected void onPause() {
super.onPause();
// This deletes OpenGL context!
mGLView.onPause();
// Delete all the non-OpenGl resources.
mOnPause = true;
RTABMapLib.onPause();
mOpenedDatabasePath = "";
RTABMapLib.openDatabase(mTempDatabasePath);
unbindService(mTangoServiceConnection);
if(!mItemPause.isChecked())
{
onOptionsItemSelected(mItemPause);
}
}
@Override
@@ -440,7 +453,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
File tempFile = new File(mTempDatabasePath);
if(tempFile.renameTo(outputFile))
{
msg = String.format("Database saved to \"%s\".", mNewDatabasePath);
msg = String.format("Database saved to \"%s\". Tip: You can open a saved database with \"Open\".", mNewDatabasePath);
Intent intent = new Intent(this, RTABMapActivity.class);
// use System.currentTimeMillis() to have a unique ID for the pending intent
@@ -653,16 +666,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{
RTABMapLib.setPausedMapping(true);
((TextView)findViewById(R.id.status)).setText("Paused");
if(mPauseFirstTime)
if(mPauseFirstTime && !mOnPause)
{
mPauseFirstTime = false;
mToast.makeText(getActivity(), String.format("Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
mToast.makeText(getActivity(), String.format("Tip: Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
}
}
else
{
RTABMapLib.setPausedMapping(false);
((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
}
}
else if (itemId == R.id.post_processing_standard)
@@ -863,6 +877,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{
item.setChecked(!item.isChecked());
RTABMapLib.setDriftCorrection(item.isChecked());
if(item.isChecked())
{
mToast.makeText(getActivity(), String.format("Tip: With drift correction is enabled, move slowly to get better results."), mToast.LENGTH_LONG).show();
}
}
else if(itemId == R.id.graph_visible)
{