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: 0.17 fixing broken localization on visualization

Browse Source
This commit is contained in:
matlabbe
2018-04-05 11:00:16 -04:00
parent b3b0caa038
commit 2b00b2c1c5
2 changed files with 42 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -188,6 +188,8 @@ RTABMapApp::RTABMapApp() :
visualizingMesh_(false), visualizingMesh_(false),
exportedMeshUpdated_(false), exportedMeshUpdated_(false),
optMesh_(new pcl::TextureMesh), optMesh_(new pcl::TextureMesh),
optRefId_(0),
optRefPose_(0),
mapToOdom_(rtabmap::Transform::getIdentity()) mapToOdom_(rtabmap::Transform::getIdentity())
{ {
@@ -208,6 +210,10 @@ RTABMapApp::~RTABMapApp() {
{ {
delete logHandler_; delete logHandler_;
} }
if(optRefPose_)
{
delete optRefPose_;
}
{ {
boost::mutex::scoped_lock lock(rtabmapMutex_); boost::mutex::scoped_lock lock(rtabmapMutex_);
if(rtabmapEvents_.size()) if(rtabmapEvents_.size())
@@ -315,6 +321,12 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
// Open visualization while we load (if there is an optimized mesh saved in database) // Open visualization while we load (if there is an optimized mesh saved in database)
optMesh_.reset(new pcl::TextureMesh); optMesh_.reset(new pcl::TextureMesh);
optTexture_ = cv::Mat(); optTexture_ = cv::Mat();
optRefId_ = 0;
if(optRefPose_)
{
delete optRefPose_;
optRefPose_ = 0;
}
cv::Mat cloudMat; cv::Mat cloudMat;
std::vector<std::vector<std::vector<unsigned int> > > polygons; std::vector<std::vector<std::vector<unsigned int> > > polygons;
#if PCL_VERSION_COMPARE(>=, 1, 8, 0) #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -573,6 +585,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
rtabmap_->setOptimizedPoses(poses); rtabmap_->setOptimizedPoses(poses);
// for optimized mesh
if(poses.size())
{
// just take the last as reference
optRefId_ = poses.rbegin()->first;
optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
}
if(camera_) if(camera_)
{ {
camera_->resetOrigin(); camera_->resetOrigin();
@@ -1109,6 +1129,12 @@ int RTABMapApp::Render()
mapToOdom_ = stats.mapCorrection(); mapToOdom_ = stats.mapCorrection();
} }
std::map<int, rtabmap::Transform>::const_iterator iter = stats.poses().find(optRefId_);
if(iter != stats.poses().end() && !iter->second.isNull() && optRefPose_)
{
// adjust opt mesh pose
main_scene_.setCloudPose(g_optMeshId, opengl_world_T_rtabmap_world * iter->second * (*optRefPose_).inverse());
}
int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f); int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f); int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f); int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
@@ -2862,6 +2888,20 @@ bool RTABMapApp::exportMesh(
} }
exporting_ = false; exporting_ = false;
optRefId_ = 0;
if(optRefPose_)
{
delete optRefPose_;
optRefPose_ = 0;
}
if(success && poses.size())
{
// for optimized mesh
// just take the last as reference
optRefId_ = poses.rbegin()->first;
optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
}
return success; return success;
} }

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

@@ -235,6 +235,8 @@ class RTABMapApp : public UEventsHandler {
bool exportedMeshUpdated_; bool exportedMeshUpdated_;
pcl::TextureMesh::Ptr optMesh_; pcl::TextureMesh::Ptr optMesh_;
cv::Mat optTexture_; cv::Mat optTexture_;
int optRefId_;
rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
// main_scene_ includes all drawable object for visualizing Tango device's // main_scene_ includes all drawable object for visualizing Tango device's
// movement and point cloud. // movement and point cloud.