mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 00:57:46 +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_REALSENSE_SLAM "Include RealSenseSlam support" ON)
|
||||||
option(WITH_OCTOMAP "Include Octomap support" ON)
|
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(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)
|
||||||
@@ -344,6 +345,13 @@ IF(WITH_CPUTSDF)
|
|||||||
ENDIF(CPUTSDF_FOUND)
|
ENDIF(CPUTSDF_FOUND)
|
||||||
ENDIF(WITH_CPUTSDF)
|
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)
|
IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
|
||||||
#Newest versions require std11
|
#Newest versions require std11
|
||||||
IF(NOT MSVC)
|
IF(NOT MSVC)
|
||||||
@@ -466,6 +474,11 @@ IF(NOT CPUTSDF_FOUND)
|
|||||||
ELSE()
|
ELSE()
|
||||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES})
|
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES})
|
||||||
ENDIF()
|
ENDIF()
|
||||||
|
IF(NOT libfovis_FOUND)
|
||||||
|
SET(FOVIS "//")
|
||||||
|
ELSE()
|
||||||
|
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_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)
|
||||||
@@ -789,6 +802,14 @@ ELSE()
|
|||||||
MESSAGE(STATUS " With CPUTSDF = NO (CPUTSDF not found)")
|
MESSAGE(STATUS " With CPUTSDF = NO (CPUTSDF not found)")
|
||||||
ENDIF()
|
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)
|
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)
|
||||||
|
|||||||
@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
|
@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
|
||||||
@OCTOMAP@#define RTABMAP_OCTOMAP
|
@OCTOMAP@#define RTABMAP_OCTOMAP
|
||||||
@CPUTSDF@#define RTABMAP_CPUTSDF
|
@CPUTSDF@#define RTABMAP_CPUTSDF
|
||||||
|
@FOVIS@#define RTABMAP_FOVIS
|
||||||
|
|
||||||
#endif /* VERSION_H_ */
|
#endif /* VERSION_H_ */
|
||||||
|
|
||||||
|
|||||||
@@ -45,7 +45,8 @@ public:
|
|||||||
enum Type {
|
enum Type {
|
||||||
kTypeUndef = -1,
|
kTypeUndef = -1,
|
||||||
kTypeF2M = 0,
|
kTypeF2M = 0,
|
||||||
kTypeF2F = 1
|
kTypeF2F = 1,
|
||||||
|
kTypeFovis = 2
|
||||||
};
|
};
|
||||||
|
|
||||||
public:
|
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");
|
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
|
||||||
|
|
||||||
// Odometry
|
// 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, 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, 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).");
|
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
|
||||||
|
|||||||
@@ -60,6 +60,7 @@ SET(SRC_FILES
|
|||||||
OdometryF2M.cpp
|
OdometryF2M.cpp
|
||||||
OdometryMono.cpp
|
OdometryMono.cpp
|
||||||
OdometryF2F.cpp
|
OdometryF2F.cpp
|
||||||
|
OdometryFovis.cpp
|
||||||
|
|
||||||
Stereo.cpp
|
Stereo.cpp
|
||||||
StereoDense.cpp
|
StereoDense.cpp
|
||||||
@@ -275,6 +276,17 @@ IF(OCTOMAP_FOUND)
|
|||||||
)
|
)
|
||||||
ENDIF(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
|
# 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/OdometryF2M.h>
|
||||||
#include "rtabmap/core/Odometry.h"
|
#include "rtabmap/core/Odometry.h"
|
||||||
#include "rtabmap/core/OdometryF2F.h"
|
#include "rtabmap/core/OdometryF2F.h"
|
||||||
|
#include "rtabmap/core/OdometryFovis.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"
|
||||||
@@ -56,6 +57,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
|
|||||||
Odometry * odometry = 0;
|
Odometry * odometry = 0;
|
||||||
switch(type)
|
switch(type)
|
||||||
{
|
{
|
||||||
|
case Odometry::kTypeFovis:
|
||||||
|
odometry = new OdometryFovis(parameters);
|
||||||
|
break;
|
||||||
case Odometry::kTypeF2F:
|
case Odometry::kTypeF2F:
|
||||||
odometry = new OdometryF2F(parameters);
|
odometry = new OdometryF2F(parameters);
|
||||||
break;
|
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">
|
<property name="geometry">
|
||||||
<rect>
|
<rect>
|
||||||
<x>0</x>
|
<x>0</x>
|
||||||
<y>-327</y>
|
<y>0</y>
|
||||||
<width>678</width>
|
<width>678</width>
|
||||||
<height>2739</height>
|
<height>2739</height>
|
||||||
</rect>
|
</rect>
|
||||||
@@ -72,7 +72,16 @@
|
|||||||
<property name="spacing">
|
<property name="spacing">
|
||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
@@ -86,7 +95,7 @@
|
|||||||
<enum>QFrame::Raised</enum>
|
<enum>QFrame::Raised</enum>
|
||||||
</property>
|
</property>
|
||||||
<property name="currentIndex">
|
<property name="currentIndex">
|
||||||
<number>5</number>
|
<number>18</number>
|
||||||
</property>
|
</property>
|
||||||
<widget class="QWidget" name="page_22">
|
<widget class="QWidget" name="page_22">
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
|
<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>
|
<string>Directory of images (optional settings)</string>
|
||||||
</property>
|
</property>
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_93">
|
<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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<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>
|
<string>Frame to Frame</string>
|
||||||
</property>
|
</property>
|
||||||
</item>
|
</item>
|
||||||
|
<item>
|
||||||
|
<property name="text">
|
||||||
|
<string>Fovis</string>
|
||||||
|
</property>
|
||||||
|
</item>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="2" column="1">
|
<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>
|
</property>
|
||||||
<widget class="QWidget" name="page_54">
|
<widget class="QWidget" name="page_54">
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_85">
|
<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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
@@ -11161,7 +11193,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
|||||||
</widget>
|
</widget>
|
||||||
<widget class="QWidget" name="page_55">
|
<widget class="QWidget" name="page_55">
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_86">
|
<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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
@@ -11319,7 +11360,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
|||||||
<property name="spacing">
|
<property name="spacing">
|
||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
@@ -11399,7 +11449,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
|||||||
<property name="spacing">
|
<property name="spacing">
|
||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
@@ -11511,7 +11570,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
|||||||
<property name="spacing">
|
<property name="spacing">
|
||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</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>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
|
|||||||
Reference in New Issue
Block a user