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

Tango: added "Pose (x,y,z)" in debug view

Browse Source
This commit is contained in:
matlabbe
2017-10-06 13:42:12 -04:00
parent 1aad6d0517
commit 9a09db9212
2 changed files with 23 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3223,6 +3223,12 @@ bool RTABMapApp::handleEvent(UEvent * event)
float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
rtabmap::Transform currentPose = main_scene_.GetCameraPose();
float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;
if(!currentPose.isNull())
{
currentPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
}
// Call JAVA callback with some stats
UINFO("Send statistics to GUI");
@@ -3236,7 +3242,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFFFI)V" );
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFFFIFFFFFF)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
@@ -3259,7 +3265,13 @@ bool RTABMapApp::handleEvent(UEvent * event)
rehearsalValue,
optimizationMaxError,
distanceTravelled,
fastMovement);
fastMovement,
x,
y,
z,
roll,
pitch,
yaw);
success = true;
}
}

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

@@ -1155,11 +1155,17 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
final float rehearsalValue,
final float optimizationMaxError,
final float distanceTravelled,
final int fastMovement)
final int fastMovement,
final float x,
final float y,
final float z,
final float roll,
final float pitch,
final float yaw)
{
if(!DISABLE_LOG) Log.i(TAG, String.format("updateStatsCallback()"));
final String[] statusTexts = new String[18];
final String[] statusTexts = new String[19];
if(mButtonPause!=null && !mButtonPause.isChecked())
{
String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate;
@@ -1246,6 +1252,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
statusTexts[index++] = getString(R.string.hypothesis)+(int)(hypothesis*100.0f) +" / " + (int)(Float.parseFloat(mLoopThr)*100.0f) + " (" + (loopClosureId>0?loopClosureId:highestHypId)+")";
statusTexts[index++] = getString(R.string.fps)+(int)fps+" Hz";
statusTexts[index++] = getString(R.string.distance)+(int)distanceTravelled+" m";
statusTexts[index++] = String.format("Pose (x,y,z): %.2f %.2f %.2f", x,y,z);
runOnUiThread(new Runnable() {
public void run() {