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: fixed export with optimization, added "Nodes Filtering" option

Browse Source
This commit is contained in:
matlabbe
2016-04-09 18:57:49 -04:00
parent 656431a03f
commit 72fc714cb4
9 changed files with 175 additions and 79 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="5"
android:versionCode="7"
android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" />

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

@@ -45,6 +45,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/Memory.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
@@ -87,6 +89,7 @@ RTABMapApp::RTABMapApp() :
logHandler_(0),
odomCloudShown_(true),
graphOptimization_(true),
nodesFiltering_(false),
localizationMode_(false),
trajectoryMode_(false),
autoExposure_(false),
@@ -196,6 +199,9 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
clearSceneOnNextRender_ = true;
rtabmap::Statistics stats;
stats.setSignatures(signatures);
stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize());
stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size());
stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed());
stats.setPoses(poses);
stats.setConstraints(links);
rtabmapEvents_.push_back(stats);
@@ -258,6 +264,20 @@ void RTABMapApp::SetViewPort(int width, int height)
main_scene_.SetupViewPort(width, height);
}
class PostRenderEvent : public UEvent
{
public:
PostRenderEvent(const rtabmap::Statistics & stats) :
stats_(stats)
{
}
virtual std::string getClassName() const {return "PostRenderEvent";}
const rtabmap::Statistics & getStats() const {return stats_;}
private:
rtabmap::Statistics stats_;
};
// OpenGL thread
int RTABMapApp::Render()
{
@@ -357,20 +377,14 @@ int RTABMapApp::Render()
}
}
const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
if(poses.size())
{
const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
//update graph
main_scene_.updateGraph(poses, links);
// update clouds
//filter poses?
// make sure the last pose is here though
//poses.insert(*rtabmapEvents.back().poses().rbegin());
boost::mutex::scoped_lock lock(meshesMutex_);
std::set<std::string> strIds;
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
@@ -382,6 +396,15 @@ int RTABMapApp::Render()
//just update pose
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);
}
}
else if(uContains(bufferedSensorData, id))
{
@@ -423,7 +446,6 @@ int RTABMapApp::Render()
// protect createdMeshes_ used also by exportMesh() method
boost::mutex::scoped_lock lock(meshesMutex_);
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = outputCloud;
@@ -443,6 +465,22 @@ int RTABMapApp::Render()
}
}
//filter poses?
if(poses.size() > 2)
{
if(nodesFiltering_)
{
for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != rtabmap::Link::kNeighbor)
{
int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
poses.erase(oldId);
}
}
}
}
//update cloud visibility
std::set<int> addedClouds = main_scene_.getAddedClouds();
for(std::set<int>::const_iterator iter=addedClouds.begin();
@@ -506,6 +544,13 @@ int RTABMapApp::Render()
lastDrawnCloudsCount_ = main_scene_.Render();
if(rtabmapEvents.size())
{
// send statistics to GUI
LOGI("Posting PostRenderEvent!");
UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
}
return notifyDataLoaded?1:0;
}
@@ -567,6 +612,27 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
void RTABMapApp::setGraphOptimization(bool enabled)
{
graphOptimization_ = enabled;
if(!camera_->isRunning())
{
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap_->getGraph(poses, links, true, true);
if(poses.size())
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap::Statistics stats = rtabmap_->getStatistics();
stats.setPoses(poses);
stats.setConstraints(links);
rtabmapEvents_.push_back(stats);
rtabmap_->setOptimizedPoses(poses);
}
}
}
void RTABMapApp::setNodesFiltering(bool enabled)
{
nodesFiltering_ = enabled;
setGraphOptimization(graphOptimization_); // this will resend the graph if paused
}
void RTABMapApp::setGraphVisible(bool visible)
{
@@ -659,6 +725,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
{
pcl::TextureMesh textureMesh;
std::vector<cv::Mat> textures;
int totalPolygons = 0;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
{
boost::mutex::scoped_lock lock(meshesMutex_);
@@ -716,6 +783,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
textureMesh.tex_polygons[oi][j] = vertices;
}
totalPolygons += densePolygons.size();
polygonsStep += denseCloud->size();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
@@ -761,7 +829,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
textureMesh.tex_materials[i].tex_file = uSplit(UFile::getName(filePath), '.').front()+"/"+textureMesh.tex_materials[i].tex_name+".png";
}
UINFO("Saving obj to %s.", filePath.c_str());
UINFO("Saving obj (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), totalPolygons, filePath.c_str());
success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
if(success)
{
@@ -814,7 +882,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
mesh.polygons = mergedPolygons;
UINFO("Saving ply to %s.", filePath.c_str());
UINFO("Saving ply (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), (int)mergedPolygons.size(), filePath.c_str());
success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
if(success)
{
@@ -878,7 +946,7 @@ int RTABMapApp::postProcessing(int approach)
if(poses.size())
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap::Statistics stats;
rtabmap::Statistics stats = rtabmap_->getStatistics();
stats.setPoses(poses);
stats.setConstraints(links);
rtabmapEvents_.push_back(stats);
@@ -900,7 +968,7 @@ void RTABMapApp::handleEvent(UEvent * event)
// called from events manager thread, so protect the data
if(event->getClassName().compare("OdometryEvent") == 0)
{
LOGI("GUI: Received OdometryEvent!");
LOGI("Received OdometryEvent!");
if(odomMutex_.try_lock())
{
odomEvents_.clear();
@@ -914,67 +982,11 @@ void RTABMapApp::handleEvent(UEvent * event)
if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
event->getClassName().compare("RtabmapEvent") == 0)
{
LOGI("GUI: Received RtabmapEvent!");
int nodes =0;
int words = 0;
int loopClosureId = 0;
float updateTime = 0.0f;
int databaseMemoryUsed = 0;
int inliers = 0;
int featuresExtracted = 0;
float hypothesis = 0.0f;
LOGI("Received RtabmapEvent!");
if(camera_->isRunning())
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
if(camera_->isRunning())
{
rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
nodes = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
words = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
updateTime = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
loopClosureId = rtabmapEvents_.back().loopClosureId()>0?rtabmapEvents_.back().loopClosureId():rtabmapEvents_.back().proximityDetectionId()>0?rtabmapEvents_.back().proximityDetectionId():0;
databaseMemoryUsed = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
inliers = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
featuresExtracted = rtabmapEvents_.back().getSignatures().size()?rtabmapEvents_.back().getSignatures().rbegin()->second.getWords().size():0;
hypothesis = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
}
}
// Call JAVA callback with some stats
bool success = false;
if(jvm && RTABMapActivity)
{
JNIEnv *env = 0;
jint rs = jvm->AttachCurrentThread(&env, NULL);
if(rs == JNI_OK && env)
{
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIFI)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
nodes,
words,
totalPoints_,
totalPolygons_,
updateTime,
loopClosureId,
databaseMemoryUsed,
inliers,
featuresExtracted,
hypothesis,
lastDrawnCloudsCount_);
success = true;
}
}
}
jvm->DetachCurrentThread();
}
if(!success)
{
UERROR("Failed to call RTABMapActivity::updateStatsCallback");
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
}
}
}
@@ -1025,7 +1037,7 @@ void RTABMapApp::handleEvent(UEvent * event)
if(event->getClassName().compare("RtabmapEventInit") == 0)
{
LOGI("GUI: Received RtabmapEventInit!");
LOGI("Received RtabmapEventInit!");
status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
@@ -1062,5 +1074,58 @@ void RTABMapApp::handleEvent(UEvent * event)
UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
}
}
if(event->getClassName().compare("PostRenderEvent") == 0)
{
LOGI("Received PostRenderEvent!");
const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getStats();
int nodes = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
int words = (int)uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
float updateTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
int loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
// Call JAVA callback with some stats
UINFO("Send statistics to GUI");
bool success = false;
if(jvm && RTABMapActivity)
{
JNIEnv *env = 0;
jint rs = jvm->AttachCurrentThread(&env, NULL);
if(rs == JNI_OK && env)
{
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIFI)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
nodes,
words,
totalPoints_,
totalPolygons_,
updateTime,
loopClosureId,
databaseMemoryUsed,
inliers,
featuresExtracted,
hypothesis,
lastDrawnCloudsCount_);
success = true;
}
}
}
jvm->DetachCurrentThread();
}
if(!success)
{
UERROR("Failed to call RTABMapActivity::updateStatsCallback");
}
}
}

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

@@ -119,6 +119,7 @@ class RTABMapApp : public UEventsHandler {
void setLocalizationMode(bool enabled);
void setTrajectoryMode(bool enabled);
void setGraphOptimization(bool enabled);
void setNodesFiltering(bool enabled);
void setGraphVisible(bool visible);
void setAutoExposure(bool enabled);
void setFullResolution(bool enabled);
@@ -146,6 +147,7 @@ class RTABMapApp : public UEventsHandler {
bool odomCloudShown_;
bool graphOptimization_;
bool nodesFiltering_;
bool localizationMode_;
bool trajectoryMode_;
bool autoExposure_;

6
app/android/jni/jni_interface.cpp
View File

@@ -156,6 +156,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setGraphOptimization(
return app.setGraphOptimization(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
JNIEnv*, jobject, bool enabled)
{
return app.setNodesFiltering(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
JNIEnv*, jobject, bool visible)
{

2
app/android/jni/scene.cpp
View File

@@ -335,7 +335,7 @@ int Scene::Render() {
{
for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
{
if(mapRendering_ || iter->first < 0)
if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
{
++cloudDrawn;
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);

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

@@ -48,6 +48,7 @@
<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/graph_optimization" android:checked="true" android:title="Optimized Graph" />
<item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
<item android:id="@+id/update_rate" android:checkable="false" android:title="Map Update Rate..." />
<item android:id="@+id/time_threshold" android:checkable="false" android:title="Time Threshold..." />
<item android:id="@+id/loop_threshold" android:checkable="false" android:title="Loop Closure Threshold..." />

2
app/android/res/values/strings.xml
View File

@@ -9,7 +9,7 @@
<string name="third_person">Third</string>
<string name="top_down">Top</string>
<string name="start">Start</string>
<string name="nodes">"Nodes: "</string>
<string name="nodes">"Nodes (WM): "</string>
<string name="points">"Number of points: "</string>
<string name="update_time">"Update time (ms): "</string>
<string name="loop_closure">"Loop closure ID: "</string>

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

@@ -581,7 +581,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
@Override
public boolean accept(File dir, String filename) {
File sel = new File(dir, filename);
return filename.endsWith(".db") || sel.isDirectory();
return filename.endsWith(".db");
}
};
@@ -735,6 +735,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
item.setChecked(!item.isChecked());
RTABMapLib.setGraphOptimization(item.isChecked());
}
else if(itemId == R.id.nodes_filtering)
{
item.setChecked(!item.isChecked());
RTABMapLib.setNodesFiltering(item.isChecked());
}
else if(itemId == R.id.graph_visible)
{
item.setChecked(!item.isChecked());
@@ -1001,6 +1006,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";
final boolean isOBJ = itemId == R.id.export_obj;
final int polygons = Integer.parseInt(((TextView)findViewById(R.id.polygons)).getText().toString());
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle(String.format("File Name (*%s):", extension));
final EditText input = new EditText(this);
@@ -1027,7 +1034,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mItemExport.setEnabled(false);
mProgressDialog.setTitle("Exporting");
mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
if(polygons > 1000000)
{
mProgressDialog.setMessage(String.format(
"Please wait while exporting \"%s\"...\n"
+ "Tip: With more than 1M polygons, to reduce exporting time and file size, consider:\n"
+ " a) increasing triangle size (Rendering options)\n"
+ " b) decreasing maximum camera depth (Rendering options)\n"
+ " c) activate Nodes Filtering (Mapping options)\n"
+ "then save/open to refresh the meshes.", fileName+extension));
}
else
{
mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
}
mProgressDialog.show();
Thread exportThread = new Thread(new Runnable() {

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

@@ -50,6 +50,7 @@ public class RTABMapLib
public static native void setLocalizationMode(boolean enabled);
public static native void setTrajectoryMode(boolean enabled);
public static native void setGraphOptimization(boolean enabled);
public static native void setNodesFiltering(boolean enabled);
public static native void setGraphVisible(boolean visible);
public static native void setAutoExposure(boolean enabled);
public static native void setFullResolution(boolean enabled);