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

Tango: Added ICP refining option, mesh vs texture vs point cloud options, show/hide grid option, drift correction option

Browse Source
This commit is contained in:
matlabbe
2016-09-02 22:37:12 -04:00
parent 4bd9e4f847
commit e5074afdeb
17 changed files with 350 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/CameraTango.cpp
+16 -3
View File
@@ -37,7 +37,7 @@ namespace rtabmap {
#define nullptr 0
const int kVersionStringLength = 128;
const int holeSize = 10;
const int holeSize = 1;
const float maxDepthError = 0.10;
// Callbacks
@@ -558,6 +558,8 @@ SensorData CameraTango::captureImage(CameraInfo * info)
poseDepth.setNull();
}
int scanDownsampling = 10;
cv::Mat scan;
if(!poseDepth.isNull() && !poseColor.isNull())
{
// The Color Camera frame at timestamp t0 with respect to Depth
@@ -568,11 +570,18 @@ SensorData CameraTango::captureImage(CameraInfo * info)
int pixelsSet = 0;
depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
CameraModel depthModel = model_.scaled(1.0f/8.0f);
std::vector<cv::Point3f> scanData(cloud.total());
int oi=0;
for(unsigned int i=0; i<cloud.total(); ++i)
{
cv::Vec3f & p = cloud.at<cv::Vec3f>(i);
float * p = cloud.ptr<float>(0,i);
cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
if(pt.z > 0.0f && i%scanDownsampling == 0)
{
scanData.at(oi++) = pt;
}
int pixel_x, pixel_y;
// get the coordinate on image plane.
pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
@@ -591,6 +600,10 @@ SensorData CameraTango::captureImage(CameraInfo * info)
}
}
}
if(oi)
{
scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
}
LOGI("pixels depth set= %d", pixelsSet);
}
else
@@ -607,7 +620,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
//Rotate in RTAB-Map's coordinate
Transform odom = rtabmap_world_T_opengl_world * poseColorOpenGL * depth_camera_T_opengl_camera * model.localTransform().inverse();
data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
data.setGroundTruth(odom);
}
else
app/android/jni/RTABMapApp.cpp
+77 -39
View File
@@ -76,13 +76,23 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
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(), uBool2Str(!localizationMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1"));
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")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), std::string("6")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
return parameters;
}
@@ -95,13 +105,14 @@ RTABMapApp::RTABMapApp() :
odomCloudShown_(true),
graphOptimization_(true),
nodesFiltering_(false),
driftCorrection_(false),
localizationMode_(false),
trajectoryMode_(false),
autoExposure_(false),
fullResolution_(false),
maxCloudDepth_(0.0),
meshTrianglePix_(1),
meshAngleToleranceDeg_(10.0),
meshAngleToleranceDeg_(15.0),
clearSceneOnNextRender_(false),
totalPoints_(0),
totalPolygons_(0),
@@ -109,7 +120,6 @@ RTABMapApp::RTABMapApp() :
renderingFPS_(0.0f)
{
}
RTABMapApp::~RTABMapApp() {
@@ -422,7 +432,7 @@ int RTABMapApp::Render()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get());
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get());
if(cloud->size() && indices->size())
{
@@ -525,7 +535,7 @@ int RTABMapApp::Render()
if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), 1, maxCloudDepth_);
if(cloud->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
@@ -599,9 +609,9 @@ void RTABMapApp::setOdomCloudShown(bool shown)
odomCloudShown_ = shown;
main_scene_.setTraceVisible(shown);
}
void RTABMapApp::setMeshRendering(bool enabled)
void RTABMapApp::setMeshRendering(bool enabled, bool withTexture)
{
main_scene_.setMeshRendering(enabled);
main_scene_.setMeshRendering(enabled, withTexture);
}
void RTABMapApp::setLocalizationMode(bool enabled)
{
@@ -643,9 +653,22 @@ void RTABMapApp::setNodesFiltering(bool enabled)
nodesFiltering_ = enabled;
setGraphOptimization(graphOptimization_); // this will resend the graph if paused
}
void RTABMapApp::setDriftCorrection(bool enabled)
{
driftCorrection_ = enabled;
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0")));
this->post(new rtabmap::ParamEvent(parameters));
}
void RTABMapApp::setGraphVisible(bool visible)
{
main_scene_.setGraphVisible(visible);
main_scene_.setTraceVisible(visible);
}
void RTABMapApp::setGridVisible(bool visible)
{
main_scene_.setGridVisible(visible);
}
void RTABMapApp::setAutoExposure(bool enabled)
@@ -907,49 +930,64 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
int RTABMapApp::postProcessing(int approach)
{
LOGI("postProcessing(%d)", approach);
int returnedValue = 0;
if(rtabmap_)
{
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
if(approach == 2 || approach == 0)
{
if(approach == 2)
{
// detect more loop closures
returnedValue = rtabmap_->detectMoreLoopClosures();
}
if(returnedValue >= 0)
// detect more loop closures
if(approach == -1 || approach == 2)
{
// detect more loop closures
returnedValue = rtabmap_->detectMoreLoopClosures(0.5f, M_PI/6.0f, approach == -1?3:1);
}
// ICP refining
if(returnedValue >=0 && ((approach == -1 && !driftCorrection_) || approach == 3))
{
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string("1"))); // ICP
rtabmap_->parseParameters(parameters);
int r = rtabmap_->refineLinks();
if(approach == 3 )
{
returnedValue = r;
}
// reset back default registration (visual)
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0"))); // Visual
rtabmap_->parseParameters(parameters);
}
// graph optimization
if(returnedValue >=0)
{
if (approach == 1)
{
if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
{
std::map<int, rtabmap::Signature> signatures;
rtabmap_->getGraph(poses, links, false, true, &signatures);
rtabmap::ParametersMap param;
param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
delete sba;
}
else
{
LOGE("g2o not available!");
}
}
else
{
// simple graph optmimization
rtabmap_->getGraph(poses, links, true, true);
}
}
else if (approach == 1)
{
if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
{
std::map<int, rtabmap::Signature> signatures;
rtabmap_->getGraph(poses, links, false, true, &signatures);
rtabmap::ParametersMap param;
param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
delete sba;
}
else
{
LOGE("g2o not available!");
}
}
else
{
LOGE("Invalid approach %d (should be 0 (graph optimization), 1 (sba) or 2 (detect more loop closures))", approach);
returnedValue = -1;
}
if(poses.size())
{
app/android/jni/RTABMapApp.h
+4 -1
View File
@@ -112,12 +112,14 @@ class RTABMapApp : public UEventsHandler {
void setPausedMapping(bool paused);
void setMapCloudShown(bool shown);
void setOdomCloudShown(bool shown);
void setMeshRendering(bool enabled);
void setMeshRendering(bool enabled, bool withTexture);
void setLocalizationMode(bool enabled);
void setTrajectoryMode(bool enabled);
void setGraphOptimization(bool enabled);
void setNodesFiltering(bool enabled);
void setDriftCorrection(bool enabled);
void setGraphVisible(bool visible);
void setGridVisible(bool visible);
void setAutoExposure(bool enabled);
void setFullResolution(bool enabled);
void setMaxCloudDepth(float value);
@@ -145,6 +147,7 @@ class RTABMapApp : public UEventsHandler {
bool odomCloudShown_;
bool graphOptimization_;
bool nodesFiltering_;
bool driftCorrection_;
bool localizationMode_;
bool trajectoryMode_;
bool autoExposure_;
app/android/jni/jni_interface.cpp
+14 -2
View File
@@ -133,9 +133,9 @@ Java_com_introlab_rtabmap_RTABMapLib_setOdomCloudShown(
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
JNIEnv*, jobject, bool enabled)
JNIEnv*, jobject, bool enabled, bool withTexture)
{
return app.setMeshRendering(enabled);
return app.setMeshRendering(enabled, withTexture);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
@@ -162,12 +162,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
return app.setNodesFiltering(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setDriftCorrection(
JNIEnv*, jobject, bool enabled)
{
return app.setDriftCorrection(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
JNIEnv*, jobject, bool visible)
{
return app.setGraphVisible(visible);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
JNIEnv*, jobject, bool visible)
{
return app.setGridVisible(visible);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
JNIEnv*, jobject, bool enabled)
{
app/android/jni/point_cloud_drawable.cpp
+2 -2
View File
@@ -180,11 +180,11 @@ void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
pose_ = glmFromTransform(pose);
}
void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize) {
void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize, bool textureRendering) {
if(vertex_buffers_ && nPoints_ && visible_)
{
if(meshRendering && textures_)
if(meshRendering && textureRendering && textures_)
{
glUseProgram(texture_shader_program_);
app/android/jni/point_cloud_drawable.h
+1 -1
View File
@@ -60,7 +60,7 @@ class PointCloudDrawable {
// @param view_mat: view matrix from current render camera.
// @param model_mat: model matrix for this point cloud frame.
// @param vertices: all vertices in this point cloud frame.
void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f);
void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f, bool textureRendering = false);
private:
// Vertex buffer of the point cloud geometry.
app/android/jni/scene.cpp
+14 -5
View File
@@ -109,6 +109,7 @@ Scene::Scene() :
trace_(0),
graph_(0),
graphVisible_(true),
gridVisible_(true),
traceVisible_(true),
currentPose_(0),
cloud_shader_program_(0),
@@ -116,6 +117,7 @@ Scene::Scene() :
graph_shader_program_(0),
mapRendering_(true),
meshRendering_(true),
meshRenderingTexture_(true),
pointSize_(3.0f) {}
Scene::~Scene() {DeleteResources();}
@@ -275,9 +277,11 @@ int Scene::Render() {
}
}
grid_->Render(gesture_camera_->GetProjectionMatrix(),
gesture_camera_->GetViewMatrix());
if(gridVisible_)
{
grid_->Render(gesture_camera_->GetProjectionMatrix(),
gesture_camera_->GetViewMatrix());
}
bool frustumCulling = true;
int cloudDrawn=0;
@@ -327,7 +331,7 @@ int Scene::Render() {
for(unsigned int i=0; i<indices->size(); ++i)
{
++cloudDrawn;
pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
}
}
}
@@ -338,7 +342,7 @@ int Scene::Render() {
if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
{
++cloudDrawn;
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
}
}
}
@@ -400,6 +404,11 @@ void Scene::setGraphVisible(bool visible)
graphVisible_ = visible;
}
void Scene::setGridVisible(bool visible)
{
gridVisible_ = visible;
}
void Scene::setTraceVisible(bool visible)
{
traceVisible_ = visible;
app/android/jni/scene.h
+4 -1
View File
@@ -94,6 +94,7 @@ class Scene {
const std::multimap<int, rtabmap::Link> & links);
void setGraphVisible(bool visible);
void setGridVisible(bool visible);
void setTraceVisible(bool visible);
void addCloud(
@@ -109,7 +110,7 @@ class Scene {
std::set<int> getAddedClouds() const;
void setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
void setPointSize(float size) {pointSize_ = size;}
private:
@@ -129,6 +130,7 @@ class Scene {
tango_gl::Trace* trace_;
GraphDrawable * graph_;
bool graphVisible_;
bool gridVisible_;
bool traceVisible_;
std::map<int, PointCloudDrawable*> pointClouds_;
@@ -142,6 +144,7 @@ class Scene {
bool mapRendering_;
bool meshRendering_;
bool meshRenderingTexture_;
float pointSize_;
};
app/android/res/menu/optionmenu.xml
+23 -9
View File
@@ -8,11 +8,15 @@
<group android:id="@+id/group_actions">
<item android:id="@+id/post_processing" android:title="Post-Processing...">
<menu>
<group android:id="@+id/group_post_processing">
<item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
<item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
<item android:id="@+id/sba" android:title="Bundle Adjustement" />
</group>
<item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
<item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
<menu >
<item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
<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" />
</menu>
</item>
</menu>
</item>
<item android:id="@+id/open" android:title="Open"/>
@@ -31,14 +35,23 @@
<menu >
<group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
<item android:id="@+id/debug" android:checked="false" android:title="Debug" />
<item android:id="@+id/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
<item android:id="@+id/menu_rendering" android:checkable="false" android:title="Mesh Rendering..." >
<menu>
<group android:checkableBehavior="single">
<item android:id="@+id/point_cloud" android:title="Point Cloud" />
<item android:id="@+id/mesh" android:title="Mesh" />
<item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
</group>
<item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
<item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
<item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />
</menu>
</item>
<item android:id="@+id/map_shown" android:checked="true" android:title="Map 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/grid_visible" android:checked="true" android:title="Grid Visible" />
<item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
<item android:id="@+id/max_depth" android:checkable="false" android:title="Cloud/Mesh Max Depth..." />
<item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
<item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
</group>
</menu>
</item>
@@ -49,6 +62,7 @@
<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/drift_correction" android:checked="false" android:title="Drift Correction" />
<item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
<item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
<menu >
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+76 -4
View File
@@ -74,6 +74,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private MenuItem mItemExport;
private MenuItem mItemLocalizationMode;
private MenuItem mItemTrajectoryMode;
private MenuItem mItemRenderingPointCloud;
private MenuItem mItemRenderingMesh;
private MenuItem mItemRenderingTextureMesh;
private String mOpenedDatabasePath = "";
private String mTempDatabasePath = "";
@@ -81,7 +85,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private String mWorkingDirectory = "";
private int mMaxDepthIndex = 5;
private int mMeshAngleToleranceIndex = 1;
private int mMeshAngleToleranceIndex = 2;
private int mMeshTriangleSizeIndex = 0;
private int mParamUpdateRateHzIndex = 1;
@@ -316,6 +320,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mItemExport = menu.findItem(R.id.export);
mItemLocalizationMode = menu.findItem(R.id.localization_mode);
mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode);
mItemRenderingPointCloud = menu.findItem(R.id.point_cloud);
mItemRenderingMesh = menu.findItem(R.id.mesh);
mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh);
mItemSave.setEnabled(false);
mItemExport.setEnabled(false);
mItemOpen.setEnabled(false);
@@ -652,6 +659,33 @@ public class RTABMapActivity extends Activity implements OnClickListener {
((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
}
}
else if (itemId == R.id.post_processing_standard)
{
mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Please wait while optimizing..."));
mProgressDialog.show();
Thread workingThread = new Thread(new Runnable() {
public void run() {
final int loopDetected = RTABMapLib.postProcessing(-1);
runOnUiThread(new Runnable() {
public void run() {
mProgressDialog.dismiss();
if(loopDetected >= 0)
{
mTotalLoopClosures+=loopDetected;
mToast.makeText(getActivity(), String.format("Optimization done!"), mToast.LENGTH_SHORT).show();
}
else if(loopDetected < 0)
{
mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_SHORT).show();
}
}
});
}
});
workingThread.start();
}
else if (itemId == R.id.detect_more_loop_closures)
{
mProgressDialog.setTitle("Post-Processing");
@@ -679,6 +713,32 @@ public class RTABMapActivity extends Activity implements OnClickListener {
});
workingThread.start();
}
else if (itemId == R.id.icp_refining)
{
mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Please wait while refining links..."));
mProgressDialog.show();
Thread workingThread = new Thread(new Runnable() {
public void run() {
final int linksRefined = RTABMapLib.postProcessing(3);
runOnUiThread(new Runnable() {
public void run() {
mProgressDialog.dismiss();
if(linksRefined >= 0)
{
mToast.makeText(getActivity(), String.format("Refining done! %d link(s) refined.", linksRefined), mToast.LENGTH_SHORT).show();
}
else if(linksRefined < 0)
{
mToast.makeText(getActivity(), String.format("Refining failed!"), mToast.LENGTH_SHORT).show();
}
}
});
}
});
workingThread.start();
}
else if (itemId == R.id.global_graph_optimization)
{
mProgressDialog.setTitle("Post-Processing");
@@ -743,10 +803,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mLayoutDebug.setVisibility(LinearLayout.VISIBLE);
}
}
else if(itemId == R.id.mesh_rendering)
else if(itemId == R.id.mesh || itemId == R.id.texture_mesh || itemId == R.id.point_cloud)
{
item.setChecked(!item.isChecked());
RTABMapLib.setMeshRendering(item.isChecked());
item.setChecked(true);
RTABMapLib.setMeshRendering(
mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(),
mItemRenderingTextureMesh.isChecked());
}
else if(itemId == R.id.map_shown)
{
@@ -778,11 +840,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
item.setChecked(!item.isChecked());
RTABMapLib.setNodesFiltering(item.isChecked());
}
else if(itemId == R.id.drift_correction)
{
item.setChecked(!item.isChecked());
RTABMapLib.setDriftCorrection(item.isChecked());
}
else if(itemId == R.id.graph_visible)
{
item.setChecked(!item.isChecked());
RTABMapLib.setGraphVisible(item.isChecked());
}
else if(itemId == R.id.grid_visible)
{
item.setChecked(!item.isChecked());
RTABMapLib.setGridVisible(item.isChecked());
}
else if(itemId == R.id.auto_exposure)
{
item.setChecked(!item.isChecked());
app/android/src/com/introlab/rtabmap/RTABMapLib.java
+3 -1
View File
@@ -58,12 +58,14 @@ public class RTABMapLib
public static native void setPausedMapping(boolean paused);
public static native void setMapCloudShown(boolean shown);
public static native void setOdomCloudShown(boolean shown);
public static native void setMeshRendering(boolean enabled);
public static native void setMeshRendering(boolean enabled, boolean withTexture);
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 setDriftCorrection(boolean enabled);
public static native void setGraphVisible(boolean visible);
public static native void setGridVisible(boolean visible);
public static native void setAutoExposure(boolean enabled);
public static native void setFullResolution(boolean enabled);
public static native void setMaxCloudDepth(float value);
corelib/include/rtabmap/core/Memory.h
+1 -2
View File
@@ -93,8 +93,7 @@ public:
void emptyTrash();
void joinTrashThread();
bool addLink(const Link & link, bool addInDatabase = false);
void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
void updateLink(const Link & link, bool updateInDatabase = false);
void removeAllVirtualLinks();
void removeVirtualLinks(int signatureId);
std::map<int, int> getNeighborsId(
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -128,6 +128,7 @@ public:
bool global,
std::map<int, Signature> * signatures = 0);
int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);
int refineLinks();
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
corelib/include/rtabmap/core/util3d.h
+2 -2
View File
@@ -141,7 +141,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap(),
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
@@ -164,7 +164,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap(),
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
corelib/src/Memory.cpp
+49 -40
View File
@@ -2422,54 +2422,63 @@ bool Memory::addLink(const Link & link, bool addInDatabase)
return true;
}
void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
void Memory::updateLink(const Link & link, bool updateInDatabase)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
Signature * fromS = this->_getSignature(link.from());
Signature * toS = this->_getSignature(link.to());
if(fromS->hasLink(toId) && toS->hasLink(fromId))
if(fromS && toS)
{
Link::Type type = fromS->getLinks().at(toId).type();
fromS->removeLink(toId);
toS->removeLink(fromId);
fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance));
toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance));
if(type!=Link::kVirtualClosure)
if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
{
_linksChanged = true;
Link::Type oldType = fromS->getLinks().at(link.to()).type();
fromS->removeLink(link.to());
toS->removeLink(link.from());
fromS->addLink(link);
toS->addLink(link.inverse());
if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
{
_linksChanged = true;
}
}
else
{
UERROR("fromId=%d and toId=%d are not linked!", link.from(), link.to());
}
}
else if(!updateInDatabase)
{
if(!fromS)
{
UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
}
if(!toS)
{
UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
}
}
else if(fromS)
{
UDEBUG("Update link between %d and %d (db)", link.from(), link.to());
fromS->removeLink(link.to());
fromS->addLink(link);
_dbDriver->updateLink(link.inverse());
}
else if(toS)
{
UDEBUG("Update link between %d (db) and %d", link.from(), link.to());
toS->removeLink(link.from());
toS->addLink(link.inverse());
_dbDriver->updateLink(link);
}
else
{
UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
}
}
void Memory::updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
if(fromS->hasLink(toId) && toS->hasLink(fromId))
{
Link::Type type = fromS->getLinks().at(toId).type();
fromS->removeLink(toId);
toS->removeLink(fromId);
cv::Mat infMatrix = covariance.inv();
fromS->addLink(Link(fromId, toId, type, transform, infMatrix));
toS->addLink(Link(toId, fromId, type, transform.inverse(), infMatrix));
if(type!=Link::kVirtualClosure)
{
_linksChanged = true;
}
}
else
{
UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
UDEBUG("Update link between %d (db) and %d (db)", link.from(), link.to());
_dbDriver->updateLink(link);
_dbDriver->updateLink(link.inverse());
}
}
corelib/src/Rtabmap.cpp
+59 -5
View File
@@ -1012,7 +1012,7 @@ bool Rtabmap::process(
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
}
}
else
@@ -1034,7 +1034,7 @@ bool Rtabmap::process(
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.variance, info.variance));
if(_optimizeFromGraphEnd)
{
@@ -1058,7 +1058,7 @@ bool Rtabmap::process(
if(info.variance > 0)
{
double sqrtVar = sqrt(info.variance);
_memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrtVar, sqrtVar));
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1821,7 +1821,7 @@ bool Rtabmap::process(
UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
// segment poses by paths, only one detection per path
std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxNeighbors);
std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
@@ -1886,13 +1886,21 @@ bool Rtabmap::process(
// 2) compare locally with nearest locations by scan matching
//
UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
if( !signature->sensorData().laserScanCompressed().empty() &&
if( _proximityMaxNeighbors > 0 &&
!signature->sensorData().laserScanCompressed().empty() &&
(_memory->isIncremental() || lastProximitySpaceClosureId == 0))
{
// In localization mode, no need to check local loop
// closures if we are already localized by at least one
// local visual closure above.
// Parse again with if different (normally, maxNeighbors would be smaller than MaxGraphDepth)
if(_proximityMaxNeighbors != _proximityMaxGraphDepth)
{
nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxNeighbors);
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
}
proximitySpacePaths = (int)nearestPaths.size();
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
@@ -3412,6 +3420,52 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
return (int)loopClosuresAdded.size();
}
int Rtabmap::refineLinks()
{
if(!_rgbdSlamMode)
{
UERROR("Refining links can be done only in RGBD-SLAM mode.");
return -1;
}
std::list<Link> linksRefined;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures;
this->getGraph(poses, links, false, true, &signatures);
int i=0;
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!= links.end(); ++iter)
{
int from = iter->second.from();
int to = iter->second.to();
UASSERT(signatures.find(from) != signatures.end());
UASSERT(signatures.find(to) != signatures.end());
RegistrationInfo info;
// use signatures instead of IDs because some signatures may not be in WM
Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), iter->second.transform(), &info);
if(!t.isNull())
{
linksRefined.push_back(Link(from, to, iter->second.type(), t, info.variance, info.variance));
UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
}
}
UINFO("Total refined %d links.", (int)linksRefined.size());
if(linksRefined.size())
{
for(std::list<Link>::iterator iter=linksRefined.begin(); iter!=linksRefined.end(); ++iter)
{
_memory->updateLink(*iter, true);
}
}
return (int)linksRefined.size();
}
void Rtabmap::clearPath(int status)
{
UINFO("status=%d", status);
corelib/src/util3d.cpp
+4 -4
View File
@@ -699,7 +699,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & parameters,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -815,7 +815,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
}
cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), stereoParameters),
sensorData.stereoCameraModel(),
decimation,
maxDepth,
@@ -839,7 +839,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & parameters,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
UASSERT(!sensorData.imageRaw().empty());
@@ -961,7 +961,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
maxDepth,
minDepth,
validIndices,
parameters);
stereoParameters);
if(cloud->size())
{