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 Noise Filtering post-processing option, set RGBD/MaxLocalRetrieved to 0 (avoiding some high processing time peaks), fill depth holes up to 5 pixels

Browse Source
This commit is contained in:
matlabbe
2016-09-03 20:38:27 -04:00
parent 7f2c118f8d
commit 91da92346d
9 changed files with 89 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -37,8 +37,9 @@ namespace rtabmap {
#define nullptr 0
const int kVersionStringLength = 128;
const int holeSize = 1;
const int holeSize = 5;
const float maxDepthError = 0.10;
const int scanDownsampling = 10;
// Callbacks
void onPointCloudAvailableRouter(void* context, const TangoXYZij* xyz_ij)
@@ -558,7 +559,6 @@ SensorData CameraTango::captureImage(CameraInfo * info)
poseDepth.setNull();
}
int scanDownsampling = 10;
cv::Mat scan;
if(!poseDepth.isNull() && !poseColor.isNull())
{

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

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/io/obj_io.h>
const int kVersionStringLength = 128;
const int minPolygonClusterSize = 100;
static JavaVM *jvm;
static jobject RTABMapActivity = 0;
@@ -77,6 +78,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
@@ -114,6 +116,7 @@ RTABMapApp::RTABMapApp() :
meshTrianglePix_(1),
meshAngleToleranceDeg_(15.0),
clearSceneOnNextRender_(false),
filterPolygonsOnNextRender_(false),
totalPoints_(0),
totalPolygons_(0),
lastDrawnCloudsCount_(0),
@@ -559,6 +562,42 @@ int RTABMapApp::Render()
}
}
if(filterPolygonsOnNextRender_)
{
filterPolygonsOnNextRender_ = false;
boost::mutex::scoped_lock lock(meshesMutex_);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(iter->second.polygons.size())
{
// filter polygons
std::vector<std::set<int> > neighbors;
std::vector<std::set<int> > vertexToPolygons;
rtabmap::util3d::createPolygonIndexes(
iter->second.polygons,
iter->second.cloud->size(),
neighbors,
vertexToPolygons);
std::list<std::list<int> > clusters = rtabmap::util3d::clusterPolygons(
neighbors,
minPolygonClusterSize);
std::vector<pcl::Vertices> filteredPolygons(iter->second.polygons.size());
int oi=0;
for(std::list<std::list<int> >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter)
{
for(std::list<int>::iterator kter=jter->begin(); kter!=jter->end(); ++kter)
{
filteredPolygons[oi++] = iter->second.polygons.at(*kter);
}
}
filteredPolygons.resize(oi);
iter->second.polygons = filteredPolygons;
main_scene_.updateCloudPolygons(iter->first, iter->second.polygons);
}
}
notifyDataLoaded = true;
}
UTimer fpsTime;
lastDrawnCloudsCount_ = main_scene_.Render();
renderingFPS_ = 1.0/fpsTime.elapsed();
@@ -1004,6 +1043,13 @@ int RTABMapApp::postProcessing(int approach)
returnedValue = -1;
}
}
// filter polygons
if(approach == 4)
{
filterPolygonsOnNextRender_ = true;
}
return returnedValue;
}

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

@@ -160,6 +160,7 @@ class RTABMapApp : public UEventsHandler {
bool clearSceneOnNextRender_;
bool filterPolygonsOnNextRender_;
int totalPoints_;
int totalPolygons_;
int lastDrawnCloudsCount_;

36
app/android/jni/point_cloud_drawable.cpp
View File

@@ -138,21 +138,7 @@ PointCloudDrawable::PointCloudDrawable(
nPoints_ = cloud->size();
if(polygons.size())
{
int polygonSize = polygons[0].vertices.size();
UASSERT(polygonSize == 3);
polygons_.resize(polygons.size() * polygonSize);
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
UASSERT((int)polygons[i].vertices.size() == polygonSize);
for(int j=0; j<polygonSize; ++j)
{
polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
}
}
}
updatePolygons(polygons);
}
PointCloudDrawable::~PointCloudDrawable()
@@ -173,6 +159,26 @@ PointCloudDrawable::~PointCloudDrawable()
}
}
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
{
polygons_.clear();
if(polygons.size())
{
int polygonSize = polygons[0].vertices.size();
UASSERT(polygonSize == 3);
polygons_.resize(polygons.size() * polygonSize);
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
UASSERT((int)polygons[i].vertices.size() == polygonSize);
for(int j=0; j<polygonSize; ++j)
{
polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
}
}
}
}
void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
{
UASSERT(!pose.isNull());

1
app/android/jni/point_cloud_drawable.h
View File

@@ -49,6 +49,7 @@ class PointCloudDrawable {
const cv::Mat & image = cv::Mat());
virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons);
void setPose(const rtabmap::Transform & pose);
void setVisible(bool visible) {visible_=visible;}
rtabmap::Transform getPose() const {return glmToTransform(pose_);}

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

@@ -471,3 +471,12 @@ std::set<int> Scene::getAddedClouds() const
{
return uKeysSet(pointClouds_);
}
void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons)
{
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
iter->second->updatePolygons(polygons);
}
}

1
app/android/jni/scene.h
View File

@@ -108,6 +108,7 @@ class Scene {
void setCloudVisible(int id, bool visible);
bool hasCloud(int id) const;
std::set<int> getAddedClouds() const;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
void setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}

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

@@ -15,6 +15,7 @@
<item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
<item android:id="@+id/icp_refining" android:title="ICP Refining" />
<item android:id="@+id/sba" android:title="Bundle Adjustement" />
<item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
</menu>
</item>
</menu>

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

@@ -765,6 +765,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
});
workingThread.start();
}
else if (itemId == R.id.polygons_filtering)
{
mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Noise filtering..."));
mProgressDialog.show();
RTABMapLib.postProcessing(4);
}
else if (itemId == R.id.sba)
{
mProgressDialog.setTitle("Post-Processing");