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

Tango: added "Settings->Mapping->ArUco Marker Detection" option

Browse Source
This commit is contained in:
matlabbe
2019-02-23 03:02:42 +00:00
parent d089e95e5a
commit 17521e8efb
8 changed files with 175 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -100,6 +100,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kArucoMarkerLength(), std::string("0.0")));
if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
{
@@ -1379,14 +1380,14 @@ int RTABMapApp::Render()
LOGW("Looking fo data to load (%d) %fs", bufferedSensorData.size(), time.ticks());
#endif
std::map<int, rtabmap::Transform> poses = rtabmapEvents.back()->getStats().poses();
std::map<int, rtabmap::Transform> posesWithMarkers = rtabmapEvents.back()->getStats().poses();
if(!rtabmapEvents.back()->getStats().mapCorrection().isNull())
{
mapToOdom_ = rtabmapEvents.back()->getStats().mapCorrection();
}
// Transform pose in OpenGL world
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
for(std::map<int, rtabmap::Transform>::iterator iter=posesWithMarkers.begin(); iter!=posesWithMarkers.end(); ++iter)
{
if(!graphOptimization_)
{
@@ -1402,6 +1403,7 @@ int RTABMapApp::Render()
}
}
std::map<int, rtabmap::Transform> poses(posesWithMarkers.lower_bound(0), posesWithMarkers.end());
const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back()->getStats().constraints();
if(poses.size())
{
@@ -1533,7 +1535,7 @@ int RTABMapApp::Render()
}
}
if(poses.size())
if(!poses.empty())
{
//update cloud visibility
boost::mutex::scoped_lock lock(meshesMutex_);
@@ -1551,6 +1553,33 @@ int RTABMapApp::Render()
}
}
}
// Update markers
std::set<int> addedMarkers = main_scene_.getAddedMarkers();
for(std::set<int>::const_iterator iter=addedMarkers.begin();
iter!=addedMarkers.end();
++iter)
{
if(posesWithMarkers.find(*iter) == posesWithMarkers.end())
{
main_scene_.removeMarker(*iter);
}
}
for(std::map<int, rtabmap::Transform>::const_iterator iter=posesWithMarkers.begin();
iter!=posesWithMarkers.end() && iter->first<0;
++iter)
{
int id = iter->first;
if(main_scene_.hasMarker(id))
{
//just update pose
main_scene_.setMarkerPose(id, iter->second);
}
else
{
main_scene_.addMarker(id, iter->second);
}
}
}
else
{
@@ -3320,6 +3349,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f)));
uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f)));
uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f)));
uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopLandmark_detected(), uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f)));
}
// else use last data
@@ -3338,6 +3368,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
int landmarkDetected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
rtabmap::Transform currentPose = main_scene_.GetCameraPose();
float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;
if(!currentPose.isNull())
@@ -3357,7 +3388,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIFFFFFF)V" );
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIIFFFFFF)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
@@ -3381,6 +3412,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
optimizationMaxErrorRatio,
distanceTravelled,
fastMovement,
landmarkDetected,
x,
y,
z,

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

@@ -188,6 +188,10 @@ void Scene::clear()
{
delete iter->second;
}
for(std::map<int, tango_gl::Axis*>::iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
{
delete iter->second;
}
if(trace_)
{
trace_->ClearVertexArray();
@@ -198,6 +202,7 @@ void Scene::clear()
graph_ = 0;
}
pointClouds_.clear();
markers_.clear();
if(grid_)
{
grid_->SetPosition(kHeightOffset);
@@ -551,6 +556,12 @@ int Scene::Render() {
glDepthMask(GL_TRUE);
}
//draw markers on foreground
for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
{
iter->second->Render(projectionMatrix, viewMatrix);
}
return (int)cloudsToDraw.size();
}
@@ -648,6 +659,53 @@ void Scene::setTraceVisible(bool visible)
}
//Should only be called in OpenGL thread!
void Scene::addMarker(
int id,
const rtabmap::Transform & pose)
{
LOGI("add marker %d", id);
std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
if(iter == markers_.end())
{
//create
tango_gl::Axis * drawable = new tango_gl::Axis();
drawable->SetScale(glm::vec3(0.05f,0.05f,0.05f));
drawable->SetLineWidth(5);
markers_.insert(std::make_pair(id, drawable));
}
setMarkerPose(id, pose);
}
void Scene::setMarkerPose(int id, const rtabmap::Transform & pose)
{
UASSERT(!pose.isNull());
std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
if(iter != markers_.end())
{
glm::vec3 position(pose.x(), pose.y(), pose.z());
Eigen::Quaternionf quat = pose.getQuaternionf();
glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
iter->second->SetPosition(position);
iter->second->SetRotation(rotation);
}
}
bool Scene::hasMarker(int id) const
{
return markers_.find(id) != markers_.end();
}
void Scene::removeMarker(int id)
{
std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
if(iter != markers_.end())
{
delete iter->second;
markers_.erase(iter);
}
}
std::set<int> Scene::getAddedMarkers() const
{
return uKeysSet(markers_);
}
void Scene::addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,

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

@@ -102,6 +102,12 @@ class Scene {
void setGridVisible(bool visible);
void setTraceVisible(bool visible);
void addMarker(int id, const rtabmap::Transform & pose);
void setMarkerPose(int id, const rtabmap::Transform & pose);
bool hasMarker(int id) const;
void removeMarker(int id);
std::set<int> getAddedMarkers() const;
void addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -167,6 +173,8 @@ class Scene {
bool gridVisible_;
bool traceVisible_;
std::map<int, tango_gl::Axis*> markers_;
TangoSupportRotation color_camera_to_display_rotation_;
std::map<int, PointCloudDrawable*> pointClouds_;