mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
Integrated DVO odometry approach
This commit is contained in:
@@ -154,6 +154,7 @@ option(WITH_OCTOMAP "Include Octomap support" ON)
|
|||||||
option(WITH_CPUTSDF "Include CPUTSDF support" ON)
|
option(WITH_CPUTSDF "Include CPUTSDF support" ON)
|
||||||
option(WITH_FOVIS "Include FOVIS support" ON)
|
option(WITH_FOVIS "Include FOVIS support" ON)
|
||||||
option(WITH_VISO2 "Include VISO2 support" ON)
|
option(WITH_VISO2 "Include VISO2 support" ON)
|
||||||
|
option(WITH_DVO "Include DVO support" ON)
|
||||||
option(PCL_OMP "With PCL OMP implementations" ON)
|
option(PCL_OMP "With PCL OMP implementations" ON)
|
||||||
|
|
||||||
FIND_PACKAGE(OpenCV REQUIRED QUIET)
|
FIND_PACKAGE(OpenCV REQUIRED QUIET)
|
||||||
@@ -360,6 +361,12 @@ IF(WITH_VISO2)
|
|||||||
ENDIF(libviso2_FOUND)
|
ENDIF(libviso2_FOUND)
|
||||||
ENDIF(WITH_VISO2)
|
ENDIF(WITH_VISO2)
|
||||||
|
|
||||||
|
IF(WITH_DVO)
|
||||||
|
FIND_PACKAGE(dvo_core QUIET)
|
||||||
|
IF(dvo_core_FOUND)
|
||||||
|
MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}")
|
||||||
|
ENDIF(dvo_core_FOUND)
|
||||||
|
ENDIF(WITH_DVO)
|
||||||
|
|
||||||
IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
|
IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
|
||||||
#Newest versions require std11
|
#Newest versions require std11
|
||||||
@@ -493,6 +500,11 @@ IF(NOT libviso2_FOUND)
|
|||||||
ELSE()
|
ELSE()
|
||||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES})
|
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES})
|
||||||
ENDIF()
|
ENDIF()
|
||||||
|
IF(NOT dvo_core_FOUND)
|
||||||
|
SET(DVO "//")
|
||||||
|
ELSE()
|
||||||
|
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES})
|
||||||
|
ENDIF()
|
||||||
IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
|
IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
|
||||||
SET(CONF_VTK_QT true)
|
SET(CONF_VTK_QT true)
|
||||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
|
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
|
||||||
@@ -832,6 +844,14 @@ ELSE()
|
|||||||
MESSAGE(STATUS " With libviso2 = NO (libviso2 not found)")
|
MESSAGE(STATUS " With libviso2 = NO (libviso2 not found)")
|
||||||
ENDIF()
|
ENDIF()
|
||||||
|
|
||||||
|
IF(dvo_core_FOUND)
|
||||||
|
MESSAGE(STATUS " With dvo_core = YES (License: GPLv3)")
|
||||||
|
ELSEIF(NOT WITH_DVO)
|
||||||
|
MESSAGE(STATUS " With dvo_core = NO (WITH_DVO=OFF)")
|
||||||
|
ELSE()
|
||||||
|
MESSAGE(STATUS " With dvo_core = NO (dvo_core not found)")
|
||||||
|
ENDIF()
|
||||||
|
|
||||||
IF(QT4_FOUND)
|
IF(QT4_FOUND)
|
||||||
MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)")
|
MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)")
|
||||||
ELSEIF(Qt5_FOUND)
|
ELSEIF(Qt5_FOUND)
|
||||||
|
|||||||
@@ -56,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
@CPUTSDF@#define RTABMAP_CPUTSDF
|
@CPUTSDF@#define RTABMAP_CPUTSDF
|
||||||
@FOVIS@#define RTABMAP_FOVIS
|
@FOVIS@#define RTABMAP_FOVIS
|
||||||
@VISO2@#define RTABMAP_VISO2
|
@VISO2@#define RTABMAP_VISO2
|
||||||
|
@DVO@#define RTABMAP_DVO
|
||||||
|
|
||||||
#endif /* VERSION_H_ */
|
#endif /* VERSION_H_ */
|
||||||
|
|
||||||
|
|||||||
@@ -47,7 +47,8 @@ public:
|
|||||||
kTypeF2M = 0,
|
kTypeF2M = 0,
|
||||||
kTypeF2F = 1,
|
kTypeF2F = 1,
|
||||||
kTypeFovis = 2,
|
kTypeFovis = 2,
|
||||||
kTypeViso2 = 3
|
kTypeViso2 = 3,
|
||||||
|
kTypeDVO = 4
|
||||||
};
|
};
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|||||||
@@ -0,0 +1,66 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of the Universite de Sherbrooke nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef ODOMETRYDVO_H_
|
||||||
|
#define ODOMETRYDVO_H_
|
||||||
|
|
||||||
|
#include <rtabmap/core/Odometry.h>
|
||||||
|
|
||||||
|
namespace dvo {
|
||||||
|
class DenseTracker;
|
||||||
|
namespace core {
|
||||||
|
class RgbdImagePyramid;
|
||||||
|
class RgbdCameraPyramid;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace rtabmap {
|
||||||
|
|
||||||
|
class RTABMAP_EXP OdometryDVO : public Odometry
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
OdometryDVO(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
|
||||||
|
virtual ~OdometryDVO();
|
||||||
|
|
||||||
|
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||||
|
virtual Odometry::Type getType() {return Odometry::kTypeDVO;}
|
||||||
|
|
||||||
|
private:
|
||||||
|
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
|
||||||
|
|
||||||
|
private:
|
||||||
|
dvo::DenseTracker * dvo_;
|
||||||
|
dvo::core::RgbdImagePyramid * reference_;
|
||||||
|
dvo::core::RgbdCameraPyramid * camera_;
|
||||||
|
bool lost_;
|
||||||
|
Transform motionFromKeyFrame_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* ODOMETRYDVO_H_ */
|
||||||
@@ -62,6 +62,7 @@ SET(SRC_FILES
|
|||||||
OdometryF2F.cpp
|
OdometryF2F.cpp
|
||||||
OdometryFovis.cpp
|
OdometryFovis.cpp
|
||||||
OdometryViso2.cpp
|
OdometryViso2.cpp
|
||||||
|
OdometryDVO.cpp
|
||||||
|
|
||||||
Stereo.cpp
|
Stereo.cpp
|
||||||
StereoDense.cpp
|
StereoDense.cpp
|
||||||
@@ -299,6 +300,17 @@ IF(libviso2_FOUND)
|
|||||||
)
|
)
|
||||||
ENDIF(libviso2_FOUND)
|
ENDIF(libviso2_FOUND)
|
||||||
|
|
||||||
|
IF(dvo_core_FOUND)
|
||||||
|
SET(INCLUDE_DIRS
|
||||||
|
${INCLUDE_DIRS}
|
||||||
|
${dvo_core_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
SET(LIBRARIES
|
||||||
|
${LIBRARIES}
|
||||||
|
${dvo_core_LIBRARIES}
|
||||||
|
)
|
||||||
|
ENDIF(dvo_core_FOUND)
|
||||||
|
|
||||||
####################################
|
####################################
|
||||||
# Generate resources files
|
# Generate resources files
|
||||||
####################################
|
####################################
|
||||||
|
|||||||
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
#include "rtabmap/core/OdometryF2F.h"
|
#include "rtabmap/core/OdometryF2F.h"
|
||||||
#include "rtabmap/core/OdometryFovis.h"
|
#include "rtabmap/core/OdometryFovis.h"
|
||||||
#include "rtabmap/core/OdometryViso2.h"
|
#include "rtabmap/core/OdometryViso2.h"
|
||||||
|
#include "rtabmap/core/OdometryDVO.h"
|
||||||
#include "rtabmap/core/OdometryInfo.h"
|
#include "rtabmap/core/OdometryInfo.h"
|
||||||
#include "rtabmap/core/util3d.h"
|
#include "rtabmap/core/util3d.h"
|
||||||
#include "rtabmap/core/util3d_mapping.h"
|
#include "rtabmap/core/util3d_mapping.h"
|
||||||
@@ -58,6 +59,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
|
|||||||
Odometry * odometry = 0;
|
Odometry * odometry = 0;
|
||||||
switch(type)
|
switch(type)
|
||||||
{
|
{
|
||||||
|
case Odometry::kTypeDVO:
|
||||||
|
odometry = new OdometryDVO(parameters);
|
||||||
|
break;
|
||||||
case Odometry::kTypeViso2:
|
case Odometry::kTypeViso2:
|
||||||
odometry = new OdometryViso2(parameters);
|
odometry = new OdometryViso2(parameters);
|
||||||
break;
|
break;
|
||||||
|
|||||||
@@ -0,0 +1,274 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of the Universite de Sherbrooke nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "rtabmap/core/OdometryDVO.h"
|
||||||
|
#include "rtabmap/core/OdometryInfo.h"
|
||||||
|
#include "rtabmap/core/util2d.h"
|
||||||
|
#include "rtabmap/core/Version.h"
|
||||||
|
#include "rtabmap/utilite/ULogger.h"
|
||||||
|
#include "rtabmap/utilite/UTimer.h"
|
||||||
|
#include "rtabmap/utilite/UStl.h"
|
||||||
|
|
||||||
|
#ifdef RTABMAP_DVO
|
||||||
|
#include <dvo/dense_tracking.h>
|
||||||
|
#include <dvo/core/surface_pyramid.h>
|
||||||
|
#include <dvo/core/rgbd_image.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
namespace rtabmap {
|
||||||
|
|
||||||
|
OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
|
||||||
|
Odometry(parameters),
|
||||||
|
dvo_(0),
|
||||||
|
reference_(0),
|
||||||
|
camera_(0),
|
||||||
|
lost_(false),
|
||||||
|
motionFromKeyFrame_(Transform::getIdentity())
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
OdometryDVO::~OdometryDVO()
|
||||||
|
{
|
||||||
|
#ifdef RTABMAP_DVO
|
||||||
|
if(dvo_)
|
||||||
|
{
|
||||||
|
delete dvo_;
|
||||||
|
}
|
||||||
|
if(reference_)
|
||||||
|
{
|
||||||
|
delete reference_;
|
||||||
|
}
|
||||||
|
if(camera_)
|
||||||
|
{
|
||||||
|
delete camera_;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void OdometryDVO::reset(const Transform & initialPose)
|
||||||
|
{
|
||||||
|
Odometry::reset(initialPose);
|
||||||
|
#ifdef RTABMAP_DVO
|
||||||
|
if(dvo_)
|
||||||
|
{
|
||||||
|
delete dvo_;
|
||||||
|
dvo_ = 0;
|
||||||
|
}
|
||||||
|
if(reference_)
|
||||||
|
{
|
||||||
|
delete reference_;
|
||||||
|
reference_ = 0;
|
||||||
|
}
|
||||||
|
if(camera_)
|
||||||
|
{
|
||||||
|
delete camera_;
|
||||||
|
camera_ = 0;
|
||||||
|
}
|
||||||
|
lost_ = false;
|
||||||
|
motionFromKeyFrame_.setIdentity();
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// return not null transform if odometry is correctly computed
|
||||||
|
Transform OdometryDVO::computeTransform(
|
||||||
|
SensorData & data,
|
||||||
|
const Transform & guess,
|
||||||
|
OdometryInfo * info)
|
||||||
|
{
|
||||||
|
Transform t;
|
||||||
|
|
||||||
|
#ifdef RTABMAP_DVO
|
||||||
|
UTimer timer;
|
||||||
|
|
||||||
|
if(data.imageRaw().empty() ||
|
||||||
|
data.imageRaw().rows != data.depthOrRightRaw().rows ||
|
||||||
|
data.imageRaw().cols != data.depthOrRightRaw().cols)
|
||||||
|
{
|
||||||
|
UERROR("Not supported input!");
|
||||||
|
return t;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()))
|
||||||
|
{
|
||||||
|
UERROR("Invalid camera model! Only single RGB-D camera supported.");
|
||||||
|
return t;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(dvo_ == 0)
|
||||||
|
{
|
||||||
|
dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
|
||||||
|
|
||||||
|
dvo_ = new dvo::DenseTracker(cfg);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat grey, grey_s16, depth_inpainted, depth_mask, depth_mono, depth_float;
|
||||||
|
if(data.imageRaw().type() != CV_32FC1)
|
||||||
|
{
|
||||||
|
if(data.imageRaw().type() == CV_8UC3)
|
||||||
|
{
|
||||||
|
cv::cvtColor(data.imageRaw(), grey, CV_BGR2GRAY);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
grey = data.imageRaw();
|
||||||
|
}
|
||||||
|
|
||||||
|
grey.convertTo(grey_s16, CV_32F);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
grey_s16 = data.imageRaw();
|
||||||
|
}
|
||||||
|
|
||||||
|
// make sure all zeros are NAN
|
||||||
|
if(data.depthRaw().type() == CV_32FC1)
|
||||||
|
{
|
||||||
|
depth_float = data.depthRaw();
|
||||||
|
for(int i=0; i<depth_float.rows; ++i)
|
||||||
|
{
|
||||||
|
for(int j=0; j<depth_float.cols; ++j)
|
||||||
|
{
|
||||||
|
float & d = depth_float.at<float>(i,j);
|
||||||
|
if(d == 0.0f)
|
||||||
|
{
|
||||||
|
d = NAN;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(data.depthRaw().type() == CV_16UC1)
|
||||||
|
{
|
||||||
|
depth_float = cv::Mat(data.depthRaw().size(), CV_32FC1);
|
||||||
|
for(int i=0; i<data.depthRaw().rows; ++i)
|
||||||
|
{
|
||||||
|
for(int j=0; j<data.depthRaw().cols; ++j)
|
||||||
|
{
|
||||||
|
float d = float(data.depthRaw().at<unsigned short>(i,j))/1000.0f;
|
||||||
|
depth_float.at<float>(i, j) = d==0.0f?NAN:d;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
UFATAL("Unknown depth format!");
|
||||||
|
}
|
||||||
|
|
||||||
|
if(camera_ == 0)
|
||||||
|
{
|
||||||
|
dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(
|
||||||
|
data.cameraModels()[0].fx(),
|
||||||
|
data.cameraModels()[0].fy(),
|
||||||
|
data.cameraModels()[0].cx(),
|
||||||
|
data.cameraModels()[0].cy());
|
||||||
|
camera_ = new dvo::core::RgbdCameraPyramid(
|
||||||
|
data.cameraModels()[0].imageWidth(),
|
||||||
|
data.cameraModels()[0].imageHeight(),
|
||||||
|
intrinsics);
|
||||||
|
}
|
||||||
|
|
||||||
|
dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
|
||||||
|
|
||||||
|
cv::Mat covariance;
|
||||||
|
if(reference_ == 0)
|
||||||
|
{
|
||||||
|
reference_ = current;
|
||||||
|
if(!lost_)
|
||||||
|
{
|
||||||
|
t.setIdentity();
|
||||||
|
}
|
||||||
|
covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
dvo::DenseTracker::Result result;
|
||||||
|
dvo_->match(*reference_, *current, result);
|
||||||
|
t = Transform::fromEigen3d(result.Transformation);
|
||||||
|
|
||||||
|
if(result.Information(0,0) > 0.0 && result.Information(0,0) != 1.0)
|
||||||
|
{
|
||||||
|
lost_ = false;
|
||||||
|
cv::Mat information = cv::Mat::eye(6,6, CV_64FC1);
|
||||||
|
memcpy(information.data, result.Information.data(), 36*sizeof(double));
|
||||||
|
covariance = information.inv();
|
||||||
|
covariance *= 100.0; // to be in the same scale than loop closure detection
|
||||||
|
|
||||||
|
Transform currentMotion = t;
|
||||||
|
t = motionFromKeyFrame_.inverse() * t;
|
||||||
|
|
||||||
|
// TODO make parameters?
|
||||||
|
if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
|
||||||
|
{
|
||||||
|
if(info)
|
||||||
|
{
|
||||||
|
info->keyFrameAdded = true;
|
||||||
|
}
|
||||||
|
// new keyframe
|
||||||
|
delete reference_;
|
||||||
|
reference_ = current;
|
||||||
|
motionFromKeyFrame_.setIdentity();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
delete current;
|
||||||
|
motionFromKeyFrame_ = currentMotion;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
lost_ = true;
|
||||||
|
delete reference_;
|
||||||
|
delete current;
|
||||||
|
reference_ = 0; // this will make restart from the next frame
|
||||||
|
motionFromKeyFrame_.setIdentity();
|
||||||
|
t.setNull();
|
||||||
|
covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
|
||||||
|
UWARN("dvo failed to estimate motion, tracking will be reinitialized on next frame.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const Transform & localTransform = data.cameraModels()[0].localTransform();
|
||||||
|
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
|
||||||
|
{
|
||||||
|
// from camera frame to base frame
|
||||||
|
t = localTransform * t * localTransform.inverse();
|
||||||
|
}
|
||||||
|
|
||||||
|
if(info)
|
||||||
|
{
|
||||||
|
info->type = (int)kTypeDVO;
|
||||||
|
info->covariance = covariance;
|
||||||
|
}
|
||||||
|
|
||||||
|
UINFO("Odom update time = %fs", timer.elapsed());
|
||||||
|
|
||||||
|
#else
|
||||||
|
UERROR("RTAB-Map is not built with DVO support! Select another visual odometry approach.");
|
||||||
|
#endif
|
||||||
|
return t;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace rtabmap
|
||||||
@@ -88,6 +88,11 @@ AboutDialog::AboutDialog(QWidget * parent) :
|
|||||||
#else
|
#else
|
||||||
_ui->label_viso2->setText("No");
|
_ui->label_viso2->setText("No");
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef RTABMAP_DVO
|
||||||
|
_ui->label_dvo->setText("Yes");
|
||||||
|
#else
|
||||||
|
_ui->label_dvo->setText("No");
|
||||||
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1225,7 +1225,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
|
|||||||
|
|
||||||
//detect if it is OdometryMono intitialization
|
//detect if it is OdometryMono intitialization
|
||||||
bool monoInitialization = false;
|
bool monoInitialization = false;
|
||||||
if(_preferencesDialog->getOdomStrategy() == 4 && odom.info().type == (int)Odometry::kTypeF2F)
|
if(_preferencesDialog->getOdomStrategy() == 5 && odom.info().type == (int)Odometry::kTypeF2F)
|
||||||
{
|
{
|
||||||
monoInitialization = true;
|
monoInitialization = true;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -165,6 +165,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
|||||||
#ifndef RTABMAP_VISO2
|
#ifndef RTABMAP_VISO2
|
||||||
_ui->odom_strategy->setItemData(3, 0, Qt::UserRole - 1);
|
_ui->odom_strategy->setItemData(3, 0, Qt::UserRole - 1);
|
||||||
#endif
|
#endif
|
||||||
|
#ifndef RTABMAP_DVO
|
||||||
|
_ui->odom_strategy->setItemData(4, 0, Qt::UserRole - 1);
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef RTABMAP_NONFREE
|
#ifndef RTABMAP_NONFREE
|
||||||
_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
|
_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
|
||||||
|
|||||||
@@ -316,6 +316,13 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
|
<item row="21" column="0">
|
||||||
|
<widget class="QLabel" name="label_20">
|
||||||
|
<property name="text">
|
||||||
|
<string>With Octomap :</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
<item row="11" column="0">
|
<item row="11" column="0">
|
||||||
<widget class="QLabel" name="label_12">
|
<widget class="QLabel" name="label_12">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
@@ -330,10 +337,10 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="21" column="0">
|
<item row="16" column="0">
|
||||||
<widget class="QLabel" name="label_20">
|
<widget class="QLabel" name="label_17">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>With Octomap :</string>
|
<string>With stereo FlyCapture2 :</string>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -347,13 +354,6 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="16" column="0">
|
|
||||||
<widget class="QLabel" name="label_17">
|
|
||||||
<property name="text">
|
|
||||||
<string>With stereo FlyCapture2 :</string>
|
|
||||||
</property>
|
|
||||||
</widget>
|
|
||||||
</item>
|
|
||||||
<item row="10" column="0">
|
<item row="10" column="0">
|
||||||
<widget class="QLabel" name="label_23">
|
<widget class="QLabel" name="label_23">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
@@ -368,6 +368,13 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
|
<item row="22" column="0">
|
||||||
|
<widget class="QLabel" name="label_24">
|
||||||
|
<property name="text">
|
||||||
|
<string>With CPU-TSDF :</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
<item row="3" column="1">
|
<item row="3" column="1">
|
||||||
<widget class="QLabel" name="label_HomePage_2">
|
<widget class="QLabel" name="label_HomePage_2">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
@@ -385,10 +392,10 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="22" column="0">
|
<item row="24" column="0">
|
||||||
<widget class="QLabel" name="label_24">
|
<widget class="QLabel" name="label_26">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>With CPU-TSDF :</string>
|
<string>With Viso2 :</string>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -419,13 +426,6 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="24" column="0">
|
|
||||||
<widget class="QLabel" name="label_26">
|
|
||||||
<property name="text">
|
|
||||||
<string>With Viso2 :</string>
|
|
||||||
</property>
|
|
||||||
</widget>
|
|
||||||
</item>
|
|
||||||
<item row="23" column="1">
|
<item row="23" column="1">
|
||||||
<widget class="QLabel" name="label_fovis">
|
<widget class="QLabel" name="label_fovis">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
@@ -446,6 +446,23 @@ p, li { white-space: pre-wrap; }
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
|
<item row="25" column="0">
|
||||||
|
<widget class="QLabel" name="label_27">
|
||||||
|
<property name="text">
|
||||||
|
<string>With Viso2 :</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="25" column="1">
|
||||||
|
<widget class="QLabel" name="label_dvo">
|
||||||
|
<property name="text">
|
||||||
|
<string/>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
</layout>
|
</layout>
|
||||||
</item>
|
</item>
|
||||||
<item>
|
<item>
|
||||||
|
|||||||
@@ -63,7 +63,7 @@
|
|||||||
<property name="geometry">
|
<property name="geometry">
|
||||||
<rect>
|
<rect>
|
||||||
<x>0</x>
|
<x>0</x>
|
||||||
<y>-556</y>
|
<y>-34</y>
|
||||||
<width>678</width>
|
<width>678</width>
|
||||||
<height>2739</height>
|
<height>2739</height>
|
||||||
</rect>
|
</rect>
|
||||||
@@ -9709,6 +9709,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
|||||||
<string>Viso2</string>
|
<string>Viso2</string>
|
||||||
</property>
|
</property>
|
||||||
</item>
|
</item>
|
||||||
|
<item>
|
||||||
|
<property name="text">
|
||||||
|
<string>DVO</string>
|
||||||
|
</property>
|
||||||
|
</item>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="2" column="1">
|
<item row="2" column="1">
|
||||||
@@ -9980,7 +9985,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
|||||||
<item>
|
<item>
|
||||||
<widget class="QStackedWidget" name="stackedWidget_odometryType">
|
<widget class="QStackedWidget" name="stackedWidget_odometryType">
|
||||||
<property name="currentIndex">
|
<property name="currentIndex">
|
||||||
<number>2</number>
|
<number>4</number>
|
||||||
</property>
|
</property>
|
||||||
<widget class="QWidget" name="page_52">
|
<widget class="QWidget" name="page_52">
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_77">
|
<layout class="QVBoxLayout" name="verticalLayout_77">
|
||||||
@@ -11504,6 +11509,48 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
|||||||
</item>
|
</item>
|
||||||
</layout>
|
</layout>
|
||||||
</widget>
|
</widget>
|
||||||
|
<widget class="QWidget" name="page_69">
|
||||||
|
<layout class="QVBoxLayout" name="verticalLayout_122">
|
||||||
|
<item>
|
||||||
|
<widget class="QGroupBox" name="groupBox_27">
|
||||||
|
<property name="title">
|
||||||
|
<string>DVO</string>
|
||||||
|
</property>
|
||||||
|
<layout class="QVBoxLayout" name="verticalLayout_123">
|
||||||
|
<item>
|
||||||
|
<widget class="QLabel" name="label_411">
|
||||||
|
<property name="text">
|
||||||
|
<string><html><head/><body><p>Dense Visual Odometry: <a href="https://github.com/tum-vision/dvo_slam"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/tum-vision/dvo_slam</span></a></p><p><span style=" font-weight:600;">Only RGB-D is supported.</span><br/></p></body></html></string>
|
||||||
|
</property>
|
||||||
|
<property name="wordWrap">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="openExternalLinks">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="textInteractionFlags">
|
||||||
|
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<spacer name="verticalSpacer_63">
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Vertical</enum>
|
||||||
|
</property>
|
||||||
|
<property name="sizeHint" stdset="0">
|
||||||
|
<size>
|
||||||
|
<width>20</width>
|
||||||
|
<height>1029</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
</spacer>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
<widget class="QWidget" name="page_26">
|
<widget class="QWidget" name="page_26">
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_88">
|
<layout class="QVBoxLayout" name="verticalLayout_88">
|
||||||
<item>
|
<item>
|
||||||
|
|||||||
Reference in New Issue
Block a user