diff --git a/include/chilitags.hpp b/include/chilitags.hpp index 8365f81..b98c05a 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -363,10 +363,9 @@ typedef std::map TagPoseMap; To first detect th tags in the image, Chilitags3D creates a Chilitags instance, which can be accessed through the getChilitags() accessors. This - Chilitags instance is set to have a persistence of 0, but Chilitags3D sets - a default persistence of 5 frames for poses it estimates. It also sets a - gain of .5 to avoid the detected objects to "shake". Please refer to the - documentation of Chilitags3D::setFilter() for more detail. + Chilitags instance is set to have a persistence of 0, because Chilitags3D + uses a more advanced Kalman filter. See enableFilter() and setPersistence() + for more details. You can also create yourself a separate instance of Chilitagsfor the 2D detection of tags and use it by calling @@ -384,28 +383,6 @@ typedef std::map TagPoseMap; */ Chilitags3D_(cv::Size cameraResolution = cv::Size(640, 480)); -/** - Parameters to paliate with the imperfections of the detection. - - \param persistence the number of frames in which a 3d object, i.e. a tag or - a rigid object containing several tags, should be absent before being - removed from the output of estimate(). - - \param gain a value between 0 and 1 corresponding to the weight of the - previous (filtered) position in the new filtered position. 0 means that the - latest position of the object is returned. - - Chilitags3D manages persistence separately from the 2D detection of - Chilitags, because an object can be composed of several tags. If one or - more of these tags are not detected, the pose of the object is estimated - with the remaining, detected tags. In this case, it would hurt the pose - estimation to make the individual tags persistent. Chilitags3D thus sets - the 2D detection to have no persistence, and applies its persistence - mechanism only after the estimation of the pose. The same goes for the - position filtering. Note that for independent tags, it should not matter. - */ -void setFilter(int persistence, float gain); - /** Accessor to the underlying (2D) Chilitags detection. */ const Chilitags &getChilitags() const; @@ -424,8 +401,18 @@ Chilitags &getChilitags(); 0 , 0 , 0 , 1 } \endverbatim \param tags a list of tags, as returned by Chilitags::find(). + + \param camDeltaR Rotation from the previous camera frame to + the current camera frame, i.e rotation of the current camera frame in the + last camera frame. Quaternion format (scalar, vx, vy, vz). + + \param camDeltaX Translation from the previous camera frame + to the current camera frame, i.e position of the current camera frame in + the last camera frame. */ -TagPoseMap estimate(const TagCornerMap & tags); +TagPoseMap estimate(const TagCornerMap & tags, + cv::Vec const& camDeltaR = cv::Vec(1,0,0,0), + cv::Vec const& camDeltaX = cv::Vec(0,0,0)); /** This is a convenience variant of estimate() which also takes care of the @@ -449,10 +436,20 @@ TagPoseMap estimate(const TagCornerMap & tags); best return tags previously found; it won't find new ones, but can lose some. See Chilitags::DetectionTrigger for a description of the possible values. + + \param camDeltaR Rotation from the previous camera frame to + the current camera frame, i.e rotation of the current camera frame in the + last camera frame. Quaternion format (scalar, vx, vy, vz). + + \param camDeltaX Translation from the previous camera frame + to the current camera frame, i.e position of the current camera frame in + the last camera frame. */ TagPoseMap estimate( const cv::Mat &inputImage, - Chilitags::DetectionTrigger detectionTrigger = Chilitags::DETECT_ONLY); + Chilitags::DetectionTrigger detectionTrigger = Chilitags::DETECT_ONLY, + cv::Vec const& camDeltaR = cv::Vec(1,0,0,0), + cv::Vec const& camDeltaX = cv::Vec(0,0,0)); /** Chilitags3D can also detect rigid assemblies of tags. This allows for a @@ -494,6 +491,58 @@ bool readTagConfiguration( */ void setDefaultTagSize(RealT defaultSize); +/** + * @brief Enables/disables Kalman filtering on tag pose (enabled by default) + * + * @param enabled Whether to enable Kalman filtering + */ +void enableFilter(bool enabled); + +/** + * @brief Sets the persistence of tags against being discarded when not + * observed (10 by default) + * + * @param persistence Persistence value, roughly correponds to number of frames + */ +void setPersistence(RealT persistence); + +/** + * @brief Sets the process noise covariance matrix a.k.a Q for the Kalman filter + * + * The state is described by (x,y,z,qw,qx,qy,qz) where x,y,z is the tag + * position and qw,qx,qy,qz is the tag orientation in quaternion + * representation. Therefore, the input matrix should correspond to + * cov((x,y,z,qw,qx,qy,qz)). + * + * When IMU data is present, the process moves the tag according to camera + * movement. When IMU data is not present, the process tries to keep the tag + * still. Smaller values in the process covariance matrix causes the tags to + * resist motion more. By default, this matrix has + * 1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4, 1e-4 + * on its diagonal. + * + * @param covariance 7x7 covariance matrix + */ +void setFilterProcessNoiseCovariance(cv::Mat const& covariance); + +/** + * @brief Sets the observation noise covariance matrix a.k.a R for the Kalman filter + * + * The observation (done by image processing on the camera image) is described + * by (x,y,z,qw,qx,qy,qz) where x,y,z is the tag position and qw,qx,qy,qz is + * the tag orientation in quaternion representation. Therefore, the input + * matrix should correspond to cov((x,y,z,qw,qx,qy,qz)). + * + * Larger values in the observation noise covariance matrix indicates noisier + * measurements and causes observations to affect the state less, making tags + * more resistant to motion. By default, this matrix has + * 1e-3, 1e-3, 1e-1, 1e-3, 1e-2, 1e-2, 1e-5 + * on its diagonal. + * + * @param covariance 7x7 covariance matrix + */ +void setFilterObservationNoiseCovariance(cv::Mat const& covariance); + /** For accurate results, Chilitags3D can be provided the calibration data of the camera detecting the chilitags. See diff --git a/samples/3dfiltering/filter3d-gui.cpp b/samples/3dfiltering/filter3d-gui.cpp new file mode 100644 index 0000000..4b436f6 --- /dev/null +++ b/samples/3dfiltering/filter3d-gui.cpp @@ -0,0 +1,209 @@ +/******************************************************************************* + * Copyright 2013-2014 EPFL * + * Copyright 2013-2014 Quentin Bonnard * + * * + * This file is part of chilitags. * + * * + * Chilitags is free software: you can redistribute it and/or modify * + * it under the terms of the Lesser GNU General Public License as * + * published by the Free Software Foundation, either version 3 of the * + * License, or (at your option) any later version. * + * * + * Chilitags is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with Chilitags. If not, see . * + *******************************************************************************/ + +#include + +#include + +#include // for cv::Mat +#include // CV_AA +#include // for camera capture +#include // for camera capture + +using namespace std; +using namespace cv; + +int main(int argc, char* argv[]) +{ + cout + << "Usage: "<< argv[0] + << " [-c ] [-i ]\n"; + + const char* intrinsicsFilename = 0; + const char* configFilename = 0; + + for( int i = 1; i < argc; i++ ) + { + if( strcmp(argv[i], "-c") == 0 ) + configFilename = argv[++i]; + else if( strcmp(argv[i], "-i") == 0 ) + intrinsicsFilename = argv[++i]; + } + + /*****************************/ + /* Init camera capture */ + /*****************************/ + int cameraIndex = 0; + cv::VideoCapture capture(cameraIndex); + if (!capture.isOpened()) + { + cerr << "Unable to initialise video capture.\n"; + return 1; + } + + /******************************/ + /* Setting up pose estimation */ + /******************************/ +#ifdef OPENCV3 + double inputWidth = capture.get(cv::CAP_PROP_FRAME_WIDTH); + double inputHeight = capture.get(cv::CAP_PROP_FRAME_HEIGHT); +#else + double inputWidth = capture.get(CV_CAP_PROP_FRAME_WIDTH); + double inputHeight = capture.get(CV_CAP_PROP_FRAME_HEIGHT); +#endif + + chilitags::Chilitags3D chilitags3D(Size(inputWidth, inputHeight)); + + if (configFilename) chilitags3D.readTagConfiguration(configFilename); + + if (intrinsicsFilename) { + Size calibratedImageSize = chilitags3D.readCalibration(intrinsicsFilename); +#ifdef OPENCV3 + capture.set(cv::CAP_PROP_FRAME_WIDTH, calibratedImageSize.width); + capture.set(cv::CAP_PROP_FRAME_HEIGHT, calibratedImageSize.height); +#else + capture.set(CV_CAP_PROP_FRAME_WIDTH, calibratedImageSize.width); + capture.set(CV_CAP_PROP_FRAME_HEIGHT, calibratedImageSize.height); +#endif + } + + cv::Mat projectionMat = cv::Mat::zeros(4,4,CV_64F); + chilitags3D.getCameraMatrix().copyTo(projectionMat(cv::Rect(0,0,3,3))); + cv::Matx44d projection = projectionMat; + projection(3,2) = 1; + + /*****************************/ + /* Go! */ + /*****************************/ + cv::namedWindow("Pose Estimation"); + + char keyPressed; + bool filterEnabled = true; + + cv::Mat Q = (cv::Mat_(7,7) << + 1e-3f, 0, 0, 0, 0, 0, 0, + 0, 1e-3f, 0, 0, 0, 0, 0, + 0, 0, 1e-3f, 0, 0, 0, 0, + 0, 0, 0, 1e-4f, 0, 0, 0, + 0, 0, 0, 0, 1e-4f, 0, 0, + 0, 0, 0, 0, 0, 1e-4f, 0, + 0, 0, 0, 0, 0, 0, 1e-4f); + float alphaQ = 1.0f; + + cv::Mat R = (cv::Mat_(7,7) << + 1e-3f, 0, 0, 0, 0, 0, 0, + 0, 1e-3f, 0, 0, 0, 0, 0, + 0, 0, 1e-1f, 0, 0, 0, 0, + 0, 0, 0, 1e-3f, 0, 0, 0, + 0, 0, 0, 0, 1e-2f, 0, 0, + 0, 0, 0, 0, 0, 1e-2f, 0, + 0, 0, 0, 0, 0, 0, 1e-5f); + float alphaR = 1.0f; + + while ('q' != (keyPressed = (char) cv::waitKey(1))) { + + switch(keyPressed){ + case 'f': + filterEnabled = !filterEnabled; + chilitags3D.enableFilter(filterEnabled); + break; + case 'w': + alphaQ *= 10.0f; + chilitags3D.setFilterProcessNoiseCovariance(alphaQ*Q); + break; + case 's': + alphaQ /= 10.0f; + chilitags3D.setFilterProcessNoiseCovariance(alphaQ*Q); + break; + case 'e': + alphaR *= 10.0f; + chilitags3D.setFilterObservationNoiseCovariance(alphaR*R); + break; + case 'd': + alphaR /= 10.0f; + chilitags3D.setFilterObservationNoiseCovariance(alphaR*R); + break; + } + + cv::Mat inputImage; + capture.read(inputImage); + cv::Mat outputImage = inputImage.clone(); + + cv::putText(outputImage, cv::format("Filtering %s, press 'f' to toggle",filterEnabled ? "ENABLED" : "DISABLED"), + cv::Point(8,20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255)); + + cv::putText(outputImage, cv::format("Process covariance multiplier: %f, press 'w' or 's' to modify", alphaQ), + cv::Point(8,36), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255)); + + cv::putText(outputImage, cv::format("Observation covariance multiplier: %f, press 'e' or 'd' to modify", alphaR), + cv::Point(8,52), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255)); + + for (auto& kv : chilitags3D.estimate(inputImage, chilitags::Chilitags::DETECT_PERIODICALLY)) { + + static const float DEFAULT_SIZE = 20.f; + static const cv::Vec4d UNITS[4] { + {0.f, 0.f, 0.f, 1.f}, + {DEFAULT_SIZE, 0.f, 0.f, 1.f}, + {0.f, DEFAULT_SIZE, 0.f, 1.f}, + {0.f, 0.f, DEFAULT_SIZE, 1.f}, + }; + + cv::Matx44d transformation = kv.second; + cv::Vec4f referential[4] = { + projection*transformation*UNITS[0], + projection*transformation*UNITS[1], + projection*transformation*UNITS[2], + projection*transformation*UNITS[3], + }; + + std::vector t2DPoints; + for (auto homogenousPoint : referential) + t2DPoints.push_back(cv::Point2f( + homogenousPoint[0]/homogenousPoint[3], + homogenousPoint[1]/homogenousPoint[3])); + + static const int SHIFT = 16; + static const float PRECISION = 1< mFilter; +Filter mFilter; Detect mDetect; @@ -296,10 +296,7 @@ void Chilitags::setMinInputWidth(int minWidth) { mImpl->setMinInputWidth(minWidth); } -TagCornerMap Chilitags::find( - const cv::Mat &inputImage, - DetectionTrigger trigger - ) { +TagCornerMap Chilitags::find(const cv::Mat &inputImage, DetectionTrigger trigger) { return mImpl->find(inputImage, trigger); } diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index ad45f84..c5d7b9b 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -20,7 +20,6 @@ #include -#include "Filter.hpp" #include "EstimatePose3D.hpp" #include @@ -37,21 +36,15 @@ class Chilitags3D_::Impl { public: Impl(cv::Size cameraResolution) : mChilitags(), + mEstimatePose3D(cameraResolution), mOmitOtherTags(false), mDefaultTagCorners(), - mId2Configuration(), - mEstimatePose3D(cameraResolution), - mFilter(5, 0.5f) + mId2Configuration() { setDefaultTagSize(20.f); mChilitags.setFilter(0, 0.0f); } -void setFilter(int persistence, float gain) { - mFilter.setPersistence(persistence); - mFilter.setGain(gain); -} - const Chilitags &getChilitags() const { return mChilitags; } @@ -59,10 +52,17 @@ const Chilitags &getChilitags() const { return mChilitags; } -TagPoseMap estimate(const TagCornerMap &tags) { - +TagPoseMap estimate(TagCornerMap const& tags, cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) +{ TagPoseMap objects; + //Pass the latest camera movement difference for prediction (if 3D filtering is enabled) + mEstimatePose3D.setCamDelta(camDeltaR, camDeltaX); + + //Predict pose for all known tags with camera movement (if 3D filtering is enabled) + mEstimatePose3D(objects); + + //Correct pose prediction with new observations std::map< const std::string, //name of the object std::pair< @@ -120,13 +120,15 @@ TagPoseMap estimate(const TagCornerMap &tags) { objects); } - return mFilter(objects); + return objects; } TagPoseMap estimate( const cv::Mat &inputImage, - Chilitags::DetectionTrigger detectionTrigger) { - return estimate(mChilitags.find(inputImage, detectionTrigger)); + Chilitags::DetectionTrigger detectionTrigger, + cv::Vec const& camDeltaR, + cv::Vec const& camDeltaX) { + return estimate(mChilitags.find(inputImage, detectionTrigger), camDeltaR, camDeltaX); } void setDefaultTagSize(RealT defaultSize){ @@ -138,6 +140,22 @@ void setDefaultTagSize(RealT defaultSize){ }; } +void enableFilter(bool enabled){ + mEstimatePose3D.enableFilter(enabled); +} + +void setPersistence(RealT persistence){ + mEstimatePose3D.setFilterPersistence(persistence); +} + +void setFilterProcessNoiseCovariance(cv::Mat const& covariance){ + mEstimatePose3D.setFilterProcessNoiseCovariance(covariance); +} + +void setFilterObservationNoiseCovariance(cv::Mat const& covariance){ + mEstimatePose3D.setFilterObservationNoiseCovariance(covariance); +} + bool read3DConfiguration(const std::string &filenameOrString, bool omitOtherTags, bool readFromString) { mOmitOtherTags = omitOtherTags; @@ -285,15 +303,8 @@ std::vector> mDefaultTagCorners; // associates a tag id with an object name and the configuration of the tag // in this object std::map > mId2Configuration; - -Filter mFilter; }; -template -void Chilitags3D_::setFilter(int persistence, float gain) { - mImpl->setFilter(persistence, gain); -} - template Chilitags3D_::Chilitags3D_(cv::Size cameraResolution) : mImpl(new Chilitags3D_::Impl(cameraResolution)){ @@ -311,15 +322,19 @@ Chilitags &Chilitags3D_::getChilitags(){ template typename Chilitags3D_::TagPoseMap Chilitags3D_::estimate( - const TagCornerMap &tags) { - return mImpl->estimate(tags); + const TagCornerMap &tags, + cv::Vec const& camDeltaR, + cv::Vec const& camDeltaX) { + return mImpl->estimate(tags, camDeltaR, camDeltaX); } template typename Chilitags3D_::TagPoseMap Chilitags3D_::estimate( const cv::Mat &inputImage, - Chilitags::DetectionTrigger detectionTrigger) { - return mImpl->estimate(inputImage, detectionTrigger); + Chilitags::DetectionTrigger detectionTrigger, + cv::Vec const& camDeltaR, + cv::Vec const& camDeltaX) { + return mImpl->estimate(inputImage, detectionTrigger, camDeltaR, camDeltaX); } template @@ -327,6 +342,26 @@ void Chilitags3D_::setDefaultTagSize(RealT defaultSize){ mImpl->setDefaultTagSize(defaultSize); } +template +void Chilitags3D_::enableFilter(bool enabled){ + mImpl->enableFilter(enabled); +} + +template +void Chilitags3D_::setPersistence(RealT persistence){ + mImpl->setPersistence(persistence); +} + +template +void Chilitags3D_::setFilterProcessNoiseCovariance(cv::Mat const& covariance){ + mImpl->setFilterProcessNoiseCovariance(covariance); +} + +template +void Chilitags3D_::setFilterObservationNoiseCovariance(cv::Mat const& covariance){ + mImpl->setFilterObservationNoiseCovariance(covariance); +} + template bool Chilitags3D_::readTagConfiguration(const std::string &filenameOrString, bool omitOtherTags, bool readFromString){ return mImpl->read3DConfiguration(filenameOrString, omitOtherTags, readFromString); diff --git a/src/EstimatePose3D.cpp b/src/EstimatePose3D.cpp index 58f1b5c..e977e02 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -33,6 +33,8 @@ namespace chilitags{ template EstimatePose3D::EstimatePose3D(cv::Size cameraResolution) : + mFilter3D(), + mFilter3DEnabled(true), mCameraMatrix(), mDistCoeffs() { @@ -63,6 +65,44 @@ cv::Mat const& EstimatePose3D::getDistortionCoeffs() const return mDistCoeffs; } +template +void EstimatePose3D::enableFilter(bool enabled) +{ + mFilter3DEnabled = enabled; +} + +template +void EstimatePose3D::setFilterPersistence(RealT persistence) +{ + mFilter3D.setPersistence(persistence); +} + +template +void EstimatePose3D::setFilterProcessNoiseCovariance(cv::Mat const& covariance) +{ + mFilter3D.setProcessNoiseCovariance(covariance); +} + +template +void EstimatePose3D::setFilterObservationNoiseCovariance(cv::Mat const& covariance) +{ + mFilter3D.setObservationNoiseCovariance(covariance); +} + +template +void EstimatePose3D::setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) +{ + if(mFilter3DEnabled) + mFilter3D.setCamDelta(camDeltaR, camDeltaX); +} + +template +void EstimatePose3D::operator()(typename Chilitags3D_::TagPoseMap& objects) +{ + if(mFilter3DEnabled) + mFilter3D(objects); +} + template void EstimatePose3D::operator()(std::string const& name, std::vector> const& objectPoints, @@ -81,6 +121,9 @@ void EstimatePose3D::operator()(std::string const& name, #endif //TODO: Rotation and translation vectors come out of solvePnP as double + if(mFilter3DEnabled) + mFilter3D(name, mTempTranslation, mTempRotation); + cv::Rodrigues(mTempRotation, mTempRotMat); objects[name] = { diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index 916409a..d3507d8 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -34,6 +34,8 @@ #include +#include "Filter3D.hpp" + namespace chilitags { template @@ -72,6 +74,49 @@ class EstimatePose3D */ cv::Mat const& getDistortionCoeffs() const; + /** + * @brief Enabled/disables Kalman filtering on tag pose + * + * @param enabled Whether tag pose filtering is enabled/disabled + */ + void enableFilter(bool enabled); + + /** + * @brief Sets the persistence of tags against being discarded when not observed + * + * @param persistence Persistence value, roughly correponds to number of frames + */ + void setFilterPersistence(RealT persistence); + + /** + * @brief Sets the process noise covariance matrix for the Kalman filter + * + * @param covariance 7x7 covariance matrix + */ + void setFilterProcessNoiseCovariance(cv::Mat const& covariance); + + /** + * @brief Sets the observation noise covariance matrix for the Kalman filter + * + * @param covariance 7x7 covariance matrix + */ + void setFilterObservationNoiseCovariance(cv::Mat const& covariance); + + /** + * @brief Informs the rotation and translation of the current camera frame in the previous camera frame + * + * @param camDeltaR Rotation of current camera frame in previous camera frame in unit quaternion format (w,vx,vy,vz) + * @param camDeltaX Translation of current camera frame in previous camera frame + */ + void setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX); + + /** + * @brief Updates the poses of all known tags via statistical filtering + * + * @param objects Map to update in which ID maps to the transform + */ + void operator()(typename Chilitags3D_::TagPoseMap& objects); + /** * @brief Updates/inserts the pose of the given object in the given map * @@ -87,6 +132,9 @@ class EstimatePose3D protected: + Filter3D mFilter3D; ///< Kalman filter to increase stability of the tag + bool mFilter3DEnabled; ///< Whether to enable pose filtering + cv::Mat mCameraMatrix; ///< 3x3 camera matrix cv::Mat mDistCoeffs; ///< Empty or 4x1 or 5x1 or 8x1 Distortion coefficients of the camera diff --git a/src/Filter.cpp b/src/Filter.cpp index f6ec4a8..f32fd6d 100644 --- a/src/Filter.cpp +++ b/src/Filter.cpp @@ -24,18 +24,15 @@ namespace chilitags { -template -FindOutdated::FindOutdated(int persistence) : +FindOutdated::FindOutdated(int persistence) : mPersistence(persistence), mDisappearanceTime() { } -template -template -std::vector FindOutdated::operator()(const std::map &tags){ +std::vector FindOutdated::operator()(const std::map &tags){ - std::vector tagsToForget; + std::vector tagsToForget; auto tagIt = tags.cbegin(); auto ageIt = mDisappearanceTime.begin(); @@ -83,16 +80,14 @@ std::vector FindOutdated::operator()(const std::map &t return tagsToForget; } -template -Filter::Filter(int persistence, float gain): +Filter::Filter(int persistence, float gain): mFindOutdated(persistence), mGain(gain), mFilteredCoordinates() {} -template -const std::map & Filter::operator()( - const std::map &tags) { +const std::map & Filter::operator()( + const std::map &tags) { for(const auto &tagToForget : mFindOutdated(tags)) { //TODO lookup can be avoided if Ids are sorted @@ -122,11 +117,4 @@ const std::map & Filter::operator()( return mFilteredCoordinates; } -template class FindOutdated; -template class Filter; - -template class FindOutdated; -template class Filter; -template class Filter; - } diff --git a/src/Filter.hpp b/src/Filter.hpp index ad46c72..138af72 100644 --- a/src/Filter.hpp +++ b/src/Filter.hpp @@ -21,8 +21,8 @@ /** This header contains various utilities to paliate with imperfect detection. */ -#ifndef Filters_HPP -#define Filters_HPP +#ifndef Filter_HPP +#define Filter_HPP #include #include @@ -31,7 +31,6 @@ namespace chilitags { -template class FindOutdated { public: @@ -42,19 +41,17 @@ void setPersistence(int persistence) { mPersistence = persistence; } -template -std::vector operator()(const std::map &tags); +std::vector operator()(const std::map &tags); protected: int mPersistence; -std::map mDisappearanceTime; +std::map mDisappearanceTime; }; -template class Filter { public: Filter(int persistence, float gain); @@ -67,13 +64,13 @@ class Filter { mGain = gain; } - const std::map & operator()( - const std::map &tags) ; + const std::map & operator()( + const std::map &tags) ; protected: - FindOutdated mFindOutdated; + FindOutdated mFindOutdated; float mGain; - std::map mFilteredCoordinates; + std::map mFilteredCoordinates; }; diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp new file mode 100644 index 0000000..9cab79b --- /dev/null +++ b/src/Filter3D.cpp @@ -0,0 +1,357 @@ +/******************************************************************************* + * Copyright 2013-2014 EPFL * + * Copyright 2013-2014 Quentin Bonnard * + * * + * This file is part of chilitags. * + * * + * Chilitags is free software: you can redistribute it and/or modify * + * it under the terms of the Lesser GNU General Public License as * + * published by the Free Software Foundation, either version 3 of the * + * License, or (at your option) any later version. * + * * + * Chilitags is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with Chilitags. If not, see . * + *******************************************************************************/ + +/** + * @file Filter3D.cpp + * @brief 6D pose filter for multiple std::string ID'd objects + * @author Ayberk Özgür + */ + +#include "Filter3D.hpp" + +#include +#include +#include + +#include + +namespace chilitags{ + +template +Filter3D::Filter3D() : + CV_TYPE(std::is_same::value ? CV_64F : CV_32F), + EPSILON(std::is_same::value ? DBL_EPSILON : FLT_EPSILON), + mF(7, 7, CV_TYPE), + mB(7, 3, CV_TYPE), + mControl(3, 1, CV_TYPE), + mQ(), + mQChanged(true), + mR(), + mRChanged(true), + mCovScales(7, 1, CV_TYPE), + mPersistence(10.0f), + mTempState(7, 1, CV_TYPE) +{ + + //Process noise covariance is 7x7: cov((x,y,z,qw,qx,qy,qz)) + mQ = (cv::Mat_(7,7) << + 1e-3f, 0, 0, 0, 0, 0, 0, + 0, 1e-3f, 0, 0, 0, 0, 0, + 0, 0, 1e-3f, 0, 0, 0, 0, + 0, 0, 0, 1e-4f, 0, 0, 0, + 0, 0, 0, 0, 1e-4f, 0, 0, + 0, 0, 0, 0, 0, 1e-4f, 0, + 0, 0, 0, 0, 0, 0, 1e-4f); + + //Measurement noise covariance is 7x7: cov((x,y,z,qw,qx,qy,qz)) + mR = (cv::Mat_(7,7) << + 1e-3f, 0, 0, 0, 0, 0, 0, + 0, 1e-3f, 0, 0, 0, 0, 0, + 0, 0, 1e-1f, 0, 0, 0, 0, + 0, 0, 0, 1e-3f, 0, 0, 0, + 0, 0, 0, 0, 1e-2f, 0, 0, + 0, 0, 0, 0, 0, 1e-2f, 0, + 0, 0, 0, 0, 0, 0, 1e-5f); + + //Scale coefficients when calculating the trace of the covariance estimate + recalculateCovScales(); + + //Process matrix is 7x7 and is identity as long as there is no camera movement info + cv::setIdentity(mF); + + //Control matrix is 7x3 and is zero as long as there is no camera movement info + mB = cv::Mat::zeros(7, 3, CV_TYPE); + + //Control input is 3x1 and is zero as long as there is no linear camera movement info + mControl = cv::Mat::zeros(3, 1, CV_TYPE); +} + +template +inline void Filter3D::recalculateCovScales() +{ + for(int i=0;i<7;i++) + mCovScales.at(i) = sqrt(mQ.at(i,i)*mR.at(i,i)); +} + +template +void Filter3D::setPersistence(RealT persistence) +{ + mPersistence = persistence; +} + +template +void Filter3D::setProcessNoiseCovariance(cv::Mat const& covariance) +{ + covariance.copyTo(mQ); + recalculateCovScales(); + mQChanged = true; +} + +template +void Filter3D::setObservationNoiseCovariance(cv::Mat const& covariance) +{ + covariance.copyTo(mR); + recalculateCovScales(); + mRChanged = true; +} + +template +void Filter3D::setCamDelta(cv::Vec const& q, cv::Vec const& camDeltaX) +{ + RealT* F0 = (RealT*)mF.ptr(0); + RealT* F1 = (RealT*)mF.ptr(1); + RealT* F2 = (RealT*)mF.ptr(2); + RealT* F3 = (RealT*)mF.ptr(3); + RealT* F4 = (RealT*)mF.ptr(4); + RealT* F5 = (RealT*)mF.ptr(5); + RealT* F6 = (RealT*)mF.ptr(6); + + RealT* B0 = (RealT*)mB.ptr(0); + RealT* B1 = (RealT*)mB.ptr(1); + RealT* B2 = (RealT*)mB.ptr(2); + + RealT* u = (RealT*)mControl.ptr(0); + + //Set the transition matrix (other entries are zero) + F0[0] = q(0)*q(0) + q(1)*q(1) - q(2)*q(2) - q(3)*q(3); F0[1] = 2*(q(1)*q(2) + q(0)*q(3)); F0[2] = 2*(q(1)*q(3) - q(0)*q(2)); + F1[0] = 2*(q(1)*q(2) - q(0)*q(3)); F1[1] = q(0)*q(0) - q(1)*q(1) + q(2)*q(2) - q(3)*q(3); F1[2] = 2*(q(2)*q(3) + q(0)*q(1)); + F2[0] = 2*(q(1)*q(3) + q(0)*q(2)); F2[1] = 2*(q(2)*q(3) - q(0)*q(1)); F2[2] = q(0)*q(0) - q(1)*q(1) - q(2)*q(2) + q(3)*q(3); + F3[3] = q(0); F3[4] = q(1); F3[5] = q(2); F3[6] = q(3); + F4[3] = -q(1); F4[4] = q(0); F4[5] = q(3); F4[6] = -q(2); + F5[3] = -q(2); F5[4] = -q(3); F5[5] = q(0); F5[6] = q(1); + F6[3] = -q(3); F6[4] = q(2); F6[5] = -q(1); F6[6] = q(0); + + //Set the control matrix (other entries are zero) + memcpy(B0, F0, 3*sizeof(RealT)); + memcpy(B1, F1, 3*sizeof(RealT)); + memcpy(B2, F2, 3*sizeof(RealT)); + + //Set the control input + u[0] = -camDeltaX(0); + u[1] = -camDeltaX(1); + u[2] = -camDeltaX(2); +} + +template +void Filter3D::operator()(typename Chilitags3D_::TagPoseMap& tags) +{ + //TODO: These have to be double precision for rodrigues + cv::Mat predictedRot(3,1,CV_64F); + cv::Matx33d tempRotMat; + + for(auto& kfq : mFilters){ + if(kfq.second.deleted) + continue; + + cv::KalmanFilter& filter = kfq.second.filter; + + //Calculate weighted covariance estimate trace, decide to discard or not + RealT trace = 0.0f; + for(int i=0;i<7;i++) + trace += filter.errorCovPost.at(i,i)/mCovScales.at(i); + trace /= 7.0f; + if(trace > mPersistence){ + kfq.second.deleted = true; + continue; + } + + //Do prediction step + if(mQChanged) + mQ.copyTo(filter.processNoiseCov); + if(mRChanged) + mR.copyTo(filter.measurementNoiseCov); + mF.copyTo(filter.transitionMatrix); + mB.copyTo(filter.controlMatrix); + filter.predict(mControl).copyTo(mTempState); + + //Convert quaternion rotation to angle-axis rotation + getAngleAxis((RealT*)mTempState.ptr() + 3, (double*)predictedRot.ptr()); + + //Convert angle-axis to 3x3 rotation matrix + cv::Rodrigues(predictedRot, tempRotMat); + + //Write back updated pose + tags[kfq.first] = { + (RealT)tempRotMat(0,0), (RealT)tempRotMat(0,1), (RealT)tempRotMat(0,2), mTempState.at(0), + (RealT)tempRotMat(1,0), (RealT)tempRotMat(1,1), (RealT)tempRotMat(1,2), mTempState.at(1), + (RealT)tempRotMat(2,0), (RealT)tempRotMat(2,1), (RealT)tempRotMat(2,2), mTempState.at(2), + 0, 0, 0, 1 + }; + } + + mQChanged = false; + mRChanged = false; +} + +template +void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, cv::Mat& measuredRot) +{ + //Create&insert or return the related filter + //Second set of 4 params are for Kalman filter: # of state dimensions, # of measurement dimensions, + //# of control input dimensions and float precision of internal matrices + auto pair = mFilters.emplace(std::piecewise_construct, + std::make_tuple(id), + std::make_tuple(7, 7, 3, CV_TYPE)); + cv::KalmanFilter& filter = pair.first->second.filter; + cv::Vec& prevQuat = pair.first->second.prevQuat; + bool& deleted = pair.first->second.deleted; + + //Newly inserted or lazy-deleted + if(pair.second || deleted){ + deleted = false; + initFilter(filter, prevQuat, measuredTrans, measuredRot); + } + + //Already existing filter + else{ + RealT* state = (RealT*)mTempState.ptr(); + double* trans = (double*)measuredTrans.ptr(); + + //Fill state + state[0] = (RealT)trans[0]; //x + state[1] = (RealT)trans[1]; //y + state[2] = (RealT)trans[2]; //z + getQuaternion((double*)measuredRot.ptr(), state + 3); + + //Do the correction step + shortestPathQuat(prevQuat); + filter.correct(mTempState).copyTo(mTempState); + normalizeQuat(); + + //Write state back + trans[0] = state[0]; //x + trans[1] = state[1]; //y + trans[2] = state[2]; //z + getAngleAxis(state + 3, (double*)measuredRot.ptr()); + } +} + +template +void Filter3D::initFilter(cv::KalmanFilter& filter, cv::Vec& prevQuat, cv::Mat& measuredTrans, cv::Mat& measuredRot) +{ + mQ.copyTo(filter.processNoiseCov); + mR.copyTo(filter.measurementNoiseCov); + filter.errorCovPost = cv::Mat::zeros(7, 7, CV_TYPE); + + //We have no expectation from a tag other than staying still as long as there is no camera movement information + cv::setIdentity(filter.transitionMatrix); + + //We make the same measurement as the state: (x,y,z,qw,qx,qy,qz) + cv::setIdentity(filter.measurementMatrix); + + //Set initial state + RealT* statePost = (RealT*)filter.statePost.ptr(); + double* trans = (double*)measuredTrans.ptr(); + + statePost[0] = (RealT)trans[0]; //x + statePost[1] = (RealT)trans[1]; //y + statePost[2] = (RealT)trans[2]; //z + getQuaternion((double*)measuredRot.ptr(), statePost + 3); + + prevQuat(0) = statePost[3]; + prevQuat(1) = statePost[4]; + prevQuat(2) = statePost[5]; + prevQuat(3) = statePost[6]; +} + +template +void Filter3D::getAngleAxis(RealT* input, double* output) +{ + RealT theta = sqrt(input[1]*input[1] + input[2]*input[2] + input[3]*input[3]); + theta = 2*atan2(theta, input[0]); + RealT sTheta2 = sin(theta/2); + if(theta < EPSILON){ //Use lim( theta -> 0 ){ theta/sin(theta) } + output[0] = input[1]; //rx + output[1] = input[2]; //ry + output[2] = input[3]; //rz + } + else{ + output[0] = input[1]*theta/sTheta2; //rx + output[1] = input[2]*theta/sTheta2; //ry + output[2] = input[3]*theta/sTheta2; //rz + } +} + +template +void Filter3D::getQuaternion(double* input, RealT* output) +{ + RealT theta = (RealT)sqrt(input[0]*input[0] + input[1]*input[1] + input[2]*input[2]); + output[0] = cos(theta/2); //qw + if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } + output[1] = (RealT)input[0]; //qx + output[2] = (RealT)input[1]; //qy + output[3] = (RealT)input[2]; //qz + } + else{ + RealT sTheta2 = sin(theta/2); + output[1] = (RealT)input[0]/theta*sTheta2; //qx + output[2] = (RealT)input[1]/theta*sTheta2; //qy + output[3] = (RealT)input[2]/theta*sTheta2; //qz + } +} + +template +inline void Filter3D::normalizeQuat() +{ + RealT* quat = (RealT*)mTempState.ptr() + 3; + RealT norm = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]); + if(norm > EPSILON){ + quat[0] /= norm; + quat[1] /= norm; + quat[2] /= norm; + quat[3] /= norm; + } + else{ + quat[0] = 1.0f; + quat[1] = 0.0f; + quat[2] = 0.0f; + quat[3] = 0.0f; + } +} + +template +inline void Filter3D::shortestPathQuat(cv::Vec& prevQuat) +{ + RealT* quat = (RealT*)mTempState.ptr() + 3; + + //If -q would be closer to q_prev than +q, replace new q with -q + //The following comes from the derivation of |q - q_prev|^2 - |-q - q_prev|^2 + if( + quat[0]*prevQuat(0) + + quat[1]*prevQuat(1) + + quat[2]*prevQuat(2) + + quat[3]*prevQuat(3) < 0){ + quat[0] = -quat[0]; + quat[1] = -quat[1]; + quat[2] = -quat[2]; + quat[3] = -quat[3]; + } + prevQuat(0) = quat[0]; + prevQuat(1) = quat[1]; + prevQuat(2) = quat[2]; + prevQuat(3) = quat[3]; +} + +//All possible instantiations of Filter3D +template class Filter3D; +template class Filter3D; + +} /* namespace chilitags */ + diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp new file mode 100644 index 0000000..e1416d8 --- /dev/null +++ b/src/Filter3D.hpp @@ -0,0 +1,277 @@ +/******************************************************************************* + * Copyright 2013-2014 EPFL * + * Copyright 2013-2014 Quentin Bonnard * + * * + * This file is part of chilitags. * + * * + * Chilitags is free software: you can redistribute it and/or modify * + * it under the terms of the Lesser GNU General Public License as * + * published by the Free Software Foundation, either version 3 of the * + * License, or (at your option) any later version. * + * * + * Chilitags is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with Chilitags. If not, see . * + *******************************************************************************/ + +/** + * @file Filter3D.hpp + * @brief 6D pose filter for multiple std::string ID'd objects + * @author Ayberk Özgür + * + * At the core of this class is one regular Kalman filter per detected tag. + * In these filters, the state is described as the following 7x1 vector: + * + * X(t) = (x(t), y(t), z(t), q_w(t), q_x(t), q_y(t), q_z(t))^T + * = (r(t), q(t))^T + * + * where x, y, z describe the tag position and q_w, q_x, q_y, q_z describe the + * tag orientation in unit quaternion form; this tag pose is in the camera + * frame, i.e the frame where 3D Chilitags detection is made. + * + * The state transition is made according to whether there is input data + * regarding camera movement, measured by e.g inertial sensors. There are two + * cases: + * + * 1.There is no camera movement information. In this case, the state + * transition process is described as: + * + * X(t|t-1) = I*X(t-1|t-1) + * + * where I is a 7x7 identity matrix and there is no control input. In other + * words, this transition mechanism resists against tag movement. The amount + * of resistance depends on the process noise covariance matrix Q that should + * be tuned by the user. Larger values in Q tends to decrease resistance + * against motion. + * + * 2.There is camera movement information. In this case, the state transition + * process becomes: + * + * r(t|t-1) = (deltaR_camera(t in t-1)^-1)*r(t-1|t-1) - (deltaR_camera(t in t-1)^-1)*deltaX_camera(t in t-1) + * q(t|t-1) = (deltaR_camera(t in t-1)^-1)*q(t-1|t-1) + * + * Here, deltaR_camera(t in t-1) is a unit quaternion describing the rotation + * of the camera at time t with respect to the frame described by the camera + * at time t-1. deltaX_camera(t in t-1) is a 3x1 vector describing the + * translation of the camera at time t with respect to the frame described by + * the camera at time t-1 in millimeters. Therefore, the input to the + * process at time t is the camera displacement from time t-1 to time t in + * addition to the a posteriori state estimate from time t-1. + * + * In the first equation, the multiplication represents vector rotation. In + * other words, the 3x1 vectors are left multiplied by the inverse of the + * 3x3 rotation matrix described by deltaR_camera. In the second equation, + * the multiplication represents Hamilton product, a.k.a quaternion + * multiplication. + * + * The two equations can be written in one as follows, giving the usual + * Kalman filter process equation: + * + * X(t|t-1) = F(t)*X(t-1|t-1) + B(t)*u(t) + * + * Here, F(t) is a 7x7 matrix that can be written as the following: + * + * / | \ + * | rot(deltaR_camera^-1) | 0 | + * | (3x3)| | + * F(t) = |------------------------------------------------------| + * | | | + * | 0 | mat(deltaR_camera^-1) | + * \ | (4x4)/ + * + * where rot() represents the orthogonal rotation matrix and mat() represents + * the Hamilton product matrix given the input quaternion. + * + * B(t) is a 7x3 matrix that can be written as the following: + * + * / \ + * | rot(deltaR_camera^-1) | + * | (3x3)| + * B(t) = |--------------------------| + * | | + * | 0 | + * \ / + * + * And finally, u(t) is a 3x1 vector that is equal to + * -deltaX_camera(t in t-1) + * + * Please note that if the camera displacement is zero, i.e deltaX_camera + * is (0,0,0) and deltaR_camera is (1,0,0,0), the equation reduces to the one + * in case (1) where the state stays the same. + * + * The observation, coming from the actual Chilitags detection, is + * decribed (similarly to the state) as: + * + * z(t) = (x~(t), y~(t), z~(t), q_w~(t), q_x~(t), q_y~(t), q_z~(t))^T + * = (r~(t), q~(t))^T + * + * The resistance against the movement of the tag described by the observation + * depends on the observation noise covariance matrix R that is to be tuned by + * the user. Larger values in R tend to increase the resistance against + * motion. + * + * Given these, the a posteriori state estimate is calculated, as usual, as: + * + * X(t|t) = X(t|t-1) + K(t)*(z(t) - I*x(t|t-1)) + * + * where K(t) is the current Kalman gain, calculated using the a priori state + * covariance estimate and I is a 7x7 identity matrix. Finally, the a + * posteriori state estimate is exported at each step as the filtered tag pose + * output. + */ + +#ifndef FILTER3D_HPP +#define FILTER3D_HPP + +#include + +#include +#include + +#include "chilitags.hpp" + +namespace chilitags { + +template +class Filter3D { + +public: + + /** + * @brief Creates a new 6D pose filter for multiple std::string ID'd objects + */ + Filter3D(); + + /** + * @brief Sets the persistence of tags against being discarded when not observed + * + * @param persistence Persistence value, roughly correponds to number of frames + */ + void setPersistence(RealT persistence); + + /** + * @brief Sets the process noise covariance matrix Q + * + * @param covariance 7x7 covariance matrix + */ + void setProcessNoiseCovariance(cv::Mat const& covariance); + + /** + * @brief Sets the observation noise covariance matrix R + * + * @param covariance 7x7 covariance matrix + */ + void setObservationNoiseCovariance(cv::Mat const& covariance); + + /** + * @brief Informs the rotation and translation of the current camera frame in the previous camera frame + * + * @param camDeltaR Rotation of current camera frame in previous camera frame in unit quaternion format (w,vx,vy,vz) + * @param camDeltaX Translation of current camera frame in previous camera frame + */ + void setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX); + + /** + * @brief Performs KF prediction step for all known tags + * + * @param tags The list of tags + */ + void operator()(typename Chilitags3D_::TagPoseMap& tags); + + /** + * @brief Performs KF correction step for the given tag + * + * TODO: measuredTrans and measuredRot must be double for now + * + * @param id Unique identifier of the object + * @param measuredTrans Translation measurement, also the output, 3x1 vector: (x,y,z) + * @param measuredRot Rotation measurement, also the output, 3x1 axis-angle representation: (rx,ry,rz) + */ + void operator()(std::string const& id, cv::Mat& measuredTrans, cv::Mat& measuredRot); + +private: + + const int CV_TYPE; ///< One of CV_32F, CV_64F depending on the template type + const RealT EPSILON; ///< One of FLT_EPSILON, DBL_EPSILON depending on the template type + + /** + * @brief Describes a Kalman filter, an associated previous rotation state and a lazy deletion flag + */ + struct KFQ{ + cv::KalmanFilter filter; + cv::Vec prevQuat; + bool deleted; + + KFQ(int dynamParams, int measureParams, int controlParams, int type) : + filter(dynamParams, measureParams, controlParams, type), + prevQuat(), + deleted(false) + {} + }; + + std::map mFilters; ///< We keep one filter per object and do not discard them + + cv::Mat mF; ///< Process matrix, depends on the camera movement info + cv::Mat mB; ///< Control matrix, depends on the camera movement info + cv::Mat mControl; ///< Control input, depends on the camera movement info + + cv::Mat mQ; ///< Process noise covariance matrix, to be tuned + bool mQChanged; ///< Whether Q was updated externally + cv::Mat mR; ///< Measurement noise covariance matrix, to be tuned + bool mRChanged; ///< Whether R was updated externally + + cv::Mat mCovScales; ///< Coefficients to scale the covariance matrix diagonal entries + RealT mPersistence; ///< Persistence of tags against being discarded when not seen for a while + + cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,qr,qi,qj,qk) + + /** + * @brief Recalculates the covariance matrix scale coefficients based on Q and R + */ + void recalculateCovScales(); + + /** + * @brief Initializes the filter for newly discovered tag + * + * @param filter Filter to initialize + * @param prevQuat Set to the current rotation for the future + * @param measuredTrans First measurement of position + * @param measuredRot First measurement of rotation + */ + void initFilter(cv::KalmanFilter& filter, cv::Vec& prevQuat, cv::Mat& measuredTrans, cv::Mat& measuredRot); + + /** + * @brief Converts the quaternion rotation in the state to angle-axis representation + * + * @param input 4x1 quaternion + * @param output Double precision 3x1 angle-axis + */ + void getAngleAxis(RealT* input, double* output); + + /** + * @brief Converts the angle-axis to quaternion + * + * @param input Double precision 3x1 angle-axis + * @param output 4x1 quaternion + */ + void getQuaternion(double* input, RealT* output); + + /** + * @brief Normalizes the quaternion part of the internal state vector + */ + void normalizeQuat(); + + /** + * @brief Ensures the sign of the internal state vector's quaternion is right so that we prevent quaternion unwinding + */ + void shortestPathQuat(cv::Vec& prevQuat); + +}; + +} /* namespace chilitags */ + +#endif /* FILTER3D_HPP */ diff --git a/test/Filter.cpp b/test/Filter.cpp index 3cacdc9..7663e3b 100644 --- a/test/Filter.cpp +++ b/test/Filter.cpp @@ -30,31 +30,21 @@ #include namespace { - typedef chilitags::FindOutdated - FindOutdated2D; - - typedef chilitags::FindOutdated - FindOutdated3D; - const chilitags::TagCornerMap EMPTY_TAG_LIST; const chilitags::TagCornerMap ONLY_TAG_42 = {{42, {}}}; const chilitags::TagCornerMap ONLY_TAG_43 = {{43, {}}}; - - const chilitags::Chilitags3D::TagPoseMap EMPTY_OBJECT_LIST; - const chilitags::Chilitags3D::TagPoseMap ONLY_OBJECT_42 = {{"42", {}}}; - const chilitags::Chilitags3D::TagPoseMap ONLY_OBJECT_43 = {{"43", {}}}; } -TEST(FindOutdated2D, ZeroPersistence) { - FindOutdated2D findOutdated(0); +TEST(FindOutdated, ZeroPersistence) { + chilitags::FindOutdated findOutdated(0); EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); EXPECT_EQ(1, findOutdated(EMPTY_TAG_LIST).size()); } -TEST(FindOutdated2D, InvalidateOne) { - FindOutdated2D findOutdated(3); +TEST(FindOutdated, InvalidateOne) { + chilitags::FindOutdated findOutdated(3); EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); @@ -64,8 +54,8 @@ TEST(FindOutdated2D, InvalidateOne) { EXPECT_EQ(1, findOutdated(EMPTY_TAG_LIST).size()); } -TEST(FindOutdated2D, InvalidateFirst) { - FindOutdated2D findOutdated(2); +TEST(FindOutdated, InvalidateFirst) { + chilitags::FindOutdated findOutdated(2); EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); @@ -74,8 +64,8 @@ TEST(FindOutdated2D, InvalidateFirst) { EXPECT_EQ(1, findOutdated(ONLY_TAG_43).size()); } -TEST(FindOutdated2D, ChangePersistence) { - FindOutdated2D findOutdated(2); +TEST(FindOutdated, ChangePersistence) { + chilitags::FindOutdated findOutdated(2); EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); @@ -84,48 +74,8 @@ TEST(FindOutdated2D, ChangePersistence) { EXPECT_EQ(1, findOutdated(EMPTY_TAG_LIST).size()); } -TEST(FindOutdated3D, ZeroPersistence) { - FindOutdated3D findOutdated(0); - - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_OBJECT_42).size()); - EXPECT_EQ(1, findOutdated(EMPTY_OBJECT_LIST).size()); - -} - -TEST(FindOutdated3D, InvalidateOne) { - FindOutdated3D findOutdated(3); - - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_OBJECT_42).size()); - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(1, findOutdated(EMPTY_OBJECT_LIST).size()); -} - -TEST(FindOutdated3D, InvalidateFirst) { - FindOutdated3D findOutdated(2); - - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_OBJECT_42).size()); - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(1, findOutdated(ONLY_OBJECT_43).size()); -} - -TEST(FindOutdated3D, ChangePersistence) { - FindOutdated3D findOutdated(2); - - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_OBJECT_42).size()); - findOutdated.setPersistence(1); - EXPECT_EQ(0, findOutdated(EMPTY_OBJECT_LIST).size()); - EXPECT_EQ(1, findOutdated(EMPTY_OBJECT_LIST).size()); -} - TEST(Filter, ZeroGain) { - chilitags::Filter filter(0, 0.0f); + chilitags::Filter filter(0, 0.0f); chilitags::Quad coordinates { 1.0f,2.0f, 3.0f,4.0f, @@ -151,7 +101,7 @@ TEST(Filter, ZeroGain) { } TEST(Filter, NonZeroGain) { - chilitags::Filter filter(0, 0.1f); + chilitags::Filter filter(0, 0.1f); chilitags::Quad coordinates { 1.0f,2.0f, 3.0f,4.0f, diff --git a/test/float-precision.cpp b/test/float-precision.cpp index ad817c8..7b5dde5 100644 --- a/test/float-precision.cpp +++ b/test/float-precision.cpp @@ -41,11 +41,11 @@ TEST(FloatPrecision, Snapshots) { chilitags::Chilitags3Df chilitagsf; chilitagsf.getChilitags().setPerformance(chilitags::Chilitags::ROBUST); - chilitagsf.setFilter(0, 0.0f); + chilitagsf.enableFilter(false); chilitags::Chilitags3Dd chilitagsd; chilitagsd.getChilitags().setPerformance(chilitags::Chilitags::ROBUST); - chilitagsd.setFilter(0, 0.0f); + chilitagsd.enableFilter(false); for (auto testCase : TestMetadata::all) { std::string path = std::string(cvtest::TS::ptr()->get_data_path())+testCase.filename;