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

fix build errors with opencv2, added Transform::getClosestTransform() function for convenience.

Browse Source
This commit is contained in:
matlabbe
2019-05-31 16:51:37 -04:00
parent d5d00fbd7d
commit 4c8af6d6e9
7 changed files with 51 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/IMU.h
View File

@@ -65,7 +65,6 @@ public:
return localTransform_.isNull();
}
private:
cv::Vec4d orientation_;
cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set

6
corelib/include/rtabmap/core/Transform.h
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <vector>
#include <string>
#include <map>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
@@ -149,6 +150,11 @@ public:
static Transform fromString(const std::string & string);
static bool canParseString(const std::string & string);
static Transform getClosestTransform(
const std::map<double, Transform> & tfBuffer,
const double & stamp,
double * stampDiff = 0);
private:
cv::Mat data_;
};

1
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -56,7 +56,6 @@ public:
private:
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
Transform getClosestIMU(const double & stamp, double & stampDiff) const;
private:
//Parameters

37
corelib/src/Transform.cpp
View File

@@ -467,4 +467,41 @@ bool Transform::canParseString(const std::string & string)
return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
}
Transform Transform::getClosestTransform(
const std::map<double, Transform> & tfBuffer,
const double & stamp,
double * stampDiff)
{
UASSERT(!tfBuffer.empty());
std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
if(imuIterA != tfBuffer.begin())
{
imuIterA = --imuIterA;
}
if(imuIterB == tfBuffer.end())
{
imuIterB = --imuIterB;
}
Transform imuT;
if(imuIterB->first == stamp || imuIterA == imuIterB)
{
imuT = imuIterB->second;
if(stampDiff)
{
*stampDiff = fabs(imuIterB->first - stamp);
}
}
else if(imuIterA != imuIterB)
{
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
if(stampDiff)
{
*stampDiff = 0.0;
}
}
return imuT;
}
}

11
corelib/src/camera/CameraVideo.cpp
View File

@@ -30,6 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#if CV_MAJOR_VERSION > 3
#include <opencv2/videoio/videoio_c.h>
#if CV_MAJOR_VERSION > 4
#include <opencv2/videoio/legacy/constants_c.h>
#endif
#endif
namespace rtabmap
@@ -131,13 +134,13 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
{
if(_model.isValidForProjection())
{
_capture.set(cv::CAP_PROP_FRAME_WIDTH, _model.imageWidth());
_capture.set(cv::CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
_capture.set(CV_CAP_PROP_FRAME_WIDTH, _model.imageWidth());
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
}
else if(_width > 0 && _height > 0)
{
_capture.set(cv::CAP_PROP_FRAME_WIDTH, _width);
_capture.set(cv::CAP_PROP_FRAME_HEIGHT, _height);
_capture.set(CV_CAP_PROP_FRAME_WIDTH, _width);
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
}
}
if(_rectifyImages && !_model.isValidForRectification())

42
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -335,8 +335,7 @@ Transform OdometryF2M::computeTransform(
Transform imuT;
if(!imus_.empty())
{
double stampDiff = 0.0;
imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
imuT = Transform::getClosestTransform(imus_, lastFrame_->getStamp());
}
// local bundle adjustment
@@ -1304,43 +1303,4 @@ Transform OdometryF2M::computeTransform(
return output;
}
Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) const
{
UASSERT(!imus_.empty());
std::map<double, Transform>::const_iterator imuIterB = imus_.lower_bound(stamp);
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
if(imuIterA != imus_.begin())
{
imuIterA = --imuIterA;
}
if(imuIterB == imus_.end())
{
imuIterB = --imuIterB;
}
Transform imuT;
stampDiff = 0.0;
if(imuIterB->first == lastFrame_->getStamp() || imuIterA == imuIterB)
{
imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
else if(imuIterA != imuIterB)
{
if(fabs(imuIterA->first - lastFrame_->getStamp()) <
fabs(imuIterB->first - lastFrame_->getStamp()))
{
//imuT = imuIterA->second;
stampDiff = fabs(imuIterA->first - lastFrame_->getStamp());
}
else
{
//imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
}
return imuT;
}
} // namespace rtabmap