mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-03 16:47:47 +08:00
Added Odometry tests (base class only)
This commit is contained in:
matlabbe
@@ -1,138 +1,195 @@
|
||||
/*
|
||||
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 ODOMETRY_H_
|
||||
#define ODOMETRY_H_
|
||||
|
||||
#include <rtabmap/core/rtabmap_core_export.h>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class OdometryInfo;
|
||||
class ParticleFilter;
|
||||
|
||||
class RTABMAP_CORE_EXPORT Odometry
|
||||
{
|
||||
public:
|
||||
enum Type {
|
||||
kTypeUndef = -1,
|
||||
kTypeF2M = 0,
|
||||
kTypeF2F = 1,
|
||||
kTypeFovis = 2,
|
||||
kTypeViso2 = 3,
|
||||
kTypeDVO = 4,
|
||||
kTypeORBSLAM = 5,
|
||||
kTypeOkvis = 6,
|
||||
kTypeLOAM = 7,
|
||||
kTypeMSCKF = 8,
|
||||
kTypeVINSFusion = 9,
|
||||
kTypeOpenVINS = 10,
|
||||
kTypeFLOAM = 11,
|
||||
kTypeOpen3D = 12,
|
||||
kTypeCuVSLAM = 13,
|
||||
kTypeLIOSAM = 14
|
||||
};
|
||||
|
||||
public:
|
||||
static Odometry * create(const ParametersMap & parameters = ParametersMap());
|
||||
static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
|
||||
|
||||
public:
|
||||
virtual ~Odometry();
|
||||
Transform process(SensorData & data, OdometryInfo * info = 0);
|
||||
Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
virtual Odometry::Type getType() = 0;
|
||||
virtual bool canProcessRawImages() const {return false;}
|
||||
virtual bool canProcessAsyncIMU() const {return false;}
|
||||
|
||||
//getters
|
||||
const Transform & getPose() const {return _pose;}
|
||||
bool isInfoDataFilled() const {return _fillInfoData;}
|
||||
// Use getVelocityGuess() instead.
|
||||
RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
|
||||
const Transform & getVelocityGuess() const {return velocityGuess_;}
|
||||
double previousStamp() const {return previousStamp_;}
|
||||
unsigned int framesProcessed() const {return framesProcessed_;}
|
||||
bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
|
||||
|
||||
protected:
|
||||
const std::map<double, Transform> & imus() const {return imus_;}
|
||||
|
||||
private:
|
||||
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
|
||||
|
||||
void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
|
||||
void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0);
|
||||
void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw);
|
||||
|
||||
private:
|
||||
int _resetCountdown;
|
||||
bool _force3DoF;
|
||||
bool _holonomic;
|
||||
bool guessFromMotion_;
|
||||
float guessSmoothingDelay_;
|
||||
int _filteringStrategy;
|
||||
int _particleSize;
|
||||
float _particleNoiseT;
|
||||
float _particleLambdaT;
|
||||
float _particleNoiseR;
|
||||
float _particleLambdaR;
|
||||
bool _fillInfoData;
|
||||
float _kalmanProcessNoise;
|
||||
float _kalmanMeasurementNoise;
|
||||
unsigned int _imageDecimation;
|
||||
bool _alignWithGround;
|
||||
bool _publishRAMUsage;
|
||||
bool _imagesAlreadyRectified;
|
||||
bool _deskewing;
|
||||
Transform _pose;
|
||||
int _resetCurrentCount;
|
||||
double previousStamp_;
|
||||
std::list<std::pair<std::vector<float>, double> > previousVelocities_;
|
||||
Transform velocityGuess_;
|
||||
Transform imuLastTransform_;
|
||||
Transform previousGroundTruthPose_;
|
||||
float distanceTravelled_;
|
||||
unsigned int framesProcessed_;
|
||||
|
||||
std::vector<ParticleFilter *> particleFilters_;
|
||||
cv::KalmanFilter kalmanFilter_;
|
||||
std::vector<StereoCameraModel> stereoModels_;
|
||||
std::vector<CameraModel> models_;
|
||||
std::map<double, Transform> imus_;
|
||||
|
||||
protected:
|
||||
Odometry(const rtabmap::ParametersMap & parameters);
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
#endif /* ODOMETRY_H_ */
|
||||
/*
|
||||
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 ODOMETRY_H_
|
||||
#define ODOMETRY_H_
|
||||
|
||||
#include <rtabmap/core/rtabmap_core_export.h>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class OdometryInfo;
|
||||
class ParticleFilter;
|
||||
|
||||
/**
|
||||
* @class Odometry
|
||||
* @brief Abstract base class for visual, lidar and visual-inertial odometry backends.
|
||||
*
|
||||
* Odometry estimates the incremental motion between consecutive @ref SensorData frames.
|
||||
* Concrete implementations override @ref computeTransform(); the public @ref process()
|
||||
* pipeline handles IMU caching, optional motion guesses, filtering (Kalman or particle),
|
||||
* image decimation, deskewing and pose integration.
|
||||
*
|
||||
* Use @ref create() to instantiate a backend from @ref Parameters::kOdomStrategy().
|
||||
*
|
||||
* @see OdometryThread
|
||||
* @see OdometryInfo
|
||||
*/
|
||||
class RTABMAP_CORE_EXPORT Odometry
|
||||
{
|
||||
public:
|
||||
/** @brief Odometry backend selected by @ref Parameters::kOdomStrategy(). */
|
||||
enum Type {
|
||||
kTypeUndef = -1, /**< Undefined / invalid type. */
|
||||
kTypeF2M = 0, /**< Frame-to-map (default). */
|
||||
kTypeF2F = 1, /**< Frame-to-frame. */
|
||||
kTypeFovis = 2, /**< FOVIS stereo visual odometry. */
|
||||
kTypeViso2 = 3, /**< libviso2. */
|
||||
kTypeDVO = 4, /**< Dense visual odometry. */
|
||||
kTypeORBSLAM = 5, /**< ORB-SLAM 2/3. */
|
||||
kTypeOkvis = 6, /**< OKVIS. */
|
||||
kTypeLOAM = 7, /**< LOAM lidar odometry. */
|
||||
kTypeMSCKF = 8, /**< MSCKF visual-inertial. */
|
||||
kTypeVINSFusion = 9,/**< VINS-Fusion. */
|
||||
kTypeOpenVINS = 10, /**< OpenVINS. */
|
||||
kTypeFLOAM = 11, /**< FLOAM lidar odometry. */
|
||||
kTypeOpen3D = 12, /**< Open3D RGB-D odometry. */
|
||||
kTypeCuVSLAM = 13, /**< cuVSLAM. */
|
||||
kTypeLIOSAM = 14 /**< LIO-SAM. */
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Creates an odometry instance from @ref Parameters::kOdomStrategy() in @p parameters.
|
||||
* @param parameters RTAB-Map parameters (odometry strategy and related options).
|
||||
* @return New odometry object (caller owns the pointer). Falls back to @ref kTypeF2M if the type is unknown.
|
||||
*/
|
||||
static Odometry * create(const ParametersMap & parameters = ParametersMap());
|
||||
/**
|
||||
* @brief Creates an odometry instance of a given @p type.
|
||||
* @param type In/out odometry type; updated to @ref kTypeF2M if @p type is unknown.
|
||||
* @param parameters RTAB-Map parameters passed to the concrete backend.
|
||||
*/
|
||||
static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
|
||||
|
||||
virtual ~Odometry();
|
||||
/**
|
||||
* @brief Processes a sensor frame and updates the integrated pose.
|
||||
* @param data Input sensor data (must have @c id() >= 0). May be modified in place (decompression, deskewing).
|
||||
* @param info Optional output statistics and debug data.
|
||||
* @return Updated integrated pose (@ref getPose()) after the frame is processed,
|
||||
* or a null transform if odometry is lost. The incremental transform is
|
||||
* available in @ref OdometryInfo::transform when @p info is provided.
|
||||
*/
|
||||
Transform process(SensorData & data, OdometryInfo * info = 0);
|
||||
/**
|
||||
* @brief Processes a sensor frame with an external motion guess.
|
||||
* @param data Input sensor data.
|
||||
* @param guess Optional prior on the incremental transform (used by the backend when supported).
|
||||
* @param info Optional output statistics and debug data.
|
||||
*/
|
||||
Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
|
||||
/**
|
||||
* @brief Resets internal state and sets the initial pose.
|
||||
* @param initialPose Starting pose (must not be null). Z/roll/pitch may be cleared if @ref Parameters::kRegForce3DoF() is enabled.
|
||||
*/
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
/** @return Concrete odometry backend type. */
|
||||
virtual Odometry::Type getType() = 0;
|
||||
/** @return True if the backend can process unrectified camera images. */
|
||||
virtual bool canProcessRawImages() const {return false;}
|
||||
/** @return True if the backend processes IMU asynchronously outside @ref process(). */
|
||||
virtual bool canProcessAsyncIMU() const {return false;}
|
||||
|
||||
/** @return Current integrated odometry pose. */
|
||||
const Transform & getPose() const {return _pose;}
|
||||
/** @return True if @ref OdometryInfo debug/statistics fields are filled in @ref process(). */
|
||||
bool isInfoDataFilled() const {return _fillInfoData;}
|
||||
/** @deprecated Use @ref getVelocityGuess() instead. */
|
||||
RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
|
||||
/** @return Last estimated velocity used for motion guessing (may be null). */
|
||||
const Transform & getVelocityGuess() const {return velocityGuess_;}
|
||||
/** @return Timestamp of the previously processed frame. */
|
||||
double previousStamp() const {return previousStamp_;}
|
||||
/** @return Number of frames processed since the last @ref reset(). */
|
||||
unsigned int framesProcessed() const {return framesProcessed_;}
|
||||
/** @return True if input images are already rectified (see @ref Parameters::kRtabmapImagesAlreadyRectified()). */
|
||||
bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
|
||||
|
||||
protected:
|
||||
/** @return IMU orientations cached from recent frames (stamp → transform). */
|
||||
const std::map<double, Transform> & imus() const {return imus_;}
|
||||
|
||||
/** @brief Constructs the base odometry state from RTAB-Map parameters. */
|
||||
Odometry(const rtabmap::ParametersMap & parameters);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Computes the incremental transform for one frame (implemented by subclasses).
|
||||
* @param data Sensor data for this frame (may already be decimated or deskewed).
|
||||
* @param guess Motion prior from the base class or the caller.
|
||||
* @param info Optional debug/statistics output.
|
||||
* @return Incremental transform, or null if tracking failed.
|
||||
*/
|
||||
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
|
||||
|
||||
void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
|
||||
void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0);
|
||||
void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw);
|
||||
|
||||
private:
|
||||
int _resetCountdown;
|
||||
bool _force3DoF;
|
||||
bool _holonomic;
|
||||
bool guessFromMotion_;
|
||||
float guessSmoothingDelay_;
|
||||
int _filteringStrategy;
|
||||
int _particleSize;
|
||||
float _particleNoiseT;
|
||||
float _particleLambdaT;
|
||||
float _particleNoiseR;
|
||||
float _particleLambdaR;
|
||||
bool _fillInfoData;
|
||||
float _kalmanProcessNoise;
|
||||
float _kalmanMeasurementNoise;
|
||||
unsigned int _imageDecimation;
|
||||
bool _alignWithGround;
|
||||
bool _publishRAMUsage;
|
||||
bool _imagesAlreadyRectified;
|
||||
bool _deskewing;
|
||||
Transform _pose;
|
||||
int _resetCurrentCount;
|
||||
double previousStamp_;
|
||||
std::list<std::pair<std::vector<float>, double> > previousVelocities_;
|
||||
Transform velocityGuess_;
|
||||
Transform imuLastTransform_;
|
||||
Transform previousGroundTruthPose_;
|
||||
float distanceTravelled_;
|
||||
unsigned int framesProcessed_;
|
||||
|
||||
std::vector<ParticleFilter *> particleFilters_;
|
||||
cv::KalmanFilter kalmanFilter_;
|
||||
std::vector<StereoCameraModel> stereoModels_;
|
||||
std::vector<CameraModel> models_;
|
||||
std::map<double, Transform> imus_;
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
#endif /* ODOMETRY_H_ */
|
||||
|
||||
@@ -116,6 +116,11 @@ add_executable(test_compression test_compression.cpp)
|
||||
target_link_libraries(test_compression gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_compression)
|
||||
|
||||
#Odometry.h
|
||||
add_executable(test_odometry test_odometry.cpp)
|
||||
target_link_libraries(test_odometry gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_odometry)
|
||||
|
||||
#Signature.h
|
||||
add_executable(test_signature test_signature.cpp)
|
||||
target_link_libraries(test_signature gtest_main rtabmap_core)
|
||||
|
||||
@@ -0,0 +1,181 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
#include <rtabmap/core/OdometryInfo.h>
|
||||
#include <rtabmap/core/CameraModel.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace {
|
||||
|
||||
class MockOdometry : public Odometry
|
||||
{
|
||||
public:
|
||||
explicit MockOdometry(const ParametersMap & parameters = ParametersMap())
|
||||
: Odometry(parameters)
|
||||
{
|
||||
}
|
||||
|
||||
Type getType() override
|
||||
{
|
||||
return kTypeUndef;
|
||||
}
|
||||
|
||||
void setNextTransform(const Transform & transform)
|
||||
{
|
||||
nextTransform_ = transform;
|
||||
}
|
||||
|
||||
const Transform & lastGuess() const
|
||||
{
|
||||
return lastGuess_;
|
||||
}
|
||||
|
||||
protected:
|
||||
Transform computeTransform(SensorData &, const Transform & guess, OdometryInfo *) override
|
||||
{
|
||||
lastGuess_ = guess;
|
||||
return nextTransform_;
|
||||
}
|
||||
|
||||
private:
|
||||
Transform nextTransform_;
|
||||
Transform lastGuess_;
|
||||
};
|
||||
|
||||
SensorData makeImageData(int id, double stamp)
|
||||
{
|
||||
const cv::Mat image = cv::Mat::zeros(32, 32, CV_8UC1);
|
||||
const CameraModel model(100.0, 100.0, 16.0, 16.0);
|
||||
SensorData data(image, model, id);
|
||||
data.setStamp(stamp);
|
||||
return data;
|
||||
}
|
||||
|
||||
ParametersMap baseTestParameters()
|
||||
{
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kOdomGuessMotion(), "false"));
|
||||
parameters.insert(ParametersPair(Parameters::kOdomFilteringStrategy(), "0"));
|
||||
parameters.insert(ParametersPair(Parameters::kOdomFillInfoData(), "true"));
|
||||
parameters.insert(ParametersPair(Parameters::kRtabmapImagesAlreadyRectified(), "true"));
|
||||
return parameters;
|
||||
}
|
||||
|
||||
void expectTransformNear(const Transform & a, const Transform & b, float tol = 1e-4f)
|
||||
{
|
||||
EXPECT_NEAR(a.x(), b.x(), tol);
|
||||
EXPECT_NEAR(a.y(), b.y(), tol);
|
||||
EXPECT_NEAR(a.z(), b.z(), tol);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(OdometryTest, CreateUsesF2MByDefault)
|
||||
{
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kOdomStrategy(), "0"));
|
||||
|
||||
Odometry * odometry = Odometry::create(parameters);
|
||||
ASSERT_NE(odometry, nullptr);
|
||||
EXPECT_EQ(odometry->getType(), Odometry::kTypeF2M);
|
||||
delete odometry;
|
||||
}
|
||||
|
||||
TEST(OdometryTest, CreateUnknownTypeFallsBackToF2M)
|
||||
{
|
||||
Odometry::Type type = static_cast<Odometry::Type>(999);
|
||||
Odometry * odometry = Odometry::create(type);
|
||||
ASSERT_NE(odometry, nullptr);
|
||||
EXPECT_EQ(type, Odometry::kTypeF2M);
|
||||
EXPECT_EQ(odometry->getType(), Odometry::kTypeF2M);
|
||||
delete odometry;
|
||||
}
|
||||
|
||||
TEST(OdometryTest, ResetSetsInitialPoseAndCounters)
|
||||
{
|
||||
MockOdometry odometry(baseTestParameters());
|
||||
const Transform initialPose(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f);
|
||||
odometry.reset(initialPose);
|
||||
|
||||
EXPECT_EQ(odometry.framesProcessed(), 0u);
|
||||
EXPECT_DOUBLE_EQ(odometry.previousStamp(), 0.0);
|
||||
expectTransformNear(odometry.getPose(), initialPose);
|
||||
EXPECT_TRUE(odometry.getVelocityGuess().isNull());
|
||||
}
|
||||
|
||||
TEST(OdometryTest, ProcessAccumulatesPose)
|
||||
{
|
||||
MockOdometry odometry(baseTestParameters());
|
||||
odometry.reset(Transform::getIdentity());
|
||||
|
||||
const Transform step(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
odometry.setNextTransform(step);
|
||||
|
||||
SensorData frame0 = makeImageData(0, 0.0);
|
||||
const Transform pose0 = odometry.process(frame0);
|
||||
expectTransformNear(pose0, step);
|
||||
expectTransformNear(odometry.getPose(), pose0);
|
||||
EXPECT_EQ(odometry.framesProcessed(), 1u);
|
||||
|
||||
SensorData frame1 = makeImageData(1, 1.0);
|
||||
const Transform pose1 = odometry.process(frame1);
|
||||
const Transform expectedPose1(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
expectTransformNear(pose1, expectedPose1);
|
||||
expectTransformNear(odometry.getPose(), expectedPose1);
|
||||
EXPECT_EQ(odometry.framesProcessed(), 2u);
|
||||
}
|
||||
|
||||
TEST(OdometryTest, ProcessWithExternalGuess)
|
||||
{
|
||||
MockOdometry odometry(baseTestParameters());
|
||||
odometry.reset(Transform::getIdentity());
|
||||
odometry.setNextTransform(Transform(0.1f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
|
||||
|
||||
SensorData data = makeImageData(0, 0.0);
|
||||
const Transform guess(0.2f, 0.3f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
odometry.process(data, guess);
|
||||
|
||||
expectTransformNear(odometry.lastGuess(), guess);
|
||||
}
|
||||
|
||||
TEST(OdometryTest, ProcessFillsOdometryInfo)
|
||||
{
|
||||
MockOdometry odometry(baseTestParameters());
|
||||
odometry.reset(Transform::getIdentity());
|
||||
odometry.setNextTransform(Transform(0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
|
||||
|
||||
SensorData data = makeImageData(0, 1.0);
|
||||
OdometryInfo info;
|
||||
const Transform pose = odometry.process(data, &info);
|
||||
|
||||
EXPECT_FALSE(pose.isNull());
|
||||
EXPECT_FALSE(info.lost);
|
||||
EXPECT_DOUBLE_EQ(info.stamp, 1.0);
|
||||
expectTransformNear(info.transform, Transform(0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
|
||||
expectTransformNear(pose, Transform(0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
|
||||
EXPECT_TRUE(odometry.isInfoDataFilled());
|
||||
}
|
||||
|
||||
TEST(OdometryTest, ProcessLostReturnsNullTransform)
|
||||
{
|
||||
MockOdometry odometry(baseTestParameters());
|
||||
odometry.reset(Transform::getIdentity());
|
||||
odometry.setNextTransform(Transform());
|
||||
|
||||
SensorData data = makeImageData(0, 0.0);
|
||||
OdometryInfo info;
|
||||
const Transform t = odometry.process(data, &info);
|
||||
|
||||
EXPECT_TRUE(t.isNull());
|
||||
EXPECT_TRUE(info.lost);
|
||||
EXPECT_EQ(odometry.framesProcessed(), 0u);
|
||||
}
|
||||
|
||||
TEST(OdometryTest, DefaultCapabilityFlags)
|
||||
{
|
||||
MockOdometry odometry(baseTestParameters());
|
||||
EXPECT_FALSE(odometry.canProcessRawImages());
|
||||
EXPECT_FALSE(odometry.canProcessAsyncIMU());
|
||||
}
|
||||
Reference in New Issue
Block a user