i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-10 19:49:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tango: fixed black clouds. Rtabmap: only add latest optimized pose to graph if localized.

Browse Source
This commit is contained in:
matlabbe
2016-12-05 13:27:29 -05:00
parent 3c17049084
commit e5524ddc1d
6 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/AndroidManifest.xml.in
+1 -1
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="27" android:versionCode="28"
android:versionName="@RTABMAP_VERSION@"> android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" /> <uses-permission android:name="android.permission.CAMERA" />
app/android/jni/RTABMapApp.cpp
+4 -2
View File
@@ -432,7 +432,9 @@ int RTABMapApp::Render()
} }
// Did we lose OpenGL context? If so, recreate the context; // Did we lose OpenGL context? If so, recreate the context;
if(main_scene_.getAddedClouds().size() != createdMeshes_.size()) std::set<int> added = main_scene_.getAddedClouds();
added.erase(-1);
if(added.size() != createdMeshes_.size())
{ {
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{ {
@@ -668,7 +670,7 @@ int RTABMapApp::Render()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get()); cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get());
if(cloud->size()) if(cloud->size() && indices->size())
{ {
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)", LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows, odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
app/android/jni/point_cloud_drawable.cpp
+1 -1
View File
@@ -336,7 +336,7 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size"); GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
glUniform1f(point_size_handle_, pointSize); glUniform1f(point_size_handle_, pointSize);
GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain"); GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "u_gain");
glUniform1f(gain_handle, gain_); glUniform1f(gain_handle, gain_);
GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex"); GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
app/android/res/menu/optionmenu.xml
+2 -2
View File
@@ -56,7 +56,7 @@
<item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" /> <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/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/grid_visible" android:checked="true" android:title="Grid Visible" />
<item android:id="@+id/auto_exposure" android:checked="true" android:title="Auto Exposure" /> <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
</group> </group>
</menu> </menu>
</item> </item>
@@ -65,10 +65,10 @@
<group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all"> <group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
<item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" /> <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
<item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" /> <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
<item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
<item android:id="@+id/append" android:checked="true" android:title="Append Mode" /> <item android:id="@+id/append" android:checked="true" android:title="Append Mode" />
<item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" /> <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
<item android:id="@+id/drift_correction" android:checked="false" android:title="Drift Correction" /> <item android:id="@+id/drift_correction" android:checked="false" android:title="Drift Correction" />
<item android:id="@+id/auto_exposure" android:checked="true" android:title="Auto Exposure" />
<item android:id="@+id/resolution" android:checked="false" android:title="HD Mode" /> <item android:id="@+id/resolution" android:checked="false" android:title="HD Mode" />
<item android:id="@+id/data_recorder" android:checked="false" android:title="Data Recorder Mode" /> <item android:id="@+id/data_recorder" android:checked="false" android:title="Data Recorder Mode" />
<item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters..."> <item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+7 -2
View File
@@ -79,6 +79,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private MenuItem mItemExport; private MenuItem mItemExport;
private MenuItem mItemLocalizationMode; private MenuItem mItemLocalizationMode;
private MenuItem mItemTrajectoryMode; private MenuItem mItemTrajectoryMode;
private MenuItem mItemAppendMode;
private MenuItem mItemRenderingPointCloud; private MenuItem mItemRenderingPointCloud;
private MenuItem mItemRenderingMesh; private MenuItem mItemRenderingMesh;
private MenuItem mItemRenderingTextureMesh; private MenuItem mItemRenderingTextureMesh;
@@ -329,6 +330,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mItemExport = menu.findItem(R.id.export); mItemExport = menu.findItem(R.id.export);
mItemLocalizationMode = menu.findItem(R.id.localization_mode); mItemLocalizationMode = menu.findItem(R.id.localization_mode);
mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode); mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode);
mItemAppendMode = menu.findItem(R.id.append);
mItemRenderingPointCloud = menu.findItem(R.id.point_cloud); mItemRenderingPointCloud = menu.findItem(R.id.point_cloud);
mItemRenderingMesh = menu.findItem(R.id.mesh); mItemRenderingMesh = menu.findItem(R.id.mesh);
mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh); mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh);
@@ -360,7 +362,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{ {
if(mItemPause!=null) if(mItemPause!=null)
{ {
((TextView)findViewById(R.id.status)).setText(mItemPause.isChecked()?"Paused":mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]):String.format("Mapping (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex])); ((TextView)findViewById(R.id.status)).setText(mItemPause.isChecked()?"Paused":mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]):mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]):String.format("Mapping (%s Hz)", mUpdateRateValues[mParamUpdateRateHzIndex]));
} }
((TextView)findViewById(R.id.points)).setText(String.valueOf(points)); ((TextView)findViewById(R.id.points)).setText(String.valueOf(points));
@@ -425,7 +427,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
Log.i(TAG, String.format("rtabmapInitEventsUI() status=%d msg=%s", status, msg)); Log.i(TAG, String.format("rtabmapInitEventsUI() status=%d msg=%s", status, msg));
((TextView)findViewById(R.id.status)).setText( ((TextView)findViewById(R.id.status)).setText(
status == 1 && msg.isEmpty()?mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":"Mapping":msg); status == 1 && msg.isEmpty()?(mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?"Recording":"Mapping"):msg);
} }
//called from jni //called from jni
@@ -1233,6 +1235,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mItemOpen.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked()); mItemOpen.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
mItemPostProcessing.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked()); mItemPostProcessing.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
mItemExport.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked()); mItemExport.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
mItemLocalizationMode.setEnabled(!mItemDataRecorderMode.isChecked());
mItemAppendMode.setEnabled(!mItemDataRecorderMode.isChecked());
if(mItemDataRecorderMode.isChecked()) if(mItemDataRecorderMode.isChecked())
{ {
corelib/src/Rtabmap.cpp
+8 -4
View File
@@ -2351,10 +2351,11 @@ bool Rtabmap::process(
} }
Signature lastSignatureData(signature->id()); Signature lastSignatureData(signature->id());
Transform lastSignatureOptimizedPose; Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && signature->getLinks().size())
{ {
lastSignatureOptimizedPose = _optimizedPoses.at(signature->id()); // only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
} }
if(_publishLastSignatureData) if(_publishLastSignatureData)
{ {
@@ -2578,7 +2579,10 @@ bool Rtabmap::process(
groundTruth))); groundTruth)));
} }
localGraphSize = (int)poses.size(); localGraphSize = (int)poses.size();
poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureOptimizedPose)); // in case we are in localization if(!lastSignatureLocalizedPose.isNull())
{
poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureLocalizedPose)); // in case we are in localization
}
statistics_.setPoses(poses); statistics_.setPoses(poses);
statistics_.setConstraints(constraints); statistics_.setConstraints(constraints);
statistics_.setSignatures(signatures); statistics_.setSignatures(signatures);