mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 17:17:47 +08:00
Integrated Fovis (Odom/Strategy=2)
This commit is contained in:
@@ -152,6 +152,7 @@ option(WITH_REALSENSE "Include RealSense support" ON)
|
||||
option(WITH_REALSENSE_SLAM "Include RealSenseSlam support" ON)
|
||||
option(WITH_OCTOMAP "Include Octomap support" ON)
|
||||
option(WITH_CPUTSDF "Include CPUTSDF support" ON)
|
||||
option(WITH_FOVIS "Include FOVIS support" ON)
|
||||
option(PCL_OMP "With PCL OMP implementations" ON)
|
||||
|
||||
FIND_PACKAGE(OpenCV REQUIRED QUIET)
|
||||
@@ -344,6 +345,13 @@ IF(WITH_CPUTSDF)
|
||||
ENDIF(CPUTSDF_FOUND)
|
||||
ENDIF(WITH_CPUTSDF)
|
||||
|
||||
IF(WITH_FOVIS)
|
||||
FIND_PACKAGE(libfovis QUIET)
|
||||
IF(libfovis_FOUND)
|
||||
MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}")
|
||||
ENDIF(libfovis_FOUND)
|
||||
ENDIF(WITH_FOVIS)
|
||||
|
||||
IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
|
||||
#Newest versions require std11
|
||||
IF(NOT MSVC)
|
||||
@@ -466,6 +474,11 @@ IF(NOT CPUTSDF_FOUND)
|
||||
ELSE()
|
||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES})
|
||||
ENDIF()
|
||||
IF(NOT libfovis_FOUND)
|
||||
SET(FOVIS "//")
|
||||
ELSE()
|
||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES})
|
||||
ENDIF()
|
||||
IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
|
||||
SET(CONF_VTK_QT true)
|
||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
|
||||
@@ -789,6 +802,14 @@ ELSE()
|
||||
MESSAGE(STATUS " With CPUTSDF = NO (CPUTSDF not found)")
|
||||
ENDIF()
|
||||
|
||||
IF(libfovis_FOUND)
|
||||
MESSAGE(STATUS " With libfovis = YES (License: GPLv2)")
|
||||
ELSEIF(NOT WITH_FOVIS)
|
||||
MESSAGE(STATUS " With libfovis = NO (WITH_FOVIS=OFF)")
|
||||
ELSE()
|
||||
MESSAGE(STATUS " With libfovis = NO (libfovis not found)")
|
||||
ENDIF()
|
||||
|
||||
IF(QT4_FOUND)
|
||||
MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)")
|
||||
ELSEIF(Qt5_FOUND)
|
||||
|
||||
@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
|
||||
@OCTOMAP@#define RTABMAP_OCTOMAP
|
||||
@CPUTSDF@#define RTABMAP_CPUTSDF
|
||||
@FOVIS@#define RTABMAP_FOVIS
|
||||
|
||||
#endif /* VERSION_H_ */
|
||||
|
||||
|
||||
@@ -45,7 +45,8 @@ public:
|
||||
enum Type {
|
||||
kTypeUndef = -1,
|
||||
kTypeF2M = 0,
|
||||
kTypeF2F = 1
|
||||
kTypeF2F = 1,
|
||||
kTypeFovis = 2
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
/*
|
||||
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 ODOMETRYFOVIS_H_
|
||||
#define ODOMETRYFOVIS_H_
|
||||
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
|
||||
namespace fovis {
|
||||
class VisualOdometry;
|
||||
class Rectification;
|
||||
class StereoCalibration;
|
||||
class DepthImage;
|
||||
class StereoDepth;
|
||||
}
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Registration;
|
||||
|
||||
class RTABMAP_EXP OdometryFovis : public Odometry
|
||||
{
|
||||
public:
|
||||
OdometryFovis(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
|
||||
virtual ~OdometryFovis();
|
||||
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
virtual Odometry::Type getType() {return Odometry::kTypeFovis;}
|
||||
|
||||
private:
|
||||
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
|
||||
|
||||
private:
|
||||
fovis::VisualOdometry * fovis_;
|
||||
fovis::Rectification * rect_;
|
||||
fovis::StereoCalibration * stereoCalib_;
|
||||
fovis::DepthImage * depthImage_;
|
||||
fovis::StereoDepth * stereoDepth_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* ODOMETRYFOVIS_H_ */
|
||||
@@ -363,7 +363,7 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
|
||||
|
||||
// Odometry
|
||||
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
|
||||
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis");
|
||||
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
|
||||
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
|
||||
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
|
||||
|
||||
@@ -60,6 +60,7 @@ SET(SRC_FILES
|
||||
OdometryF2M.cpp
|
||||
OdometryMono.cpp
|
||||
OdometryF2F.cpp
|
||||
OdometryFovis.cpp
|
||||
|
||||
Stereo.cpp
|
||||
StereoDense.cpp
|
||||
@@ -275,6 +276,17 @@ IF(OCTOMAP_FOUND)
|
||||
)
|
||||
ENDIF(OCTOMAP_FOUND)
|
||||
|
||||
IF(libfovis_FOUND)
|
||||
SET(INCLUDE_DIRS
|
||||
${INCLUDE_DIRS}
|
||||
${libfovis_INCLUDE_DIRS}
|
||||
)
|
||||
SET(LIBRARIES
|
||||
${LIBRARIES}
|
||||
${libfovis_LIBRARIES}
|
||||
)
|
||||
ENDIF(libfovis_FOUND)
|
||||
|
||||
####################################
|
||||
# Generate resources files
|
||||
####################################
|
||||
|
||||
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/core/OdometryF2M.h>
|
||||
#include "rtabmap/core/Odometry.h"
|
||||
#include "rtabmap/core/OdometryF2F.h"
|
||||
#include "rtabmap/core/OdometryFovis.h"
|
||||
#include "rtabmap/core/OdometryInfo.h"
|
||||
#include "rtabmap/core/util3d.h"
|
||||
#include "rtabmap/core/util3d_mapping.h"
|
||||
@@ -56,6 +57,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
|
||||
Odometry * odometry = 0;
|
||||
switch(type)
|
||||
{
|
||||
case Odometry::kTypeFovis:
|
||||
odometry = new OdometryFovis(parameters);
|
||||
break;
|
||||
case Odometry::kTypeF2F:
|
||||
odometry = new OdometryF2F(parameters);
|
||||
break;
|
||||
|
||||
@@ -0,0 +1,334 @@
|
||||
/*
|
||||
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/OdometryFovis.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_FOVIS
|
||||
#include <fovis.hpp>
|
||||
#endif
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
|
||||
Odometry(parameters),
|
||||
fovis_(0),
|
||||
rect_(0),
|
||||
stereoCalib_(0),
|
||||
depthImage_(0),
|
||||
stereoDepth_(0)
|
||||
{
|
||||
}
|
||||
|
||||
OdometryFovis::~OdometryFovis()
|
||||
{
|
||||
#ifdef RTABMAP_FOVIS
|
||||
if(fovis_)
|
||||
{
|
||||
delete fovis_;
|
||||
}
|
||||
if(rect_)
|
||||
{
|
||||
delete rect_;
|
||||
}
|
||||
if(stereoCalib_)
|
||||
{
|
||||
delete stereoCalib_;
|
||||
}
|
||||
if(depthImage_)
|
||||
{
|
||||
delete depthImage_;
|
||||
}
|
||||
if(stereoDepth_)
|
||||
{
|
||||
delete stereoDepth_;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void OdometryFovis::reset(const Transform & initialPose)
|
||||
{
|
||||
Odometry::reset(initialPose);
|
||||
#ifdef RTABMAP_FOVIS
|
||||
if(fovis_)
|
||||
{
|
||||
delete fovis_;
|
||||
fovis_ = 0;
|
||||
}
|
||||
if(rect_)
|
||||
{
|
||||
delete rect_;
|
||||
rect_ = 0;
|
||||
}
|
||||
if(stereoCalib_)
|
||||
{
|
||||
delete stereoCalib_;
|
||||
stereoCalib_ = 0;
|
||||
}
|
||||
if(depthImage_)
|
||||
{
|
||||
delete depthImage_;
|
||||
depthImage_ = 0;
|
||||
}
|
||||
if(stereoDepth_)
|
||||
{
|
||||
delete stereoDepth_;
|
||||
stereoDepth_ = 0;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// return not null transform if odometry is correctly computed
|
||||
Transform OdometryFovis::computeTransform(
|
||||
SensorData & data,
|
||||
const Transform & guess,
|
||||
OdometryInfo * info)
|
||||
{
|
||||
Transform t;
|
||||
|
||||
#ifdef RTABMAP_FOVIS
|
||||
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()) ||
|
||||
(data.stereoCameraModel().isValidForProjection() &&
|
||||
data.stereoCameraModel().left().isValidForReprojection() &&
|
||||
data.stereoCameraModel().right().isValidForReprojection())))
|
||||
{
|
||||
UERROR("Invalid camera model!");
|
||||
return t;
|
||||
}
|
||||
|
||||
cv::Mat gray;
|
||||
if(data.imageRaw().type() == CV_8UC3)
|
||||
{
|
||||
cv::cvtColor(data.imageRaw(), gray, CV_BGR2GRAY);
|
||||
}
|
||||
else if(data.imageRaw().type() == CV_8UC1)
|
||||
{
|
||||
gray = data.imageRaw();
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Not supported color type!");
|
||||
}
|
||||
|
||||
fovis::VisualOdometryOptions options = fovis::VisualOdometry::getDefaultOptions();
|
||||
fovis::DepthSource * depthSource = 0;
|
||||
cv::Mat depth;
|
||||
cv::Mat right;
|
||||
Transform localTransform = Transform::getIdentity();
|
||||
if(data.cameraModels().size() == 1) //depth
|
||||
{
|
||||
fovis::CameraIntrinsicsParameters rgb_params;
|
||||
memset(&rgb_params, 0, sizeof(fovis::CameraIntrinsicsParameters));
|
||||
rgb_params.width = data.cameraModels()[0].imageWidth();
|
||||
rgb_params.height = data.cameraModels()[0].imageHeight();
|
||||
|
||||
rgb_params.fx = data.cameraModels()[0].fx();
|
||||
rgb_params.fy = data.cameraModels()[0].fy();
|
||||
rgb_params.cx = data.cameraModels()[0].cx()==0.0?double(rgb_params.width) / 2.0:data.cameraModels()[0].cx();
|
||||
rgb_params.cy = data.cameraModels()[0].cy()==0.0?double(rgb_params.height) / 2.0:data.cameraModels()[0].cy();
|
||||
localTransform = data.cameraModels()[0].localTransform();
|
||||
|
||||
if(rect_ == 0)
|
||||
{
|
||||
UINFO("Init rgbd fovis: %dx%d fx=%f fy=%f cx=%f cy=%f", rgb_params.width, rgb_params.height, rgb_params.fx, rgb_params.fy, rgb_params.cx, rgb_params.cy);
|
||||
rect_ = new fovis::Rectification(rgb_params);
|
||||
}
|
||||
|
||||
if(depthImage_ == 0)
|
||||
{
|
||||
depthImage_ = new fovis::DepthImage(rgb_params, rgb_params.width, rgb_params.height);
|
||||
}
|
||||
// make sure all zeros are NAN
|
||||
if(data.depthRaw().type() == CV_32FC1)
|
||||
{
|
||||
depth = data.depthRaw();
|
||||
for(int i=0; i<depth.rows; ++i)
|
||||
{
|
||||
for(int j=0; j<depth.cols; ++j)
|
||||
{
|
||||
float & d = depth.at<float>(i,j);
|
||||
if(d == 0.0f)
|
||||
{
|
||||
d = NAN;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(data.depthRaw().type() == CV_16UC1)
|
||||
{
|
||||
depth = 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.at<float>(i, j) = d==0.0f?NAN:d;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Unknown depth format!");
|
||||
}
|
||||
depthImage_->setDepthImage((float*)depth.data);
|
||||
depthSource = depthImage_;
|
||||
}
|
||||
else // stereo
|
||||
{
|
||||
// initialize left camera parameters
|
||||
fovis::CameraIntrinsicsParameters left_parameters;
|
||||
left_parameters.width = data.stereoCameraModel().left().imageWidth();
|
||||
left_parameters.height = data.stereoCameraModel().left().imageHeight();
|
||||
left_parameters.fx = data.stereoCameraModel().left().fx();
|
||||
left_parameters.fy = data.stereoCameraModel().left().fy();
|
||||
left_parameters.cx = data.stereoCameraModel().left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModel().left().cx();
|
||||
left_parameters.cy = data.stereoCameraModel().left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModel().left().cy();
|
||||
localTransform = data.stereoCameraModel().localTransform();
|
||||
|
||||
if(rect_ == 0)
|
||||
{
|
||||
UINFO("Init stereo fovis: %dx%d fx=%f fy=%f cx=%f cy=%f", left_parameters.width, left_parameters.height, left_parameters.fx, left_parameters.fy, left_parameters.cx, left_parameters.cy);
|
||||
rect_ = new fovis::Rectification(left_parameters);
|
||||
}
|
||||
|
||||
if(stereoCalib_ == 0)
|
||||
{
|
||||
// initialize right camera parameters
|
||||
fovis::CameraIntrinsicsParameters right_parameters;
|
||||
right_parameters.width = data.stereoCameraModel().right().imageWidth();
|
||||
right_parameters.height = data.stereoCameraModel().right().imageHeight();
|
||||
right_parameters.fx = data.stereoCameraModel().right().fx();
|
||||
right_parameters.fy = data.stereoCameraModel().right().fy();
|
||||
right_parameters.cx = data.stereoCameraModel().right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModel().right().cx();
|
||||
right_parameters.cy = data.stereoCameraModel().right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModel().right().cy();
|
||||
|
||||
// as we use rectified images, rotation is identity
|
||||
// and translation is baseline only
|
||||
fovis::StereoCalibrationParameters stereo_parameters;
|
||||
stereo_parameters.left_parameters = left_parameters;
|
||||
stereo_parameters.right_parameters = right_parameters;
|
||||
stereo_parameters.right_to_left_rotation[0] = 1.0;
|
||||
stereo_parameters.right_to_left_rotation[1] = 0.0;
|
||||
stereo_parameters.right_to_left_rotation[2] = 0.0;
|
||||
stereo_parameters.right_to_left_rotation[3] = 0.0;
|
||||
stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModel().baseline();
|
||||
stereo_parameters.right_to_left_translation[1] = 0.0;
|
||||
stereo_parameters.right_to_left_translation[2] = 0.0;
|
||||
|
||||
stereoCalib_ = new fovis::StereoCalibration(stereo_parameters);
|
||||
}
|
||||
|
||||
if(stereoDepth_ == 0)
|
||||
{
|
||||
stereoDepth_ = new fovis::StereoDepth(stereoCalib_, options);
|
||||
}
|
||||
if(data.rightRaw().type() == CV_8UC3)
|
||||
{
|
||||
cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
|
||||
}
|
||||
else if(data.rightRaw().type() == CV_8UC1)
|
||||
{
|
||||
right = data.rightRaw();
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Not supported color type!");
|
||||
}
|
||||
stereoDepth_->setRightImage(right.data);
|
||||
depthSource = stereoDepth_;
|
||||
}
|
||||
|
||||
if(fovis_ == 0)
|
||||
{
|
||||
fovis_ = new fovis::VisualOdometry(rect_, options);
|
||||
}
|
||||
|
||||
|
||||
fovis_->processFrame(gray.data, depthSource);
|
||||
|
||||
// get the motion estimate for this frame to the previous frame.
|
||||
t = Transform::fromEigen3d(fovis_->getMotionEstimate());
|
||||
|
||||
fovis::MotionEstimateStatusCode statusCode = fovis::SUCCESS;
|
||||
if(fovis_->getMotionEstimator())
|
||||
{
|
||||
statusCode = fovis_->getMotionEstimator()->getMotionEstimateStatus();
|
||||
}
|
||||
|
||||
if(statusCode > fovis::SUCCESS)
|
||||
{
|
||||
UWARN("Fovis error status: %s", fovis::MotionEstimateStatusCodeStrings[statusCode]);
|
||||
t.setNull();
|
||||
}
|
||||
|
||||
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)kTypeFovis;
|
||||
info->keyFrameAdded = fovis_->getChangeReferenceFrames();
|
||||
info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
|
||||
info->matches = fovis_->getMotionEstimator()->getNumMatches();
|
||||
info->inliers = fovis_->getMotionEstimator()->getNumInliers();
|
||||
const Eigen::MatrixXd& cov = fovis_->getMotionEstimator()->getMotionEstimateCov();
|
||||
if(cov.cols() == 6 && cov.rows() == 6 && cov(0,0) > 0.0)
|
||||
{
|
||||
info->covariance = cv::Mat::eye(6,6, CV_64FC1);
|
||||
memcpy(info->covariance.data, cov.data(), 36*sizeof(double));
|
||||
info->covariance *= 100.0; // to be in the same scale than loop closure detection
|
||||
}
|
||||
}
|
||||
|
||||
UINFO("Odom update time = %fs status=%s", timer.elapsed(), fovis::MotionEstimateStatusCodeStrings[statusCode]);
|
||||
|
||||
#else
|
||||
UERROR("RTAB-Map is not built with FOVIS support! Select another visual odometry approach.");
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
@@ -63,7 +63,7 @@
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-327</y>
|
||||
<y>0</y>
|
||||
<width>678</width>
|
||||
<height>2739</height>
|
||||
</rect>
|
||||
@@ -72,7 +72,16 @@
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -86,7 +95,7 @@
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>5</number>
|
||||
<number>18</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="page_22">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
|
||||
@@ -4521,7 +4530,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<string>Directory of images (optional settings)</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_93">
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -9681,6 +9699,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
<string>Frame to Frame</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>Fovis</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
@@ -11021,7 +11044,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
<widget class="QWidget" name="page_54">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_85">
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -11161,7 +11193,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
</widget>
|
||||
<widget class="QWidget" name="page_55">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_86">
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -11319,7 +11360,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -11399,7 +11449,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -11511,7 +11570,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
|
||||
Reference in New Issue
Block a user