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

Explicitly show licenses of optional dependencies on CMake

Browse Source
This commit is contained in:
matlabbe
2016-03-01 14:25:00 -05:00
parent e6e9c61c38
commit 1f999c4d28
20 changed files with 458 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
corelib/src/CMakeLists.txt
View File

@@ -64,14 +64,7 @@ SET(SRC_FILES
Stereo.cpp
StereoDense.cpp
StereoCameraModel.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp
toro3d/posegraph2.cpp
toro3d/treeoptimizer2.cpp
rtflann/ext/lz4.c
rtflann/ext/lz4hc.c
@@ -95,7 +88,6 @@ SET(LIBRARIES
)
IF(Freenect_FOUND)
ADD_DEFINITIONS("-DWITH_FREENECT")
IF(Freenect_DASH_INCLUDES)
ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")
ENDIF(Freenect_DASH_INCLUDES)
@@ -110,7 +102,6 @@ IF(Freenect_FOUND)
ENDIF(Freenect_FOUND)
IF(OpenNI2_FOUND)
ADD_DEFINITIONS("-DWITH_OPENNI2")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${OpenNI2_INCLUDE_DIRS}
@@ -122,7 +113,6 @@ IF(OpenNI2_FOUND)
ENDIF(OpenNI2_FOUND)
IF(freenect2_FOUND)
ADD_DEFINITIONS("-DWITH_FREENECT2")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${freenect2_INCLUDE_DIRS}
@@ -134,7 +124,6 @@ IF(freenect2_FOUND)
ENDIF(freenect2_FOUND)
IF(DC1394_FOUND)
ADD_DEFINITIONS("-DWITH_DC1394")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${DC1394_INCLUDE_DIRS}
@@ -146,7 +135,6 @@ IF(DC1394_FOUND)
ENDIF(DC1394_FOUND)
IF(FlyCapture2_FOUND)
ADD_DEFINITIONS("-DWITH_FLYCAPTURE2")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${FlyCapture2_INCLUDE_DIRS}
@@ -157,8 +145,23 @@ IF(FlyCapture2_FOUND)
)
ENDIF(FlyCapture2_FOUND)
IF(WITH_TORO)
SET(LIBRARIES
${LIBRARIES}
${G2O_LIBRARIES}
)
SET(SRC_FILES
${SRC_FILES}
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp
toro3d/posegraph2.cpp
toro3d/treeoptimizer2.cpp
)
ENDIF(WITH_TORO)
IF(G2O_FOUND)
ADD_DEFINITIONS("-DWITH_G2O")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
@@ -168,18 +171,19 @@ IF(G2O_FOUND)
${G2O_LIBRARIES}
)
SET(SRC_FILES
${SRC_FILES}
vertigo/g2o/edge_se2Switchable.cpp
vertigo/g2o/edge_se3Switchable.cpp
vertigo/g2o/edge_switchPrior.cpp
vertigo/g2o/types_g2o_robust.cpp
vertigo/g2o/vertex_switchLinear.cpp
)
IF(WITH_VERTIGO)
SET(SRC_FILES
${SRC_FILES}
vertigo/g2o/edge_se2Switchable.cpp
vertigo/g2o/edge_se3Switchable.cpp
vertigo/g2o/edge_switchPrior.cpp
vertigo/g2o/types_g2o_robust.cpp
vertigo/g2o/vertex_switchLinear.cpp
)
ENDIF(WITH_VERTIGO)
ENDIF(G2O_FOUND)
IF(GTSAM_FOUND)
ADD_DEFINITIONS("-DWITH_GTSAM")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${GTSAM_INCLUDE_DIRS}
@@ -191,7 +195,6 @@ IF(GTSAM_FOUND)
ENDIF(GTSAM_FOUND)
IF(cvsba_FOUND)
ADD_DEFINITIONS("-DWITH_CVSBA")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${cvsba_INCLUDE_DIRS}

64
corelib/src/CameraRGBD.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Version.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
@@ -46,7 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/io/openni_camera/openni_image.h>
#endif
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
#include <libfreenect.h>
#ifdef FREENECT_DASH_INCLUDES
#include <libfreenect-registration.h>
@@ -55,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#endif
#endif
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
@@ -63,20 +64,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <libfreenect2/config.h>
#endif
#ifdef WITH_DC1394
#include <dc1394/dc1394.h>
#endif
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
#include <OniVersion.h>
#include <OpenNI.h>
#endif
#ifdef WITH_FLYCAPTURE2
#include <triclops.h>
#include <fc2triclops.h>
#endif
namespace rtabmap
{
@@ -361,7 +353,7 @@ SensorData CameraOpenNICV::captureImage()
/////////////////////////
bool CameraOpenNI2::available()
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
return true;
#else
return false;
@@ -382,7 +374,7 @@ CameraOpenNI2::CameraOpenNI2(
float imageRate,
const rtabmap::Transform & localTransform) :
Camera(imageRate, localTransform),
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
_device(new openni::Device()),
_color(new openni::VideoStream()),
_depth(new openni::VideoStream()),
@@ -400,7 +392,7 @@ CameraOpenNI2::CameraOpenNI2(
CameraOpenNI2::~CameraOpenNI2()
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
_color->stop();
_color->destroy();
_depth->stop();
@@ -416,7 +408,7 @@ CameraOpenNI2::~CameraOpenNI2()
bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
if(_color && _color->getCameraSettings())
{
return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
@@ -429,7 +421,7 @@ bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
bool CameraOpenNI2::setAutoExposure(bool enabled)
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
if(_color && _color->getCameraSettings())
{
return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
@@ -442,7 +434,7 @@ bool CameraOpenNI2::setAutoExposure(bool enabled)
bool CameraOpenNI2::setExposure(int value)
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
if(_color && _color->getCameraSettings())
{
@@ -459,7 +451,7 @@ bool CameraOpenNI2::setExposure(int value)
bool CameraOpenNI2::setGain(int value)
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
if(_color && _color->getCameraSettings())
{
@@ -476,7 +468,7 @@ bool CameraOpenNI2::setGain(int value)
bool CameraOpenNI2::setMirroring(bool enabled)
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
if(_color->isValid() && _depth->isValid())
{
return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
@@ -488,7 +480,7 @@ bool CameraOpenNI2::setMirroring(bool enabled)
bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
openni::OpenNI::initialize();
if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
@@ -666,7 +658,7 @@ bool CameraOpenNI2::isCalibrated() const
std::string CameraOpenNI2::getSerial() const
{
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
if(_device)
{
return _device->getDeviceInfo().getName();
@@ -678,7 +670,7 @@ std::string CameraOpenNI2::getSerial() const
SensorData CameraOpenNI2::captureImage()
{
SensorData data;
#ifdef WITH_OPENNI2
#ifdef RTABMAP_OPENNI2
int readyStream = -1;
if(_device->isValid() &&
_depth->isValid() &&
@@ -740,7 +732,7 @@ SensorData CameraOpenNI2::captureImage()
return data;
}
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
//
// FreenectDevice
//
@@ -947,7 +939,7 @@ private:
//
bool CameraFreenect::available()
{
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
return true;
#else
return false;
@@ -960,7 +952,7 @@ CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform &
ctx_(0),
freenectDevice_(0)
{
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
// claim camera
freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
@@ -969,7 +961,7 @@ CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform &
CameraFreenect::~CameraFreenect()
{
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
if(freenectDevice_)
{
freenectDevice_->join(true);
@@ -985,7 +977,7 @@ CameraFreenect::~CameraFreenect()
bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
if(freenectDevice_)
{
freenectDevice_->join(true);
@@ -1026,7 +1018,7 @@ bool CameraFreenect::isCalibrated() const
std::string CameraFreenect::getSerial() const
{
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
if(freenectDevice_)
{
return freenectDevice_->getSerial();
@@ -1038,7 +1030,7 @@ std::string CameraFreenect::getSerial() const
SensorData CameraFreenect::captureImage()
{
SensorData data;
#ifdef WITH_FREENECT
#ifdef RTABMAP_FREENECT
if(ctx_ && freenectDevice_)
{
if(freenectDevice_->isRunning())
@@ -1078,7 +1070,7 @@ SensorData CameraFreenect::captureImage()
//
bool CameraFreenect2::available()
{
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
return true;
#else
return false;
@@ -1108,7 +1100,7 @@ CameraFreenect2::CameraFreenect2(
edgeAwareFiltering_(edgeAwareFiltering),
noiseFiltering_(noiseFiltering)
{
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
freenect2_ = new libfreenect2::Freenect2();
switch(type_)
@@ -1131,7 +1123,7 @@ CameraFreenect2::CameraFreenect2(
CameraFreenect2::~CameraFreenect2()
{
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
UDEBUG("");
if(dev_)
{
@@ -1160,7 +1152,7 @@ CameraFreenect2::~CameraFreenect2()
bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
if(dev_)
{
dev_->stop();
@@ -1298,7 +1290,7 @@ bool CameraFreenect2::isCalibrated() const
std::string CameraFreenect2::getSerial() const
{
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
if(dev_)
{
return dev_->getSerialNumber();
@@ -1310,7 +1302,7 @@ std::string CameraFreenect2::getSerial() const
SensorData CameraFreenect2::captureImage()
{
SensorData data;
#ifdef WITH_FREENECT2
#ifdef RTABMAP_FREENECT2
if(dev_ && listener_)
{
libfreenect2::FrameMap frames;

35
corelib/src/CameraStereo.cpp
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/Version.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
@@ -38,11 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
#include <dc1394/dc1394.h>
#endif
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
#include <triclops.h>
#include <fc2triclops.h>
#endif
@@ -55,7 +56,7 @@ namespace rtabmap
// Inspired from ROS camera1394stereo package
//
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
class DC1394Device
{
public:
@@ -334,7 +335,7 @@ private:
bool CameraStereoDC1394::available()
{
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
return true;
#else
return false;
@@ -345,14 +346,14 @@ CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localT
Camera(imageRate, localTransform),
device_(0)
{
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
device_ = new DC1394Device();
#endif
}
CameraStereoDC1394::~CameraStereoDC1394()
{
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
if(device_)
{
delete device_;
@@ -362,7 +363,7 @@ CameraStereoDC1394::~CameraStereoDC1394()
bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
if(device_)
{
bool ok = device_->init();
@@ -401,7 +402,7 @@ bool CameraStereoDC1394::isCalibrated() const
std::string CameraStereoDC1394::getSerial() const
{
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
if(device_)
{
return device_->guid();
@@ -413,7 +414,7 @@ std::string CameraStereoDC1394::getSerial() const
SensorData CameraStereoDC1394::captureImage()
{
SensorData data;
#ifdef WITH_DC1394
#ifdef RTABMAP_DC1394
if(device_)
{
cv::Mat left, right;
@@ -458,14 +459,14 @@ CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transfor
camera_(0),
triclopsCtx_(0)
{
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
camera_ = new FlyCapture2::Camera();
#endif
}
CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
{
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
// Close the camera
camera_->StopCapture();
camera_->Disconnect();
@@ -479,7 +480,7 @@ CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
bool CameraStereoFlyCapture2::available()
{
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
return true;
#else
return false;
@@ -488,7 +489,7 @@ bool CameraStereoFlyCapture2::available()
bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
if(camera_)
{
// Close the camera
@@ -567,7 +568,7 @@ bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const
bool CameraStereoFlyCapture2::isCalibrated() const
{
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
if(triclopsCtx_)
{
float fx, cx, cy, baseline;
@@ -582,7 +583,7 @@ bool CameraStereoFlyCapture2::isCalibrated() const
std::string CameraStereoFlyCapture2::getSerial() const
{
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
if(camera_ && camera_->IsConnected())
{
FlyCapture2::CameraInfo camInfo;
@@ -596,7 +597,7 @@ std::string CameraStereoFlyCapture2::getSerial() const
}
// struct containing image needed for processing
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
struct ImageContainer
{
FlyCapture2::Image tmp[2];
@@ -607,7 +608,7 @@ struct ImageContainer
SensorData CameraStereoFlyCapture2::captureImage()
{
SensorData data;
#ifdef WITH_FLYCAPTURE2
#ifdef RTABMAP_FLYCAPTURE2
if(camera_ && triclopsCtx_ && camera_->IsConnected())
{
// grab image from camera.

47
corelib/src/Features2d.cpp
View File

@@ -419,28 +419,27 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
}
Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
{
if(RTABMAP_NONFREE == 0)
#ifndef RTABMAP_NONFREE
if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
{
if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
{
#if CV_MAJOR_VERSION < 3
UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
#else
UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
#endif
type = Feature2D::kFeatureOrb;
}
#if CV_MAJOR_VERSION == 3
if(type == Feature2D::kFeatureFastBrief ||
type == Feature2D::kFeatureFastFreak ||
type == Feature2D::kFeatureGfttBrief ||
type == Feature2D::kFeatureGfttFreak)
{
UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
type = Feature2D::kFeatureOrb;
}
UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
#endif
type = Feature2D::kFeatureOrb;
}
#if CV_MAJOR_VERSION == 3
if(type == Feature2D::kFeatureFastBrief ||
type == Feature2D::kFeatureFastFreak ||
type == Feature2D::kFeatureGfttBrief ||
type == Feature2D::kFeatureGfttFreak)
{
UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
type = Feature2D::kFeatureOrb;
}
#endif
#endif
Feature2D * feature2D = 0;
switch(type)
@@ -475,7 +474,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
case Feature2D::kFeatureBrisk:
feature2D = new BRISK(parameters);
break;
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
default:
feature2D = new SURF(parameters);
type = Feature2D::kFeatureSurf;
@@ -697,7 +696,7 @@ void SURF::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
#if CV_MAJOR_VERSION < 3
if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
{
@@ -733,7 +732,7 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
cv::Mat imgRoi(image, roi);
cv::Mat maskRoi;
if(!mask.empty())
@@ -766,7 +765,7 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
if(gpuVersion_)
{
#if CV_MAJOR_VERSION < 3
@@ -825,7 +824,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
#if CV_MAJOR_VERSION < 3
_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
#else
@@ -840,7 +839,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
cv::Mat imgRoi(image, roi);
cv::Mat maskRoi;
if(!mask.empty())
@@ -858,7 +857,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
#if RTABMAP_NONFREE == 1
#ifdef RTABMAP_NONFREE
_sift->compute(image, keypoints, descriptors);
#else
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");

59
corelib/src/Optimizer.cpp
View File

@@ -57,7 +57,7 @@ bool Optimizer::isAvailable(Optimizer::Type type)
}
else if(type == Optimizer::kTypeTORO)
{
return true;
return OptimizerTORO::available();
}
return false;
}
@@ -72,20 +72,65 @@ Optimizer * Optimizer::create(const ParametersMap & parameters)
Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & parameters)
{
UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available(),
"RTAB-Map is not built with any graph optimization approach!");
if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
{
if(OptimizerGTSAM::available())
{
UWARN("TORO optimizer not available. GTSAM will be used instead.");
type = Optimizer::kTypeGTSAM;
}
else if(OptimizerG2O::available())
{
UWARN("TORO optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O;
}
}
if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
{
UWARN("g2o optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
if(OptimizerTORO::available())
{
UWARN("g2o optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
else if(OptimizerGTSAM::available())
{
UWARN("g2o optimizer not available. GTSAM will be used instead.");
type = Optimizer::kTypeGTSAM;
}
}
if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
{
UWARN("GTSAM optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
if(OptimizerTORO::available())
{
UWARN("GTSAM optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
else if(OptimizerG2O::available())
{
UWARN("GTSAM optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O;
}
}
if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
{
UWARN("CVSBA optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
if(OptimizerTORO::available())
{
UWARN("CVSBA optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
else if(OptimizerGTSAM::available())
{
UWARN("CVSBA optimizer not available. GTSAM will be used instead.");
type = Optimizer::kTypeGTSAM;
}
else if(OptimizerG2O::available())
{
UWARN("CVSBA optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O;
}
}
Optimizer * optimizer = 0;
switch(type)

6
corelib/src/OptimizerCVSBA.cpp
View File

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OptimizerCVSBA.h>
#ifdef WITH_CVSBA
#ifdef RTABMAP_CVSBA
#include <cvsba/cvsba.h>
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d_transforms.h"
@@ -46,7 +46,7 @@ namespace rtabmap {
bool OptimizerCVSBA::available()
{
#ifdef WITH_CVSBA
#ifdef RTABMAP_CVSBA
return true;
#else
return false;
@@ -59,7 +59,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
{
#ifdef WITH_CVSBA
#ifdef RTABMAP_CVSBA
// run sba optimization
cvsba::Sba sba;

25
corelib/src/OptimizerG2O.cpp
View File

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OptimizerG2O.h>
#ifdef WITH_G2O
#ifdef RTABMAP_G2O
#include "g2o/config.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
@@ -63,18 +63,20 @@ typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSpa
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
#endif
#ifdef RTABMAP_VERTIGO
#include "vertigo/g2o/edge_switchPrior.h"
#include "vertigo/g2o/edge_se2Switchable.h"
#include "vertigo/g2o/edge_se3Switchable.h"
#include "vertigo/g2o/vertex_switchLinear.h"
#endif
#endif // end WITH_G2O
#endif // end RTABMAP_G2O
namespace rtabmap {
bool OptimizerG2O::available()
{
#ifdef WITH_G2O
#ifdef RTABMAP_G2O
return true;
#else
return false;
@@ -132,8 +134,17 @@ std::map<int, Transform> OptimizerG2O::optimize(
int * iterationsDone)
{
std::map<int, Transform> optimizedPoses;
#ifdef WITH_G2O
#ifdef RTABMAP_G2O
UDEBUG("Optimizing graph...");
#ifndef RTABMAP_VERTIGO
if(this->isRobust())
{
UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
setRobust(false);
}
#endif
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
@@ -226,6 +237,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::HyperGraph::Edge * edge = 0;
#ifdef RTABMAP_VERTIGO
VertexSwitchLinear * v = 0;
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
@@ -253,6 +265,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
#endif
if(isSlam2d())
{
@@ -270,6 +283,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -287,6 +301,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
edge = e;
}
else
#endif
{
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -313,6 +328,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
constraint = a.rotation();
constraint.translation() = a.translation();
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -330,6 +346,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
edge = e;
}
else
#endif
{
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);

25
corelib/src/OptimizerGTSAM.cpp
View File

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OptimizerGTSAM.h>
#ifdef WITH_GTSAM
#ifdef RTABMAP_GTSAM
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
@@ -50,17 +50,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#ifdef RTABMAP_VERTIGO
#include "vertigo/gtsam/betweenFactorMaxMix.h"
#include "vertigo/gtsam/betweenFactorSwitchable.h"
#include "vertigo/gtsam/switchVariableLinear.h"
#include "vertigo/gtsam/switchVariableSigmoid.h"
#endif // end WITH_GTSAM
#endif
#endif // end RTABMAP_GTSAM
namespace rtabmap {
bool OptimizerGTSAM::available()
{
#ifdef WITH_GTSAM
#ifdef RTABMAP_GTSAM
return true;
#else
return false;
@@ -76,7 +78,16 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
int * iterationsDone)
{
std::map<int, Transform> optimizedPoses;
#ifdef WITH_GTSAM
#ifdef RTABMAP_GTSAM
#ifndef RTABMAP_VERTIGO
if(this->isRobust())
{
UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
setRobust(false);
}
#endif
UDEBUG("Optimizing graph...");
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
@@ -120,6 +131,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UASSERT(!iter->second.transform().isNull());
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -140,6 +152,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
#endif
if(isSlam2d())
{
@@ -159,6 +172,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -167,6 +181,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
@@ -183,6 +198,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -191,6 +207,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}

15
corelib/src/OptimizerTORO.cpp
View File

@@ -35,11 +35,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OptimizerTORO.h>
#ifdef RTABMAP_TORO
#include "toro3d/treeoptimizer3.hh"
#include "toro3d/treeoptimizer2.hh"
#endif
namespace rtabmap {
bool OptimizerTORO::available()
{
#ifdef RTABMAP_TORO
return true;
#else
return false;
#endif
}
std::map<int, Transform> OptimizerTORO::optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -49,6 +60,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
int * iterationsDone)
{
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_TORO
UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
@@ -302,6 +314,9 @@ std::map<int, Transform> OptimizerTORO::optimize(
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with TORO support!");
#endif
return optimizedPoses;
}

4
corelib/src/toro3d/treeoptimizer2.cpp
View File

@@ -266,8 +266,8 @@ void TreeOptimizer2::propagateErrors(){
updatePoseChain(v1,top);
updatePoseChain(v2,top);
Pose pf1=v1->pose;
Pose pf2=v2->pose;
//Pose pf1=v1->pose;
//Pose pf2=v2->pose;
//DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
//DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;