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

Tango: added mesh smoothing, show file size on Open, default timestamp filename on save/export

Browse Source
This commit is contained in:
matlabbe
2016-11-04 16:39:52 -04:00
parent c8d5a446ea
commit 7fd3d9f331
9 changed files with 183 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -29,8 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "util.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/util2d.h"
#include <tango_client_api.h>
namespace rtabmap {
@@ -613,6 +613,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
}
}
}
if(oi)
{
scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();

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

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "RTABMapApp.h"
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
@@ -137,6 +138,7 @@ RTABMapApp::RTABMapApp() :
clearSceneOnNextRender_(false),
filterPolygonsOnNextRender_(false),
gainCompensationOnNextRender_(0),
bilateralFilteringOnNextRender_(false),
cameraJustInitialized_(false),
totalPoints_(0),
totalPolygons_(0),
@@ -324,6 +326,49 @@ private:
rtabmap::Statistics stats_;
};
// OpenGL thread
bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
{
UTimer t;
// reconstruct depth image
cv::Mat depth = cv::Mat::zeros(mesh.height, mesh.width, CV_32FC1);
rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse();
for(unsigned int i=0; i<mesh.denseToOrganizedIndices.size(); ++i)
{
// FastBilateralFilter works in camera frame
pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(i), localTransformInv);
depth.at<float>(mesh.denseToOrganizedIndices[i]) = pt.z;
}
depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOrganized = rtabmap::util3d::cloudFromDepthRGB(mesh.texture.rows>1?mesh.texture:rtabmap::uncompressImage(mesh.texture), depth, mesh.cameraModel, 1, maxCloudDepth_);
cloudOrganized = rtabmap::util3d::transformPointCloud(cloudOrganized, mesh.cameraModel.localTransform());
//reconstruct the mesh with smoothed surfaces
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloudOrganized, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
// filter NaN points
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloudOrganized, polygons, *outputCloud, outputPolygons);
LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
if(outputPolygons.size())
{
mesh.cloud = outputCloud;
mesh.polygons = outputPolygons;
mesh.denseToOrganizedIndices = denseToOrganizedIndices;
}
else
{
LOGE("smoothMesh() Failed to smooth surface %d", id);
return false;
}
return true;
}
// OpenGL thread
int RTABMapApp::Render()
{
@@ -368,7 +413,7 @@ int RTABMapApp::Render()
{
cv::Mat compressed = iter->second.texture;
iter->second.texture = rtabmap::uncompressImage(iter->second.texture);
main_scene_.addMesh(iter->first, iter->second, iter->second.pose);
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.setCloudVisible(iter->first, iter->second.visible);
iter->second.texture = compressed;
}
@@ -484,7 +529,7 @@ int RTABMapApp::Render()
main_scene_.setCloudVisible(id, true);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.pose = iter->second;
meshIter->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
meshIter->second.visible = true;
}
else if(uContains(bufferedSensorData, id))
@@ -514,7 +559,7 @@ int RTABMapApp::Render()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputCloud, outputPolygons);
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*output, polygons, *outputCloud, outputPolygons);
LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
@@ -529,9 +574,10 @@ int RTABMapApp::Render()
inserted.first->second.width = cloud->width;
inserted.first->second.height = cloud->height;
inserted.first->second.polygons = outputPolygons;
inserted.first->second.pose = iter->second;
inserted.first->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
inserted.first->second.visible = true;
inserted.first->second.texture = data.imageRaw();
inserted.first->second.cameraModel = data.cameraModels()[0];
main_scene_.addMesh(id, inserted.first->second, iter->second);
@@ -582,7 +628,7 @@ int RTABMapApp::Render()
}
else
{
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_);
//just process the last one
if(!odomEvent.pose().isNull())
@@ -660,14 +706,44 @@ int RTABMapApp::Render()
compensator.apply(iter->first, iter->second.texture);
}
}
main_scene_.updateMesh(iter->first, iter->second);
// If we do bilateral filtering, do it right now as the texture is uncompressed
if((notifyDataLoaded || bilateralFilteringOnNextRender_) &&
iter->second.cloud->size() && smoothMesh(iter->first, iter->second))
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
}
else
{
main_scene_.updateMesh(iter->first, iter->second);
}
iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
}
bilateralFilteringOnNextRender_ = false;
}
gainCompensationOnNextRender_ = 0;
notifyDataLoaded = true;
}
if(bilateralFilteringOnNextRender_)
{
LOGI("Bilateral filtering...");
bilateralFilteringOnNextRender_ = false;
boost::mutex::scoped_lock lock(meshesMutex_);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(iter->second.cloud->size())
{
if(smoothMesh(iter->first, iter->second))
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
}
}
}
notifyDataLoaded = true;
}
if(filterPolygonsOnNextRender_)
{
LOGI("Polygon filtering...");
@@ -1122,7 +1198,7 @@ int RTABMapApp::postProcessing(int approach)
LOGE("g2o not available!");
}
}
else if(approach!=4 && approach!=5)
else if(approach!=4 && approach!=5 && approach != 7)
{
// simple graph optmimization
rtabmap_->getGraph(poses, links, true, true);
@@ -1139,26 +1215,31 @@ int RTABMapApp::postProcessing(int approach)
rtabmap_->setOptimizedPoses(poses);
}
else if(approach!=4 && approach!=5)
else if(approach!=4 && approach!=5 && approach != 7)
{
returnedValue = -1;
}
if(returnedValue >=0)
{
boost::mutex::scoped_lock lock(renderingMutex_);
// filter polygons
if(approach == 4)
if(approach == -1 && approach == 4)
{
boost::mutex::scoped_lock lock(renderingMutex_);
filterPolygonsOnNextRender_ = true;
}
// gain compensation
if(approach == -1 || approach == 5 || approach == 6)
{
boost::mutex::scoped_lock lock(renderingMutex_);
gainCompensationOnNextRender_ = approach == 6 ? 2 : 1; // 2 = full, 1 = fast
}
// bilateral filtering
if(approach == -1 && approach == 7)
{
bilateralFilteringOnNextRender_ = true;
}
}
}

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

@@ -138,6 +138,7 @@ class RTABMapApp : public UEventsHandler {
private:
rtabmap::ParametersMap getRtabmapParameters();
bool smoothMesh(int id, Mesh & mesh);
private:
rtabmap::CameraTango * camera_;
@@ -164,6 +165,7 @@ class RTABMapApp : public UEventsHandler {
bool clearSceneOnNextRender_;
bool filterPolygonsOnNextRender_;
int gainCompensationOnNextRender_;
bool bilateralFilteringOnNextRender_;
bool cameraJustInitialized_;
int totalPoints_;
int totalPolygons_;

5
app/android/jni/util.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <android/log.h>
#include <rtabmap/utilite/UEventsHandler.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/CameraModel.h>
#include <tango-gl/util.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
@@ -42,6 +43,7 @@ class LogHandler : public UEventsHandler
public:
LogHandler()
{
ULogger::setLevel(ULogger::kWarning);
ULogger::setEventLevel(ULogger::kWarning);
ULogger::setPrintThreadId(true);
@@ -142,9 +144,10 @@ struct Mesh
std::vector<int> denseToOrganizedIndices; // should be set if cloud is dense, used for texturing
unsigned int width; // width of the organized cloud
unsigned int height; // height of the organized cloud
rtabmap::Transform pose;
rtabmap::Transform pose; // in rtabmap coordinates
bool visible;
cv::Mat texture;
rtabmap::CameraModel cameraModel;
};
#endif /* UTIL_H_ */

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

@@ -16,6 +16,7 @@
<item android:id="@+id/icp_refining" android:title="ICP Refining" />
<item android:id="@+id/gain_compensation_fast" android:title="Adjust Colors (Fast)" />
<item android:id="@+id/gain_compensation_full" android:title="Adjust Colors (Full)" />
<item android:id="@+id/bilateral_filtering" android:title="Mesh Smoothing" />
<item android:id="@+id/sba" android:title="Bundle Adjustement" />
<item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
</menu>

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

@@ -2,6 +2,8 @@ package com.introlab.rtabmap;
import java.io.File;
import java.io.FilenameFilter;
import java.text.SimpleDateFormat;
import java.util.Date;
import android.app.Activity;
import android.app.AlertDialog;
@@ -106,6 +108,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private LinearLayout mLayoutDebug;
private int mTotalLoopClosures = 0;
private boolean mMapIsEmpty = false;
private Toast mToast = null;
@@ -676,17 +679,18 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mPauseFirstTime = false;
mToast.makeText(getActivity(), String.format("Tip: Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
}
mMapIsEmpty = false;
}
else
{
RTABMapLib.setPausedMapping(false);
((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode.isChecked()?"Recording":"Mapping");
if(mItemDataRecorderMode.isChecked())
{
mToast.makeText(getActivity(), String.format("Data Recorder Mode: no map is created, only raw data is recorded."), mToast.LENGTH_LONG).show();
}
else
else if(!mMapIsEmpty)
{
((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
}
}
@@ -705,7 +709,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
if(loopDetected >= 0)
{
mTotalLoopClosures+=loopDetected;
mProgressDialog.setMessage(String.format("Optimization done! Adjusting colors..."));
mProgressDialog.setMessage(String.format("Optimization done! Increasing visual appeal..."));
}
else if(loopDetected < 0)
{
@@ -817,6 +821,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mProgressDialog.show();
RTABMapLib.postProcessing(6);
}
else if (itemId == R.id.bilateral_filtering)
{
mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Mesh smoothing..."));
mProgressDialog.show();
RTABMapLib.postProcessing(7);
}
else if (itemId == R.id.sba)
{
mProgressDialog.setTitle("Post-Processing");
@@ -1094,7 +1105,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("RTAB-Map Database Name (*.db):");
final EditText input = new EditText(this);
input.setInputType(InputType.TYPE_CLASS_TEXT);
input.setInputType(InputType.TYPE_CLASS_TEXT);
String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
input.setText(timeStamp);
input.setSelectAllOnFocus(true);
input.selectAll();
builder.setView(input);
builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
@Override
@@ -1195,6 +1210,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mOpenedDatabasePath = "";
RTABMapLib.openDatabase(mTempDatabasePath);
}
mMapIsEmpty = true;
}
else if(itemId == R.id.data_recorder)
{
@@ -1258,6 +1274,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
final EditText input = new EditText(this);
input.setInputType(InputType.TYPE_CLASS_TEXT);
builder.setView(input);
String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
input.setText(timeStamp);
input.setSelectAllOnFocus(true);
input.selectAll();
builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which)
@@ -1377,9 +1397,16 @@ public class RTABMapActivity extends Activity implements OnClickListener {
final String[] files = loadFileList(mWorkingDirectory);
if(files.length > 0)
{
String[] filesWithSize = files;
for(int i = 0; i<filesWithSize.length; ++i)
{
File filePath = new File(mWorkingDirectory+files[i]);
long mb = filePath.length()/(1024*1024);
filesWithSize[i] += " ("+mb+" MB)";
}
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Choose your file");
builder.setItems(files, new DialogInterface.OnClickListener() {
builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, int which) {
mOpenedDatabasePath = mWorkingDirectory + files[which];

5
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -100,6 +100,11 @@ std::vector<int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons);
std::vector<int> RTABMAP_EXP filterNaNPointsFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons);
std::vector<pcl::Vertices> RTABMAP_EXP filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,

2
corelib/src/util2d.cpp
View File

@@ -1845,7 +1845,9 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
v = 0.0f;
}
if(depth.type()==CV_32FC1)
{
output.at<float>(y,x) = v;
}
else
{
v*=1000.0f;

43
corelib/src/util3d_surface.cpp
View File

@@ -346,6 +346,49 @@ std::vector<int> filterNotUsedVerticesFromMesh(
return output;
}
std::vector<int> filterNaNPointsFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons)
{
UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
std::map<int, int> addedVertices; //<oldIndex, newIndex>
std::vector<int> output; //<oldIndex>
output.resize(cloud.size());
outputCloud.resize(cloud.size());
outputCloud.is_dense = true;
std::vector<int> organizedToDense(cloud.size(), -1);
int oi = 0;
for(unsigned int i=0; i<cloud.size(); ++i)
{
if(pcl::isFinite(cloud.at(i)))
{
outputCloud.at(oi) = cloud.at(i);
output[oi] = i;
organizedToDense[i] = oi;
++oi;
}
}
outputCloud.resize(oi);
output.resize(oi);
// remap polygons to dense cloud
outputPolygons = polygons;
for(unsigned int i=0; i<outputPolygons.size(); ++i)
{
pcl::Vertices & v = outputPolygons[i];
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
UASSERT(organizedToDense[v.vertices[j]] >= 0);
v.vertices[j] = organizedToDense[v.vertices[j]];
}
}
return output;
}
std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
const std::vector<pcl::Vertices> & polygons,