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 "Adjust colors" post-processing option. Updated ICP parameters. Added "Mem/LaserScanNormalK" parameter for convenience.

Browse Source
This commit is contained in:
matlabbe
2016-09-07 17:19:21 -04:00
parent ce1acd9d44
commit 4a072b3dfc
18 changed files with 501 additions and 351 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="10"
android:versionCode="11"
android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" />

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

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/GainCompensator.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
@@ -90,10 +91,13 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
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::kIcpPointToPlane(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), 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::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.3")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
return parameters;
@@ -117,6 +121,7 @@ RTABMapApp::RTABMapApp() :
meshAngleToleranceDeg_(15.0),
clearSceneOnNextRender_(false),
filterPolygonsOnNextRender_(false),
gainCompensationOnNextRender_(false),
totalPoints_(0),
totalPolygons_(0),
lastDrawnCloudsCount_(0),
@@ -171,8 +176,6 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
{
logHandler_ = new LogHandler();
}
ULogger::setEventLevel(ULogger::kInfo);
ULogger::setPrintThreadId(true);
this->registerToEventsManager();
@@ -300,6 +303,8 @@ private:
// OpenGL thread
int RTABMapApp::Render()
{
boost::mutex::scoped_lock lock(renderingMutex_);
// should be before clearSceneOnNextRender_ in case openDatabase is called
std::list<rtabmap::Statistics> rtabmapEvents;
{
@@ -464,11 +469,11 @@ int RTABMapApp::Render()
main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
// protect createdMeshes_ used also by exportMesh() method
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = outputCloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = outputPolygons;
inserted.first->second.pose = iter->second;
inserted.first->second.texture = data.imageCompressed();
@@ -545,8 +550,7 @@ int RTABMapApp::Render()
event.data().imageRaw().cols, event.data().imageRaw().rows,
event.data().depthRaw().cols, event.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
main_scene_.addCloud(-1, cloud, std::vector<pcl::Vertices>(), opengl_world_T_rtabmap_world*event.pose());
main_scene_.setCloudVisible(-1, true);
}
else
@@ -562,6 +566,39 @@ int RTABMapApp::Render()
}
}
if(gainCompensationOnNextRender_)
{
gainCompensationOnNextRender_ = false;
rtabmap::GainCompensator compensator;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
std::map<int, pcl::IndicesPtr> indices;
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
clouds.insert(std::make_pair(iter->first, iter->second.cloud));
indices.insert(std::make_pair(iter->first, iter->second.indices));
}
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap_->getGraph(poses, links, false, true);
compensator.feed(clouds, indices, links);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudCpy(new pcl::PointCloud<pcl::PointXYZRGB>);
*cloudCpy = *iter->second.cloud;
cv::Mat imageCpy = rtabmap::uncompressImage(iter->second.texture);
if(!cloudCpy->empty())
{
compensator.apply(iter->first, cloudCpy, iter->second.indices);
if(!imageCpy.empty())
{
compensator.apply(iter->first, imageCpy);
}
}
main_scene_.updateCloudColors(iter->first, cloudCpy, imageCpy);
}
notifyDataLoaded = true;
}
if(filterPolygonsOnNextRender_)
{
filterPolygonsOnNextRender_ = false;
@@ -1021,7 +1058,7 @@ int RTABMapApp::postProcessing(int approach)
LOGE("g2o not available!");
}
}
else
else if(approach!=4 || approach!=5)
{
// simple graph optmimization
rtabmap_->getGraph(poses, links, true, true);
@@ -1038,16 +1075,27 @@ int RTABMapApp::postProcessing(int approach)
rtabmap_->setOptimizedPoses(poses);
}
else
else if(approach!=4 || approach!=5)
{
returnedValue = -1;
}
}
// filter polygons
if(approach == 4)
{
filterPolygonsOnNextRender_ = true;
if(returnedValue >=0)
{
// filter polygons
if(approach == 4)
{
boost::mutex::scoped_lock lock(renderingMutex_);
filterPolygonsOnNextRender_ = true;
}
// gain compensation
if(approach == -1 || approach == 5)
{
boost::mutex::scoped_lock lock(renderingMutex_);
gainCompensationOnNextRender_ = true;
}
}
}
return returnedValue;

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

@@ -161,6 +161,7 @@ class RTABMapApp : public UEventsHandler {
bool clearSceneOnNextRender_;
bool filterPolygonsOnNextRender_;
bool gainCompensationOnNextRender_;
int totalPoints_;
int totalPolygons_;
int lastDrawnCloudsCount_;
@@ -178,10 +179,12 @@ class RTABMapApp : public UEventsHandler {
boost::mutex meshesMutex_;
boost::mutex odomMutex_;
boost::mutex poseMutex_;
boost::mutex renderingMutex_;
struct Mesh
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices;
std::vector<pcl::Vertices> polygons;
rtabmap::Transform pose;
cv::Mat texture;

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

@@ -48,8 +48,74 @@ PointCloudDrawable::PointCloudDrawable(
visible_(true),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram)
{
updateCloud(cloud, image);
updatePolygons(polygons);
}
PointCloudDrawable::~PointCloudDrawable()
{
LOGI("Freeing cloud buffer %d", vertex_buffers_);
if (vertex_buffers_)
{
glDeleteBuffers(1, &vertex_buffers_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
vertex_buffers_ = 0;
}
if (textures_)
{
glDeleteTextures(1, &textures_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
textures_ = 0;
}
}
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::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image)
{
UASSERT(!cloud->empty());
if(nPoints_)
{
UASSERT((int)cloud->size() == nPoints_);
}
nPoints_ = 0;
if (vertex_buffers_)
{
glDeleteBuffers(1, &vertex_buffers_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
vertex_buffers_ = 0;
}
if(!image.empty())
{
if (textures_)
{
glDeleteTextures(1, &textures_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
textures_ = 0;
}
}
glGenBuffers(1, &vertex_buffers_);
if(!vertex_buffers_)
@@ -61,7 +127,7 @@ PointCloudDrawable::PointCloudDrawable(
if(!cloud->is_dense && !image.empty())
{
LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
UASSERT(!cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
glGenTextures(1, &textures_);
if(!textures_)
{
@@ -114,12 +180,12 @@ PointCloudDrawable::PointCloudDrawable(
return;
}
if(textures_)
if(textures_ && !image.empty())
{
// gen texture from image
glBindTexture(GL_TEXTURE_2D, textures_);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
cv::Mat rgbImage;
cv::cvtColor(image, rgbImage, CV_BGR2RGB);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
@@ -137,46 +203,6 @@ PointCloudDrawable::PointCloudDrawable(
}
nPoints_ = cloud->size();
updatePolygons(polygons);
}
PointCloudDrawable::~PointCloudDrawable()
{
LOGI("Freeing cloud buffer %d", vertex_buffers_);
if (vertex_buffers_)
{
glDeleteBuffers(1, &vertex_buffers_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
vertex_buffers_ = 0;
}
if (textures_)
{
glDeleteTextures(1, &textures_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
textures_ = 0;
}
}
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)

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

@@ -50,6 +50,7 @@ class PointCloudDrawable {
virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons);
void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image = cv::Mat());
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

@@ -480,3 +480,12 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
iter->second->updatePolygons(polygons);
}
}
void Scene::updateCloudColors(int id, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image)
{
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
iter->second->updateCloud(cloud, image);
}
}

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

@@ -109,6 +109,7 @@ class Scene {
bool hasCloud(int id) const;
std::set<int> getAddedClouds() const;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
void updateCloudColors(int id, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image = cv::Mat());
void setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}

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

@@ -39,6 +39,9 @@ class LogHandler : public UEventsHandler
public:
LogHandler()
{
ULogger::setEventLevel(ULogger::kWarning);
ULogger::setPrintThreadId(true);
registerToEventsManager();
}
protected:

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

@@ -14,6 +14,7 @@
<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/gain_compensation" android:title="Adjust Colors" />
<item android:id="@+id/sba" android:title="Bundle Adjustement" />
<item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
</menu>

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

@@ -670,11 +670,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
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();
mToast.makeText(getActivity(), String.format("Optimization done! Adjusting colors..."), mToast.LENGTH_SHORT).show();
}
else if(loopDetected < 0)
{
@@ -772,6 +771,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mProgressDialog.show();
RTABMapLib.postProcessing(4);
}
else if (itemId == R.id.gain_compensation)
{
mProgressDialog.setTitle("Post-Processing");
mProgressDialog.setMessage(String.format("Gain compensation..."));
mProgressDialog.show();
RTABMapLib.postProcessing(5);
}
else if (itemId == R.id.sba)
{
mProgressDialog.setTitle("Post-Processing");