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: 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 hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f); float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 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 // Call JAVA callback with some stats
UINFO("Send statistics to GUI"); UINFO("Send statistics to GUI");
@@ -3236,7 +3242,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
jclass clazz = env->GetObjectClass(RTABMapActivity); jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz) if(clazz)
{ {
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFFFI)V" ); jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFFFIFFFFFF)V" );
if(methodID) if(methodID)
{ {
env->CallVoidMethod(RTABMapActivity, methodID, env->CallVoidMethod(RTABMapActivity, methodID,
@@ -3259,7 +3265,13 @@ bool RTABMapApp::handleEvent(UEvent * event)
rehearsalValue, rehearsalValue,
optimizationMaxError, optimizationMaxError,
distanceTravelled, distanceTravelled,
fastMovement); fastMovement,
x,
y,
z,
roll,
pitch,
yaw);
success = true; 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 rehearsalValue,
final float optimizationMaxError, final float optimizationMaxError,
final float distanceTravelled, 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()")); 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()) if(mButtonPause!=null && !mButtonPause.isChecked())
{ {
String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate; 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.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.fps)+(int)fps+" Hz";
statusTexts[index++] = getString(R.string.distance)+(int)distanceTravelled+" m"; 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() { runOnUiThread(new Runnable() {
public void run() { public void run() {