From 9d48a2cab6058b39ab68f38816735afe76ac0b31 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Wed, 19 Nov 2014 16:24:22 +0100 Subject: [PATCH 01/22] Filter3D: Stubs --- src/EstimatePose3D.hpp | 4 ++++ src/Filter3D.cpp | 31 +++++++++++++++++++++++++++++++ src/Filter3D.hpp | 42 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 77 insertions(+) create mode 100644 src/Filter3D.cpp create mode 100644 src/Filter3D.hpp diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index 916409a..72e863b 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -34,6 +34,8 @@ #include +#include "Filter3D.hpp" + namespace chilitags { template @@ -87,6 +89,8 @@ class EstimatePose3D protected: + Filter3D mFilter3D; ///< Kalman filter to increase stability of the tag + 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/Filter3D.cpp b/src/Filter3D.cpp new file mode 100644 index 0000000..debc1d9 --- /dev/null +++ b/src/Filter3D.cpp @@ -0,0 +1,31 @@ +/******************************************************************************* + * 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 "Filter3D.hpp" + +chilitags::Filter3D::Filter3D() +{ +} + +void chilitags::Filter3D::operator()(std::string const& id, cv::Mat& translation, cv::Mat& rotation) +{ + +} + diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp new file mode 100644 index 0000000..d702d58 --- /dev/null +++ b/src/Filter3D.hpp @@ -0,0 +1,42 @@ +/******************************************************************************* + * 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 . * + *******************************************************************************/ + +#ifndef FILTER3D_HPP +#define FILTER3D_HPP + +#include + +#include + +namespace chilitags { + +class Filter3D { + +public: + + Filter3D(); + + void operator()(std::string const& id, cv::Mat& translation, cv::Mat& rotation); + +}; + +} /* namespace chilitags */ + +#endif /* FILTER3D_HPP */ From 4cd2e45f0e6f21136115780b4e8222b00f04fd59 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Tue, 18 Nov 2014 18:02:32 +0100 Subject: [PATCH 02/22] Filter3D: Initial Kalman filter prototype --- src/EstimatePose3D.cpp | 3 ++ src/EstimatePose3D.hpp | 2 +- src/Filter3D.cpp | 102 ++++++++++++++++++++++++++++++++++++++++- src/Filter3D.hpp | 38 ++++++++++++++- 4 files changed, 141 insertions(+), 4 deletions(-) diff --git a/src/EstimatePose3D.cpp b/src/EstimatePose3D.cpp index 58f1b5c..f96f5ec 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -33,6 +33,7 @@ namespace chilitags{ template EstimatePose3D::EstimatePose3D(cv::Size cameraResolution) : + mFilter3D(), mCameraMatrix(), mDistCoeffs() { @@ -81,6 +82,8 @@ void EstimatePose3D::operator()(std::string const& name, #endif //TODO: Rotation and translation vectors come out of solvePnP as double + mFilter3D(name, mTempTranslation, mTempRotation); + cv::Rodrigues(mTempRotation, mTempRotMat); objects[name] = { diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index 72e863b..c6576b2 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -89,7 +89,7 @@ class EstimatePose3D protected: - Filter3D mFilter3D; ///< Kalman filter to increase stability of the tag + Filter3D mFilter3D; ///< Kalman filter to increase stability of the tag 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/Filter3D.cpp b/src/Filter3D.cpp index debc1d9..26b524e 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -18,14 +18,112 @@ * 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" -chilitags::Filter3D::Filter3D() +#include + +namespace chilitags{ + +template +inline int Filter3D::getCVType() +{ + return std::is_same::value ? CV_64F : CV_32F; +} + +template +Filter3D::Filter3D() : + mQ(), + mR(), + mTempState(6,1, getCVType()) { + //Process noise covariance is 6x6: cov((x,y,z,rx,ry,rz)) + mQ = (cv::Mat_(6,6) << + 1e-3f, 0, 0, 0, 0, 0, + 0, 1e-3f, 0, 0, 0, 0, + 0, 0, 1e-3f, 0, 0, 0, + 0, 0, 0, 1e-5f, 0, 0, + 0, 0, 0, 0, 1e-5f, 0, + 0, 0, 0, 0, 0, 1e-5f + ); + + //Measurement noise covariance is 6x6: cov((x,y,z,rx,ry,rz)) + mR = (cv::Mat_(6,6) << + 1e-2f, 0, 0, 0, 0, 0, + 0, 1e-2f, 0, 0, 0, 0, + 0, 0, 1e-1f, 0, 0, 0, + 0, 0, 0, 1e-2f, 0, 0, + 0, 0, 0, 0, 1e-2f, 0, + 0, 0, 0, 0, 0, 1e-3f + ); } -void chilitags::Filter3D::operator()(std::string const& id, cv::Mat& translation, cv::Mat& rotation) +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: state dimensions, measurement dimensions, + //control input dimensions and float precision of internal matrices + auto pair = mFilters.emplace(std::piecewise_construct, + std::make_tuple(id), + std::make_tuple(6, 6, 0, getCVType())); + cv::KalmanFilter& filter = pair.first->second; + + //Newly inserted + if(pair.second){ + mQ.copyTo(filter.processNoiseCov); + mR.copyTo(filter.measurementNoiseCov); + //TODO: Control input will be the IMU data that "drives" the tag + //cv::setIdentity(filter.controlMatrix); + + //We have no expectation from a tag other than staying still + cv::setIdentity(filter.transitionMatrix); + + //We make the same measurement as the state: (x,y,z,rx,ry,rz) + cv::setIdentity(filter.measurementMatrix); + + //Set initial state + //TODO: Replace these with memcpys + filter.statePost.at(0) = (RealT)measuredTrans.at(0); + filter.statePost.at(1) = (RealT)measuredTrans.at(1); + filter.statePost.at(2) = (RealT)measuredTrans.at(2); + filter.statePost.at(3) = (RealT)measuredRot.at(0); + filter.statePost.at(4) = (RealT)measuredRot.at(1); + filter.statePost.at(5) = (RealT)measuredRot.at(2); + } + + //Already existing filter + else{ + + //TODO: Replace these with memcpys + mTempState.at(0) = (RealT)measuredTrans.at(0); + mTempState.at(1) = (RealT)measuredTrans.at(1); + mTempState.at(2) = (RealT)measuredTrans.at(2); + mTempState.at(3) = (RealT)measuredRot.at(0); + mTempState.at(4) = (RealT)measuredRot.at(1); + mTempState.at(5) = (RealT)measuredRot.at(2); + + filter.predict(); + filter.correct(mTempState).copyTo(mTempState); + + //TODO: Replace these with memcpys + measuredTrans.at(0) = mTempState.at(0,0); + measuredTrans.at(1) = mTempState.at(1,0); + measuredTrans.at(2) = mTempState.at(2,0); + measuredRot.at(0) = mTempState.at(3,0); + measuredRot.at(1) = mTempState.at(4,0); + measuredRot.at(2) = mTempState.at(5,0); + } } +//All possible instantiations of Filter3D +template class Filter3D; +template class Filter3D; + +} /* namespace chilitags */ diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index d702d58..9ba5cff 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -18,22 +18,58 @@ * along with Chilitags. If not, see . * *******************************************************************************/ +/** + * @file Filter3D.hpp + * @brief 6D pose filter for multiple std::string ID'd objects + * @author Ayberk Özgür + */ + #ifndef FILTER3D_HPP #define FILTER3D_HPP #include #include +#include namespace chilitags { +template class Filter3D { public: + /** + * @brief Creates a new 6D pose filter for multiple std::string ID'd objects + */ Filter3D(); - void operator()(std::string const& id, cv::Mat& translation, cv::Mat& rotation); + /** + * @brief Filters the given 6D pose depending on the past measurements + * + * TODO: measuredTrans and measuredRot must be double for now + * + * @param id Unique identifier of the object + * @param measuredTrans Translation measurement to be filtered, 3x1 vector: (x,y,z) + * @param measuredRot Rotation measurement to be filtered, 3x1 vector: (rx,ry,rz) + */ + void operator()(std::string const& id, cv::Mat& measuredTrans, cv::Mat& measuredRot); + +private: + + std::map mFilters; ///< We keep one filter per object and do not discard them + + cv::Mat mQ; ///< Process noise covariance matrix, to be tuned + cv::Mat mR; ///< Measurement noise covariance matrix, to be tuned + + cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,rx,ry,rz) + + /** + * @brief Gets one of CV_32F, CV_64F depending on the template type + * + * @return CV_32F or CV_64F + */ + int getCVType(); }; From b9b05242ec158d8bcf7ebdb5ae086fe4c63c096f Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Wed, 19 Nov 2014 15:34:29 +0100 Subject: [PATCH 03/22] Filter3D: Replace angle-axis with quaternion representation in state --- src/Filter3D.cpp | 196 +++++++++++++++++++++++++++++++++++------------ src/Filter3D.hpp | 42 ++++++++-- 2 files changed, 182 insertions(+), 56 deletions(-) diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index 26b524e..1963d54 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -27,52 +27,55 @@ #include "Filter3D.hpp" #include +#include +#include namespace chilitags{ -template -inline int Filter3D::getCVType() -{ - return std::is_same::value ? CV_64F : CV_32F; -} - template Filter3D::Filter3D() : + CV_TYPE(std::is_same::value ? CV_64F : CV_32F), + EPSILON(std::is_same::value ? DBL_EPSILON : FLT_EPSILON), mQ(), mR(), - mTempState(6,1, getCVType()) + mTempState(7,1, CV_TYPE) { + //Process noise covariance is 6x6: cov((x,y,z,rx,ry,rz)) - mQ = (cv::Mat_(6,6) << - 1e-3f, 0, 0, 0, 0, 0, - 0, 1e-3f, 0, 0, 0, 0, - 0, 0, 1e-3f, 0, 0, 0, - 0, 0, 0, 1e-5f, 0, 0, - 0, 0, 0, 0, 1e-5f, 0, - 0, 0, 0, 0, 0, 1e-5f - ); + 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 6x6: cov((x,y,z,rx,ry,rz)) - mR = (cv::Mat_(6,6) << - 1e-2f, 0, 0, 0, 0, 0, - 0, 1e-2f, 0, 0, 0, 0, - 0, 0, 1e-1f, 0, 0, 0, - 0, 0, 0, 1e-2f, 0, 0, - 0, 0, 0, 0, 1e-2f, 0, - 0, 0, 0, 0, 0, 1e-3f - ); + 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); } 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: state dimensions, measurement dimensions, - //control input dimensions and float precision of internal matrices + //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(6, 6, 0, getCVType())); - cv::KalmanFilter& filter = pair.first->second; + std::make_tuple(7, 7, 0, CV_TYPE)); + cv::KalmanFilter& filter = pair.first->second.filter; + cv::Vec& prevQuat = pair.first->second.prevQuat; + + RealT theta = norm(measuredRot); + RealT sTheta2 = sin(theta/2); //Newly inserted if(pair.second){ @@ -85,45 +88,138 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //We have no expectation from a tag other than staying still cv::setIdentity(filter.transitionMatrix); - //We make the same measurement as the state: (x,y,z,rx,ry,rz) + //We make the same measurement as the state: (x,y,z,qr,qi,qj,qk) cv::setIdentity(filter.measurementMatrix); //Set initial state - //TODO: Replace these with memcpys - filter.statePost.at(0) = (RealT)measuredTrans.at(0); - filter.statePost.at(1) = (RealT)measuredTrans.at(1); - filter.statePost.at(2) = (RealT)measuredTrans.at(2); - filter.statePost.at(3) = (RealT)measuredRot.at(0); - filter.statePost.at(4) = (RealT)measuredRot.at(1); - filter.statePost.at(5) = (RealT)measuredRot.at(2); + //TODO: Replace these with memcpys when double/float is unified + filter.statePost.at(0) = (RealT)measuredTrans.at(0); //x + filter.statePost.at(1) = (RealT)measuredTrans.at(1); //y + filter.statePost.at(2) = (RealT)measuredTrans.at(2); //z + + filter.statePost.at(3) = cos(theta/2); //qr + if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } + filter.statePost.at(4) = ((RealT)measuredRot.at(0)); //qi + filter.statePost.at(5) = ((RealT)measuredRot.at(1)); //qj + filter.statePost.at(6) = ((RealT)measuredRot.at(2)); //qk + } + else{ + filter.statePost.at(4) = ((RealT)measuredRot.at(0))/theta*sTheta2; //qi + filter.statePost.at(5) = ((RealT)measuredRot.at(1))/theta*sTheta2; //qj + filter.statePost.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk + } + + prevQuat(0) = filter.statePost.at(3); + prevQuat(1) = filter.statePost.at(4); + prevQuat(2) = filter.statePost.at(5); + prevQuat(3) = filter.statePost.at(6); } //Already existing filter else{ - //TODO: Replace these with memcpys - mTempState.at(0) = (RealT)measuredTrans.at(0); - mTempState.at(1) = (RealT)measuredTrans.at(1); - mTempState.at(2) = (RealT)measuredTrans.at(2); - mTempState.at(3) = (RealT)measuredRot.at(0); - mTempState.at(4) = (RealT)measuredRot.at(1); - mTempState.at(5) = (RealT)measuredRot.at(2); - + //TODO: Replace these with memcpys when double/float is unified + mTempState.at(0) = (RealT)measuredTrans.at(0); //x + mTempState.at(1) = (RealT)measuredTrans.at(1); //y + mTempState.at(2) = (RealT)measuredTrans.at(2); //z + + mTempState.at(3) = cos(theta/2); //qr + if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } + mTempState.at(4) = ((RealT)measuredRot.at(0)); //qi + mTempState.at(5) = ((RealT)measuredRot.at(1)); //qj + mTempState.at(6) = ((RealT)measuredRot.at(2)); //qk + } + else{ + mTempState.at(4) = ((RealT)measuredRot.at(0))/theta*sTheta2; //qi + mTempState.at(5) = ((RealT)measuredRot.at(1))/theta*sTheta2; //qj + mTempState.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk + } + + shortestPathQuat(prevQuat); filter.predict(); filter.correct(mTempState).copyTo(mTempState); + normalizeQuat(); + + //TODO: Replace these with memcpys when double/float is unified + measuredTrans.at(0) = mTempState.at(0); //x + measuredTrans.at(1) = mTempState.at(1); //y + measuredTrans.at(2) = mTempState.at(2); //z + + theta = sqrt( + mTempState.at(4)*mTempState.at(4) + + mTempState.at(5)*mTempState.at(5) + + mTempState.at(6)*mTempState.at(6)); + theta = 2*atan2(theta, mTempState.at(3)); + sTheta2 = sin(theta/2); + if(theta < EPSILON){ //Use lim( theta -> 0 ){ theta/sin(theta) } + measuredRot.at(0) = mTempState.at(4); //rx + measuredRot.at(1) = mTempState.at(5); //ry + measuredRot.at(2) = mTempState.at(6); //rz + } + else{ + measuredRot.at(0) = mTempState.at(4)*theta/sTheta2; //rx + measuredRot.at(1) = mTempState.at(5)*theta/sTheta2; //ry + measuredRot.at(2) = mTempState.at(6)*theta/sTheta2; //rz + } + } +} - //TODO: Replace these with memcpys - measuredTrans.at(0) = mTempState.at(0,0); - measuredTrans.at(1) = mTempState.at(1,0); - measuredTrans.at(2) = mTempState.at(2,0); - measuredRot.at(0) = mTempState.at(3,0); - measuredRot.at(1) = mTempState.at(4,0); - measuredRot.at(2) = mTempState.at(5,0); +template +inline RealT Filter3D::norm(cv::Mat& vec3d) +{ + return sqrt((RealT)( + vec3d.at(0)*vec3d.at(0) + + vec3d.at(1)*vec3d.at(1) + + vec3d.at(2)*vec3d.at(2))); +} + +template +inline void Filter3D::normalizeQuat() +{ + RealT norm = sqrt( + mTempState.at(3)*mTempState.at(3) + + mTempState.at(4)*mTempState.at(4) + + mTempState.at(5)*mTempState.at(5) + + mTempState.at(6)*mTempState.at(6)); + + if(norm > EPSILON){ + mTempState.at(3) /= norm; + mTempState.at(4) /= norm; + mTempState.at(5) /= norm; + mTempState.at(6) /= norm; + } + else{ + mTempState.at(3) = 1.0f; + mTempState.at(4) = 0.0f; + mTempState.at(5) = 0.0f; + mTempState.at(6) = 0.0f; } } +template +inline void Filter3D::shortestPathQuat(cv::Vec& prevQuat) +{ + //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( + mTempState.at(3)*prevQuat(0) + + mTempState.at(4)*prevQuat(1) + + mTempState.at(5)*prevQuat(2) + + mTempState.at(6)*prevQuat(3) < 0){ + mTempState.at(3) = -mTempState.at(3); + mTempState.at(4) = -mTempState.at(4); + mTempState.at(5) = -mTempState.at(5); + mTempState.at(6) = -mTempState.at(6); + } + prevQuat(0) = mTempState.at(3); + prevQuat(1) = mTempState.at(4); + prevQuat(2) = mTempState.at(5); + prevQuat(3) = mTempState.at(6); +} + //All possible instantiations of Filter3D template class Filter3D; template class Filter3D; } /* namespace chilitags */ + diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index 9ba5cff..c7be68e 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -51,25 +51,55 @@ class Filter3D { * * @param id Unique identifier of the object * @param measuredTrans Translation measurement to be filtered, 3x1 vector: (x,y,z) - * @param measuredRot Rotation measurement to be filtered, 3x1 vector: (rx,ry,rz) + * @param measuredRot Rotation measurement to be filtered, 3x1 axis-angle representation: (rx,ry,rz) */ void operator()(std::string const& id, cv::Mat& measuredTrans, cv::Mat& measuredRot); private: - std::map mFilters; ///< We keep one filter per object and do not discard them + 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 and an associated previous quaternion rotation state + */ + struct KFQ{ + cv::KalmanFilter filter; + cv::Vec prevQuat; + + KFQ(int dynamParams, int measureParams, int controlParams, int type) : + filter(dynamParams, measureParams, controlParams, type), + prevQuat() + {} + }; + + std::map mFilters; ///< We keep one filter per object and do not discard them cv::Mat mQ; ///< Process noise covariance matrix, to be tuned cv::Mat mR; ///< Measurement noise covariance matrix, to be tuned - cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,rx,ry,rz) + cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,qr,qi,qj,qk) /** - * @brief Gets one of CV_32F, CV_64F depending on the template type + * @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); + + /** + * @brief Calculates the norm of the given 3-vector + * + * TODO: vec3d must be double for now + * + * @param vec3d 3-vector of doubles * - * @return CV_32F or CV_64F + * @return The calculated norm in single-precision */ - int getCVType(); + RealT norm(cv::Mat& vec3d); }; From 492c1c243ccc81c9a9be604efcd4de1c24606b44 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Wed, 19 Nov 2014 15:59:36 +0100 Subject: [PATCH 04/22] Filter3D: Updated TODO note for memcpys --- src/Filter3D.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index 1963d54..ebf1029 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -30,6 +30,8 @@ #include #include +//TODO: Replace all assignment sequences with memcpys (if possible) when double/float is unified + namespace chilitags{ template @@ -92,7 +94,6 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, cv::setIdentity(filter.measurementMatrix); //Set initial state - //TODO: Replace these with memcpys when double/float is unified filter.statePost.at(0) = (RealT)measuredTrans.at(0); //x filter.statePost.at(1) = (RealT)measuredTrans.at(1); //y filter.statePost.at(2) = (RealT)measuredTrans.at(2); //z @@ -117,8 +118,6 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //Already existing filter else{ - - //TODO: Replace these with memcpys when double/float is unified mTempState.at(0) = (RealT)measuredTrans.at(0); //x mTempState.at(1) = (RealT)measuredTrans.at(1); //y mTempState.at(2) = (RealT)measuredTrans.at(2); //z @@ -140,7 +139,6 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, filter.correct(mTempState).copyTo(mTempState); normalizeQuat(); - //TODO: Replace these with memcpys when double/float is unified measuredTrans.at(0) = mTempState.at(0); //x measuredTrans.at(1) = mTempState.at(1); //y measuredTrans.at(2) = mTempState.at(2); //z From 07eb4131db4782b07b727518af2892661520dfe2 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Wed, 19 Nov 2014 17:04:15 +0100 Subject: [PATCH 05/22] Filter3D: Enable/disable API --- include/chilitags.hpp | 7 +++++++ src/Chilitags3D.cpp | 9 +++++++++ src/EstimatePose3D.cpp | 10 +++++++++- src/EstimatePose3D.hpp | 8 ++++++++ 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/include/chilitags.hpp b/include/chilitags.hpp index 8365f81..3b20adc 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -494,6 +494,13 @@ 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 enableFilter3D(bool enabled); + /** For accurate results, Chilitags3D can be provided the calibration data of the camera detecting the chilitags. See diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index ad45f84..db5ce9d 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -138,6 +138,10 @@ void setDefaultTagSize(RealT defaultSize){ }; } +void enableFilter3D(bool enabled){ + mEstimatePose3D.enableFilter(enabled); +} + bool read3DConfiguration(const std::string &filenameOrString, bool omitOtherTags, bool readFromString) { mOmitOtherTags = omitOtherTags; @@ -327,6 +331,11 @@ void Chilitags3D_::setDefaultTagSize(RealT defaultSize){ mImpl->setDefaultTagSize(defaultSize); } +template +void Chilitags3D_::enableFilter3D(bool enabled){ + mImpl->enableFilter3D(enabled); +} + 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 f96f5ec..47e2dca 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -34,6 +34,7 @@ namespace chilitags{ template EstimatePose3D::EstimatePose3D(cv::Size cameraResolution) : mFilter3D(), + mFilter3DEnabled(true), mCameraMatrix(), mDistCoeffs() { @@ -64,6 +65,12 @@ cv::Mat const& EstimatePose3D::getDistortionCoeffs() const return mDistCoeffs; } +template +void EstimatePose3D::enableFilter(bool enabled) +{ + mFilter3DEnabled = enabled; +} + template void EstimatePose3D::operator()(std::string const& name, std::vector> const& objectPoints, @@ -82,7 +89,8 @@ void EstimatePose3D::operator()(std::string const& name, #endif //TODO: Rotation and translation vectors come out of solvePnP as double - mFilter3D(name, mTempTranslation, mTempRotation); + if(mFilter3DEnabled) + mFilter3D(name, mTempTranslation, mTempRotation); cv::Rodrigues(mTempRotation, mTempRotMat); diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index c6576b2..6b020b2 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -74,6 +74,13 @@ 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 Updates/inserts the pose of the given object in the given map * @@ -90,6 +97,7 @@ 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 From e1f1fc5a8cd41545355f0558b729b6859facea34 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Wed, 19 Nov 2014 18:11:14 +0100 Subject: [PATCH 06/22] Filter3D: Added sample --- samples/3dfiltering/filter3d-gui.cpp | 164 +++++++++++++++++++++++++++ samples/CMakeLists.txt | 5 + 2 files changed, 169 insertions(+) create mode 100644 samples/3dfiltering/filter3d-gui.cpp diff --git a/samples/3dfiltering/filter3d-gui.cpp b/samples/3dfiltering/filter3d-gui.cpp new file mode 100644 index 0000000..da63d8c --- /dev/null +++ b/samples/3dfiltering/filter3d-gui.cpp @@ -0,0 +1,164 @@ +/******************************************************************************* + * 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; + while ('q' != (keyPressed = (char) cv::waitKey(1))) { + + if(keyPressed == 'f'){ + filterEnabled = !filterEnabled; + chilitags3D.enableFilter3D(filterEnabled); + } + + 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)); + + 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< Date: Wed, 10 Dec 2014 12:50:04 +0100 Subject: [PATCH 07/22] Filter3D: IMU API preview --- include/chilitags.hpp | 24 ++++++++++++++++++++++-- src/Chilitags3D.cpp | 22 ++++++++++++++-------- src/EstimatePose3D.cpp | 6 ++++++ src/EstimatePose3D.hpp | 8 ++++++++ src/Filter3D.cpp | 9 +++++++++ src/Filter3D.hpp | 8 ++++++++ 6 files changed, 67 insertions(+), 10 deletions(-) diff --git a/include/chilitags.hpp b/include/chilitags.hpp index 3b20adc..2acd679 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -424,8 +424,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 +459,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 diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index db5ce9d..415056a 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -37,10 +37,10 @@ class Chilitags3D_::Impl { public: Impl(cv::Size cameraResolution) : mChilitags(), + mEstimatePose3D(cameraResolution), mOmitOtherTags(false), mDefaultTagCorners(), mId2Configuration(), - mEstimatePose3D(cameraResolution), mFilter(5, 0.5f) { setDefaultTagSize(20.f); @@ -59,7 +59,7 @@ 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; @@ -125,8 +125,10 @@ TagPoseMap estimate(const TagCornerMap &tags) { 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){ @@ -315,15 +317,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 diff --git a/src/EstimatePose3D.cpp b/src/EstimatePose3D.cpp index 47e2dca..eaecb6e 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -71,6 +71,12 @@ void EstimatePose3D::enableFilter(bool enabled) mFilter3DEnabled = enabled; } +template +void EstimatePose3D::setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) +{ + mFilter3D.setCamDelta(camDeltaR, camDeltaX); +} + template void EstimatePose3D::operator()(std::string const& name, std::vector> const& objectPoints, diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index 6b020b2..0841f1b 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -81,6 +81,14 @@ class EstimatePose3D */ void enableFilter(bool enabled); + /** + * @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/inserts the pose of the given object in the given map * diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index ebf1029..fc61c71 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -62,6 +62,15 @@ Filter3D::Filter3D() : 0, 0, 0, 0, 1e-2f, 0, 0, 0, 0, 0, 0, 0, 1e-2f, 0, 0, 0, 0, 0, 0, 0, 1e-5f); +} + +template +void Filter3D::setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) +{ + + + + } template diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index c7be68e..08ab0f8 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -44,6 +44,14 @@ class Filter3D { */ Filter3D(); + /** + * @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 Filters the given 6D pose depending on the past measurements * From 734bf04eaf116e68d974f220e2e4f9656d2fd45f Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 11:06:58 +0100 Subject: [PATCH 08/22] Filter3D: Separate prediction/correction and process IMU input --- src/Chilitags3D.cpp | 13 +++-- src/EstimatePose3D.cpp | 6 +++ src/EstimatePose3D.hpp | 7 +++ src/Filter3D.cpp | 114 ++++++++++++++++++++++++++++++++++++++--- src/Filter3D.hpp | 29 ++++++++--- 5 files changed, 150 insertions(+), 19 deletions(-) diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index 415056a..26d3133 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -59,10 +59,17 @@ const Chilitags &getChilitags() const { return mChilitags; } -TagPoseMap estimate(TagCornerMap const& tags, cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) { - +TagPoseMap estimate(TagCornerMap const& tags, cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) +{ TagPoseMap objects; + //Pass the latest camera movement difference for prediction + mEstimatePose3D.setCamDelta(camDeltaR, camDeltaX); + + //Predict pose for all known tags with camera movement + mEstimatePose3D(objects); + + //Correct pose prediction with new observations std::map< const std::string, //name of the object std::pair< @@ -120,7 +127,7 @@ TagPoseMap estimate(TagCornerMap const& tags, cv::Vec const& camDeltaR objects); } - return mFilter(objects); + return objects; } TagPoseMap estimate( diff --git a/src/EstimatePose3D.cpp b/src/EstimatePose3D.cpp index eaecb6e..323a51d 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -77,6 +77,12 @@ void EstimatePose3D::setCamDelta(cv::Vec const& camDeltaR, cv:: mFilter3D.setCamDelta(camDeltaR, camDeltaX); } +template +void EstimatePose3D::operator()(typename Chilitags3D_::TagPoseMap& objects) +{ + mFilter3D(objects); +} + template void EstimatePose3D::operator()(std::string const& name, std::vector> const& objectPoints, diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index 0841f1b..e1e45d0 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -89,6 +89,13 @@ class EstimatePose3D */ 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 * diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index fc61c71..8fa3944 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -30,6 +30,8 @@ #include #include +#include + //TODO: Replace all assignment sequences with memcpys (if possible) when double/float is unified namespace chilitags{ @@ -38,12 +40,15 @@ 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(), mR(), - mTempState(7,1, CV_TYPE) + mTempState(7, 1, CV_TYPE) { - //Process noise covariance is 6x6: cov((x,y,z,rx,ry,rz)) + //Process noise covariance is 7x7: cov((x,y,z,qw,qi,qj,qk)) mQ = (cv::Mat_(7,7) << 1e-3f, 0, 0, 0, 0, 0, 0, 0, 1e-3f, 0, 0, 0, 0, 0, @@ -53,7 +58,7 @@ Filter3D::Filter3D() : 0, 0, 0, 0, 0, 1e-4f, 0, 0, 0, 0, 0, 0, 0, 1e-4f); - //Measurement noise covariance is 6x6: cov((x,y,z,rx,ry,rz)) + //Measurement noise covariance is 7x7: cov((x,y,z,qw,qi,qj,qk)) mR = (cv::Mat_(7,7) << 1e-3f, 0, 0, 0, 0, 0, 0, 0, 1e-3f, 0, 0, 0, 0, 0, @@ -62,15 +67,103 @@ Filter3D::Filter3D() : 0, 0, 0, 0, 1e-2f, 0, 0, 0, 0, 0, 0, 0, 1e-2f, 0, 0, 0, 0, 0, 0, 0, 1e-5f); + + //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 -void Filter3D::setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) +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) + //TODO: memcpy + B0[0] = F0[0]; B0[1] = F0[1]; B0[2] = F0[2]; + B1[0] = F1[0]; B1[1] = F1[1]; B1[2] = F1[2]; + B2[0] = F2[0]; B2[1] = F2[1]; B2[2] = F2[2]; + + //Set the control input + //TODO: memcpy + 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; + RealT theta; + RealT sTheta2; + + for(auto& kfq : mFilters){ + cv::KalmanFilter& filter = kfq.second.filter; + //cv::Vec& prevQuat = kfq.second.prevQuat; + + //Prediction step + mF.copyTo(filter.transitionMatrix); + mB.copyTo(filter.controlMatrix); + filter.predict(mControl).copyTo(mTempState); + + //Convert quaternion rotation to angle-axis rotation + theta = sqrt( + mTempState.at(4)*mTempState.at(4) + + mTempState.at(5)*mTempState.at(5) + + mTempState.at(6)*mTempState.at(6)); + theta = 2*atan2(theta, mTempState.at(3)); + sTheta2 = sin(theta/2); + if(theta < EPSILON){ //Use lim( theta -> 0 ){ theta/sin(theta) } + predictedRot.at(0) = mTempState.at(4); //rx + predictedRot.at(1) = mTempState.at(5); //ry + predictedRot.at(2) = mTempState.at(6); //rz + } + else{ + predictedRot.at(0) = mTempState.at(4)*theta/sTheta2; //rx + predictedRot.at(1) = mTempState.at(5)*theta/sTheta2; //ry + predictedRot.at(2) = mTempState.at(6)*theta/sTheta2; //rz + } + //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 + }; + } } template @@ -81,7 +174,7 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //# 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, 0, CV_TYPE)); + std::make_tuple(7, 7, 3, CV_TYPE)); cv::KalmanFilter& filter = pair.first->second.filter; cv::Vec& prevQuat = pair.first->second.prevQuat; @@ -96,7 +189,7 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //TODO: Control input will be the IMU data that "drives" the tag //cv::setIdentity(filter.controlMatrix); - //We have no expectation from a tag other than staying still + //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,qr,qi,qj,qk) @@ -127,6 +220,12 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //Already existing filter else{ + + //Prediction step + //mF.copyTo(filter.transitionMatrix); + //filter.predict(); + + //Correction step mTempState.at(0) = (RealT)measuredTrans.at(0); //x mTempState.at(1) = (RealT)measuredTrans.at(1); //y mTempState.at(2) = (RealT)measuredTrans.at(2); //z @@ -144,7 +243,6 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, } shortestPathQuat(prevQuat); - filter.predict(); filter.correct(mTempState).copyTo(mTempState); normalizeQuat(); diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index 08ab0f8..7536d46 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -32,6 +32,8 @@ #include #include +#include "chilitags.hpp" + namespace chilitags { template @@ -44,7 +46,7 @@ class Filter3D { */ Filter3D(); - /** + /** * @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) @@ -53,13 +55,20 @@ class Filter3D { void setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX); /** - * @brief Filters the given 6D pose depending on the past measurements + * @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 to be filtered, 3x1 vector: (x,y,z) - * @param measuredRot Rotation measurement to be filtered, 3x1 axis-angle representation: (rx,ry,rz) + * @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); @@ -81,12 +90,16 @@ class Filter3D { {} }; - std::map mFilters; ///< We keep one filter per object and do not discard them + 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 - cv::Mat mR; ///< Measurement noise covariance matrix, to be tuned + cv::Mat mQ; ///< Process noise covariance matrix, to be tuned + cv::Mat mR; ///< Measurement noise covariance matrix, to be tuned - cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,qr,qi,qj,qk) + cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,qr,qi,qj,qk) /** * @brief Normalizes the quaternion part of the internal state vector From ff44df8c77ed256af5e949d2355dfb1980dd66cb Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 11:58:20 +0100 Subject: [PATCH 09/22] Filter3D: Refactored filter init and quat conversion --- src/Filter3D.cpp | 147 +++++++++++++++++++++-------------------------- src/Filter3D.hpp | 17 ++++++ 2 files changed, 84 insertions(+), 80 deletions(-) diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index 8fa3944..aec88b2 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -111,7 +111,6 @@ void Filter3D::setCamDelta(cv::Vec const& q, cv::Vec B2[0] = F2[0]; B2[1] = F2[1]; B2[2] = F2[2]; //Set the control input - //TODO: memcpy u[0] = -camDeltaX(0); u[1] = -camDeltaX(1); u[2] = -camDeltaX(2); @@ -123,35 +122,17 @@ 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; - RealT theta; - RealT sTheta2; for(auto& kfq : mFilters){ cv::KalmanFilter& filter = kfq.second.filter; - //cv::Vec& prevQuat = kfq.second.prevQuat; - //Prediction step + //Do prediction step mF.copyTo(filter.transitionMatrix); mB.copyTo(filter.controlMatrix); filter.predict(mControl).copyTo(mTempState); //Convert quaternion rotation to angle-axis rotation - theta = sqrt( - mTempState.at(4)*mTempState.at(4) + - mTempState.at(5)*mTempState.at(5) + - mTempState.at(6)*mTempState.at(6)); - theta = 2*atan2(theta, mTempState.at(3)); - sTheta2 = sin(theta/2); - if(theta < EPSILON){ //Use lim( theta -> 0 ){ theta/sin(theta) } - predictedRot.at(0) = mTempState.at(4); //rx - predictedRot.at(1) = mTempState.at(5); //ry - predictedRot.at(2) = mTempState.at(6); //rz - } - else{ - predictedRot.at(0) = mTempState.at(4)*theta/sTheta2; //rx - predictedRot.at(1) = mTempState.at(5)*theta/sTheta2; //ry - predictedRot.at(2) = mTempState.at(6)*theta/sTheta2; //rz - } + getAngleAxis(predictedRot); //Convert angle-axis to 3x3 rotation matrix cv::Rodrigues(predictedRot, tempRotMat); @@ -178,54 +159,16 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, cv::KalmanFilter& filter = pair.first->second.filter; cv::Vec& prevQuat = pair.first->second.prevQuat; - RealT theta = norm(measuredRot); - RealT sTheta2 = sin(theta/2); - //Newly inserted - if(pair.second){ - mQ.copyTo(filter.processNoiseCov); - mR.copyTo(filter.measurementNoiseCov); - - //TODO: Control input will be the IMU data that "drives" the tag - //cv::setIdentity(filter.controlMatrix); - - //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,qr,qi,qj,qk) - cv::setIdentity(filter.measurementMatrix); - - //Set initial state - filter.statePost.at(0) = (RealT)measuredTrans.at(0); //x - filter.statePost.at(1) = (RealT)measuredTrans.at(1); //y - filter.statePost.at(2) = (RealT)measuredTrans.at(2); //z - - filter.statePost.at(3) = cos(theta/2); //qr - if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } - filter.statePost.at(4) = ((RealT)measuredRot.at(0)); //qi - filter.statePost.at(5) = ((RealT)measuredRot.at(1)); //qj - filter.statePost.at(6) = ((RealT)measuredRot.at(2)); //qk - } - else{ - filter.statePost.at(4) = ((RealT)measuredRot.at(0))/theta*sTheta2; //qi - filter.statePost.at(5) = ((RealT)measuredRot.at(1))/theta*sTheta2; //qj - filter.statePost.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk - } - - prevQuat(0) = filter.statePost.at(3); - prevQuat(1) = filter.statePost.at(4); - prevQuat(2) = filter.statePost.at(5); - prevQuat(3) = filter.statePost.at(6); - } + if(pair.second) + initFilter(filter, prevQuat, measuredTrans, measuredRot); //Already existing filter else{ + RealT theta = norm(measuredRot); + RealT sTheta2 = sin(theta/2); - //Prediction step - //mF.copyTo(filter.transitionMatrix); - //filter.predict(); - - //Correction step + //Fill state mTempState.at(0) = (RealT)measuredTrans.at(0); //x mTempState.at(1) = (RealT)measuredTrans.at(1); //y mTempState.at(2) = (RealT)measuredTrans.at(2); //z @@ -242,30 +185,74 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, mTempState.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk } + //Do the correction step shortestPathQuat(prevQuat); filter.correct(mTempState).copyTo(mTempState); normalizeQuat(); + //Write state back measuredTrans.at(0) = mTempState.at(0); //x measuredTrans.at(1) = mTempState.at(1); //y measuredTrans.at(2) = mTempState.at(2); //z + getAngleAxis(measuredRot); + } +} - theta = sqrt( - mTempState.at(4)*mTempState.at(4) + - mTempState.at(5)*mTempState.at(5) + - mTempState.at(6)*mTempState.at(6)); - theta = 2*atan2(theta, mTempState.at(3)); - sTheta2 = sin(theta/2); - if(theta < EPSILON){ //Use lim( theta -> 0 ){ theta/sin(theta) } - measuredRot.at(0) = mTempState.at(4); //rx - measuredRot.at(1) = mTempState.at(5); //ry - measuredRot.at(2) = mTempState.at(6); //rz - } - else{ - measuredRot.at(0) = mTempState.at(4)*theta/sTheta2; //rx - measuredRot.at(1) = mTempState.at(5)*theta/sTheta2; //ry - measuredRot.at(2) = mTempState.at(6)*theta/sTheta2; //rz - } +template +void Filter3D::initFilter(cv::KalmanFilter& filter, cv::Vec& prevQuat, cv::Mat& measuredTrans, cv::Mat& measuredRot) +{ + mQ.copyTo(filter.processNoiseCov); + mR.copyTo(filter.measurementNoiseCov); + + //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,qr,qi,qj,qk) + cv::setIdentity(filter.measurementMatrix); + + //Set initial state + filter.statePost.at(0) = (RealT)measuredTrans.at(0); //x + filter.statePost.at(1) = (RealT)measuredTrans.at(1); //y + filter.statePost.at(2) = (RealT)measuredTrans.at(2); //z + + RealT theta = norm(measuredRot); + filter.statePost.at(3) = cos(theta/2); //qr + if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } + filter.statePost.at(4) = ((RealT)measuredRot.at(0)); //qi + filter.statePost.at(5) = ((RealT)measuredRot.at(1)); //qj + filter.statePost.at(6) = ((RealT)measuredRot.at(2)); //qk + } + else{ + RealT sTheta2 = sin(theta/2); + filter.statePost.at(4) = ((RealT)measuredRot.at(0))/theta*sTheta2; //qi + filter.statePost.at(5) = ((RealT)measuredRot.at(1))/theta*sTheta2; //qj + filter.statePost.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk + } + + prevQuat(0) = filter.statePost.at(3); + prevQuat(1) = filter.statePost.at(4); + prevQuat(2) = filter.statePost.at(5); + prevQuat(3) = filter.statePost.at(6); +} + +template +void Filter3D::getAngleAxis(cv::Mat& output) +{ + RealT theta = sqrt( + mTempState.at(4)*mTempState.at(4) + + mTempState.at(5)*mTempState.at(5) + + mTempState.at(6)*mTempState.at(6)); + theta = 2*atan2(theta, mTempState.at(3)); + RealT sTheta2 = sin(theta/2); + if(theta < EPSILON){ //Use lim( theta -> 0 ){ theta/sin(theta) } + output.at(0) = mTempState.at(4); //rx + output.at(1) = mTempState.at(5); //ry + output.at(2) = mTempState.at(6); //rz + } + else{ + output.at(0) = mTempState.at(4)*theta/sTheta2; //rx + output.at(1) = mTempState.at(5)*theta/sTheta2; //ry + output.at(2) = mTempState.at(6)*theta/sTheta2; //rz } } diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index 7536d46..0594db0 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -101,6 +101,23 @@ class Filter3D { cv::Mat mTempState; ///< Temporary matrix to hold the state (x,y,z,qr,qi,qj,qk) + /** + * @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 output Double precision 3x1 matrix + */ + void getAngleAxis(cv::Mat& output); + /** * @brief Normalizes the quaternion part of the internal state vector */ From 2700bdb837161518f96eb420b1cd3189f125606b Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 11:59:10 +0100 Subject: [PATCH 10/22] Filter3D: Apply filter enable/disable to prediction as well --- src/Chilitags3D.cpp | 4 ++-- src/EstimatePose3D.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index 26d3133..5c62cf4 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -63,10 +63,10 @@ TagPoseMap estimate(TagCornerMap const& tags, cv::Vec const& camDeltaR { TagPoseMap objects; - //Pass the latest camera movement difference for prediction + //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 + //Predict pose for all known tags with camera movement (if 3D filtering is enabled) mEstimatePose3D(objects); //Correct pose prediction with new observations diff --git a/src/EstimatePose3D.cpp b/src/EstimatePose3D.cpp index 323a51d..d13d2a4 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -74,13 +74,15 @@ void EstimatePose3D::enableFilter(bool enabled) template void EstimatePose3D::setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) { - mFilter3D.setCamDelta(camDeltaR, camDeltaX); + if(mFilter3DEnabled) + mFilter3D.setCamDelta(camDeltaR, camDeltaX); } template void EstimatePose3D::operator()(typename Chilitags3D_::TagPoseMap& objects) { - mFilter3D(objects); + if(mFilter3DEnabled) + mFilter3D(objects); } template From 58430166dbd106d71adae24ee5ce0c4870e75d80 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 12:21:26 +0100 Subject: [PATCH 11/22] Filter3D: Refactored conversion to quaternion --- src/Filter3D.cpp | 48 ++++++++++++++++++++---------------------------- src/Filter3D.hpp | 8 ++++++++ 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index aec88b2..71af512 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -165,25 +165,12 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //Already existing filter else{ - RealT theta = norm(measuredRot); - RealT sTheta2 = sin(theta/2); //Fill state mTempState.at(0) = (RealT)measuredTrans.at(0); //x mTempState.at(1) = (RealT)measuredTrans.at(1); //y mTempState.at(2) = (RealT)measuredTrans.at(2); //z - - mTempState.at(3) = cos(theta/2); //qr - if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } - mTempState.at(4) = ((RealT)measuredRot.at(0)); //qi - mTempState.at(5) = ((RealT)measuredRot.at(1)); //qj - mTempState.at(6) = ((RealT)measuredRot.at(2)); //qk - } - else{ - mTempState.at(4) = ((RealT)measuredRot.at(0))/theta*sTheta2; //qi - mTempState.at(5) = ((RealT)measuredRot.at(1))/theta*sTheta2; //qj - mTempState.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk - } + getQuaternion(measuredRot, mTempState); //Do the correction step shortestPathQuat(prevQuat); @@ -214,20 +201,7 @@ void Filter3D::initFilter(cv::KalmanFilter& filter, cv::Vec& pre filter.statePost.at(0) = (RealT)measuredTrans.at(0); //x filter.statePost.at(1) = (RealT)measuredTrans.at(1); //y filter.statePost.at(2) = (RealT)measuredTrans.at(2); //z - - RealT theta = norm(measuredRot); - filter.statePost.at(3) = cos(theta/2); //qr - if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } - filter.statePost.at(4) = ((RealT)measuredRot.at(0)); //qi - filter.statePost.at(5) = ((RealT)measuredRot.at(1)); //qj - filter.statePost.at(6) = ((RealT)measuredRot.at(2)); //qk - } - else{ - RealT sTheta2 = sin(theta/2); - filter.statePost.at(4) = ((RealT)measuredRot.at(0))/theta*sTheta2; //qi - filter.statePost.at(5) = ((RealT)measuredRot.at(1))/theta*sTheta2; //qj - filter.statePost.at(6) = ((RealT)measuredRot.at(2))/theta*sTheta2; //qk - } + getQuaternion(measuredRot, filter.statePost); prevQuat(0) = filter.statePost.at(3); prevQuat(1) = filter.statePost.at(4); @@ -256,6 +230,24 @@ void Filter3D::getAngleAxis(cv::Mat& output) } } +template +void Filter3D::getQuaternion(cv::Mat& input, cv::Mat& output) +{ + RealT theta = norm(input); + output.at(3) = cos(theta/2); //qr + if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } + output.at(4) = ((RealT)input.at(0)); //qi + output.at(5) = ((RealT)input.at(1)); //qj + output.at(6) = ((RealT)input.at(2)); //qk + } + else{ + RealT sTheta2 = sin(theta/2); + output.at(4) = ((RealT)input.at(0))/theta*sTheta2; //qi + output.at(5) = ((RealT)input.at(1))/theta*sTheta2; //qj + output.at(6) = ((RealT)input.at(2))/theta*sTheta2; //qk + } +} + template inline RealT Filter3D::norm(cv::Mat& vec3d) { diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index 0594db0..b4034e9 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -118,6 +118,14 @@ class Filter3D { */ void getAngleAxis(cv::Mat& output); + /** + * @brief Converts the angle-axis to quaternion and writes it to indices (3,4,5,6) + * + * @param input Angle-axis + * @param output State to receive quaternion + */ + void getQuaternion(cv::Mat& input, cv::Mat& output); + /** * @brief Normalizes the quaternion part of the internal state vector */ From 98b95ed66781f9b461ab54fa022c61b2731b1c90 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 14:51:14 +0100 Subject: [PATCH 12/22] Filter3D: Array access refactoring --- src/Filter3D.cpp | 148 ++++++++++++++++++++++------------------------- src/Filter3D.hpp | 24 +++----- 2 files changed, 75 insertions(+), 97 deletions(-) diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index 71af512..35626d9 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -32,8 +32,6 @@ #include -//TODO: Replace all assignment sequences with memcpys (if possible) when double/float is unified - namespace chilitags{ template @@ -105,10 +103,9 @@ void Filter3D::setCamDelta(cv::Vec const& q, cv::Vec F6[3] = -q(3); F6[4] = q(2); F6[5] = -q(1); F6[6] = q(0); //Set the control matrix (other entries are zero) - //TODO: memcpy - B0[0] = F0[0]; B0[1] = F0[1]; B0[2] = F0[2]; - B1[0] = F1[0]; B1[1] = F1[1]; B1[2] = F1[2]; - B2[0] = F2[0]; B2[1] = F2[1]; B2[2] = F2[2]; + 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); @@ -132,7 +129,7 @@ void Filter3D::operator()(typename Chilitags3D_::TagPoseMap& tags) filter.predict(mControl).copyTo(mTempState); //Convert quaternion rotation to angle-axis rotation - getAngleAxis(predictedRot); + getAngleAxis((RealT*)mTempState.ptr() + 3, (double*)predictedRot.ptr()); //Convert angle-axis to 3x3 rotation matrix cv::Rodrigues(predictedRot, tempRotMat); @@ -165,12 +162,14 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, //Already existing filter else{ + RealT* state = (RealT*)mTempState.ptr(); + double* trans = (double*)measuredTrans.ptr(); //Fill state - mTempState.at(0) = (RealT)measuredTrans.at(0); //x - mTempState.at(1) = (RealT)measuredTrans.at(1); //y - mTempState.at(2) = (RealT)measuredTrans.at(2); //z - getQuaternion(measuredRot, mTempState); + 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); @@ -178,10 +177,10 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, normalizeQuat(); //Write state back - measuredTrans.at(0) = mTempState.at(0); //x - measuredTrans.at(1) = mTempState.at(1); //y - measuredTrans.at(2) = mTempState.at(2); //z - getAngleAxis(measuredRot); + trans[0] = state[0]; //x + trans[1] = state[1]; //y + trans[2] = state[2]; //z + getAngleAxis(state + 3, (double*)measuredRot.ptr()); } } @@ -198,107 +197,96 @@ void Filter3D::initFilter(cv::KalmanFilter& filter, cv::Vec& pre cv::setIdentity(filter.measurementMatrix); //Set initial state - filter.statePost.at(0) = (RealT)measuredTrans.at(0); //x - filter.statePost.at(1) = (RealT)measuredTrans.at(1); //y - filter.statePost.at(2) = (RealT)measuredTrans.at(2); //z - getQuaternion(measuredRot, filter.statePost); - - prevQuat(0) = filter.statePost.at(3); - prevQuat(1) = filter.statePost.at(4); - prevQuat(2) = filter.statePost.at(5); - prevQuat(3) = filter.statePost.at(6); + 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(cv::Mat& output) +void Filter3D::getAngleAxis(RealT* input, double* output) { - RealT theta = sqrt( - mTempState.at(4)*mTempState.at(4) + - mTempState.at(5)*mTempState.at(5) + - mTempState.at(6)*mTempState.at(6)); - theta = 2*atan2(theta, mTempState.at(3)); + 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.at(0) = mTempState.at(4); //rx - output.at(1) = mTempState.at(5); //ry - output.at(2) = mTempState.at(6); //rz + output[0] = input[1]; //rx + output[1] = input[2]; //ry + output[2] = input[3]; //rz } else{ - output.at(0) = mTempState.at(4)*theta/sTheta2; //rx - output.at(1) = mTempState.at(5)*theta/sTheta2; //ry - output.at(2) = mTempState.at(6)*theta/sTheta2; //rz + 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(cv::Mat& input, cv::Mat& output) +void Filter3D::getQuaternion(double* input, RealT* output) { - RealT theta = norm(input); - output.at(3) = cos(theta/2); //qr + RealT theta = (RealT)sqrt(input[0]*input[0] + input[1]*input[1] + input[2]*input[2]); + output[0] = cos(theta/2); //qr if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } - output.at(4) = ((RealT)input.at(0)); //qi - output.at(5) = ((RealT)input.at(1)); //qj - output.at(6) = ((RealT)input.at(2)); //qk + output[1] = (RealT)input[0]; //qi + output[2] = (RealT)input[1]; //qj + output[3] = (RealT)input[2]; //qk } else{ RealT sTheta2 = sin(theta/2); - output.at(4) = ((RealT)input.at(0))/theta*sTheta2; //qi - output.at(5) = ((RealT)input.at(1))/theta*sTheta2; //qj - output.at(6) = ((RealT)input.at(2))/theta*sTheta2; //qk + output[1] = (RealT)input[0]/theta*sTheta2; //qi + output[2] = (RealT)input[1]/theta*sTheta2; //qj + output[3] = (RealT)input[2]/theta*sTheta2; //qk } } -template -inline RealT Filter3D::norm(cv::Mat& vec3d) -{ - return sqrt((RealT)( - vec3d.at(0)*vec3d.at(0) + - vec3d.at(1)*vec3d.at(1) + - vec3d.at(2)*vec3d.at(2))); -} - template inline void Filter3D::normalizeQuat() { - RealT norm = sqrt( - mTempState.at(3)*mTempState.at(3) + - mTempState.at(4)*mTempState.at(4) + - mTempState.at(5)*mTempState.at(5) + - mTempState.at(6)*mTempState.at(6)); - + 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){ - mTempState.at(3) /= norm; - mTempState.at(4) /= norm; - mTempState.at(5) /= norm; - mTempState.at(6) /= norm; + quat[0] /= norm; + quat[1] /= norm; + quat[2] /= norm; + quat[3] /= norm; } else{ - mTempState.at(3) = 1.0f; - mTempState.at(4) = 0.0f; - mTempState.at(5) = 0.0f; - mTempState.at(6) = 0.0f; + 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( - mTempState.at(3)*prevQuat(0) + - mTempState.at(4)*prevQuat(1) + - mTempState.at(5)*prevQuat(2) + - mTempState.at(6)*prevQuat(3) < 0){ - mTempState.at(3) = -mTempState.at(3); - mTempState.at(4) = -mTempState.at(4); - mTempState.at(5) = -mTempState.at(5); - mTempState.at(6) = -mTempState.at(6); + 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) = mTempState.at(3); - prevQuat(1) = mTempState.at(4); - prevQuat(2) = mTempState.at(5); - prevQuat(3) = mTempState.at(6); + prevQuat(0) = quat[0]; + prevQuat(1) = quat[1]; + prevQuat(2) = quat[2]; + prevQuat(3) = quat[3]; } //All possible instantiations of Filter3D diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index b4034e9..a2b2a35 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -114,17 +114,18 @@ class Filter3D { /** * @brief Converts the quaternion rotation in the state to angle-axis representation * - * @param output Double precision 3x1 matrix + * @param input 4x1 quaternion + * @param output Double precision 3x1 angle-axis */ - void getAngleAxis(cv::Mat& output); + void getAngleAxis(RealT* input, double* output); /** - * @brief Converts the angle-axis to quaternion and writes it to indices (3,4,5,6) + * @brief Converts the angle-axis to quaternion * - * @param input Angle-axis - * @param output State to receive quaternion + * @param input Double precision 3x1 angle-axis + * @param output 4x1 quaternion */ - void getQuaternion(cv::Mat& input, cv::Mat& output); + void getQuaternion(double* input, RealT* output); /** * @brief Normalizes the quaternion part of the internal state vector @@ -136,17 +137,6 @@ class Filter3D { */ void shortestPathQuat(cv::Vec& prevQuat); - /** - * @brief Calculates the norm of the given 3-vector - * - * TODO: vec3d must be double for now - * - * @param vec3d 3-vector of doubles - * - * @return The calculated norm in single-precision - */ - RealT norm(cv::Mat& vec3d); - }; } /* namespace chilitags */ From 40dd0ad077c6856c161a24bb192c781ef62f968b Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 16:22:24 +0100 Subject: [PATCH 13/22] Removed Filter --- include/chilitags.hpp | 43 +----- samples/detection/detect-live.cpp | 8 - samples/multithreaded/async-detection.cpp | 1 - samples/tracking/tracking.cpp | 2 - src/Chilitags.cpp | 28 +--- src/Chilitags3D.cpp | 17 +- src/Filter.cpp | 132 ---------------- src/Filter.hpp | 82 ---------- test/CMakeLists.txt | 1 - test/Filter.cpp | 179 ---------------------- test/detection-performance.cpp | 2 - test/float-precision.cpp | 4 +- 12 files changed, 10 insertions(+), 489 deletions(-) delete mode 100644 src/Filter.cpp delete mode 100644 src/Filter.hpp delete mode 100644 test/Filter.cpp diff --git a/include/chilitags.hpp b/include/chilitags.hpp index 2acd679..3c6854a 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -61,27 +61,12 @@ class Chilitags /** Constructs a object ready to detect tags. - It sets a default persistence of 5 frames for the tags, and a gain of 0. - Please refer to the documentation of setFilter() for more detail. The default detection performance is balanced between accuracy and processing time (see Chilitags::FAST); it can be changed with setPerformance(). */ Chilitags(); -/** - Parameters to paliate with the imperfections of the detection. - - \param persistence the number of frames in which a tag should be absent - before being removed from the output of find(). 0 means that tags disappear - directly if they are not detected. - - \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 tag is returned. - */ -void setFilter(int persistence, float gain); - /** Values of the parameter to tell find() how to combine tracking and full detection. @@ -362,11 +347,7 @@ typedef std::map TagPoseMap; object, and how big they are. 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. + instance, which can be accessed through the getChilitags() accessors. You can also create yourself a separate instance of Chilitagsfor the 2D detection of tags and use it by calling @@ -384,28 +365,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; diff --git a/samples/detection/detect-live.cpp b/samples/detection/detect-live.cpp index 52be44c..85cbfc5 100644 --- a/samples/detection/detect-live.cpp +++ b/samples/detection/detect-live.cpp @@ -71,14 +71,6 @@ int main(int argc, char* argv[]) // The tag detection happens in the Chilitags class. chilitags::Chilitags chilitags; - // The detection is not perfect, so if a tag is not detected during one frame, - // the tag will shortly disappears, which results in flickering. - // To address this, Chilitags "cheats" by keeping tags for n frames - // at the same position. When tags disappear for more than 5 frames, - // Chilitags actually removes it. - // Here, we cancel this to show the raw detection results. - chilitags.setFilter(0, 0.0f); - cv::namedWindow("DisplayChilitags"); // Main loop, exiting when 'q is pressed' for (; 'q' != (char) cv::waitKey(1); ) { diff --git a/samples/multithreaded/async-detection.cpp b/samples/multithreaded/async-detection.cpp index 3174e72..1fc8d3f 100644 --- a/samples/multithreaded/async-detection.cpp +++ b/samples/multithreaded/async-detection.cpp @@ -66,7 +66,6 @@ int main(int argc, char* argv[]) cv::Mat inputImage; chilitags::Chilitags chilitags; - chilitags.setFilter(0, 0.); cv::namedWindow("DisplayChilitags"); diff --git a/samples/tracking/tracking.cpp b/samples/tracking/tracking.cpp index a031738..ddfb935 100644 --- a/samples/tracking/tracking.cpp +++ b/samples/tracking/tracking.cpp @@ -75,12 +75,10 @@ int main(int argc, char* argv[]) // This one is the reference Chilitags chilitags::Chilitags detectedChilitags; - detectedChilitags.setFilter(0, 0.0f); // This one will be called with JUST_TRACK when it has previously detected // something chilitags::Chilitags trackedChilitags; - trackedChilitags.setFilter(0, 0.0f); cv::namedWindow("DisplayChilitags"); diff --git a/src/Chilitags.cpp b/src/Chilitags.cpp index 6bb38e9..8eb0160 100644 --- a/src/Chilitags.cpp +++ b/src/Chilitags.cpp @@ -21,7 +21,6 @@ #include #include "EnsureGreyscale.hpp" -#include "Filter.hpp" #include "Detect.hpp" #include "Track.hpp" @@ -56,21 +55,15 @@ Impl() : mEnsureGreyscale(), mDecode(), - mFilter(5, 0.f), mDetect(), mTrack(), - mCallsBeforeDetection(15), - mCallsBeforeNextDetection(0) + mCallsBeforeNextDetection(0), + mCallsBeforeDetection(15) { setPerformance(FAST); } -void setFilter(int persistence, float gain) { - mFilter.setPersistence(persistence); - mFilter.setGain(gain); -} - void setPerformance(PerformancePreset preset) { switch (preset) { case FASTER: @@ -155,7 +148,7 @@ TagCornerMap find( case DETECT_ONLY: mDetect(mResizedGrayscaleInput, tags); - return scaleBy(mFilter(tags), scaleFactor); + return scaleBy(tags, scaleFactor); case TRACK_ONLY: return scaleBy(mTrack(mResizedGrayscaleInput), scaleFactor); @@ -166,7 +159,7 @@ TagCornerMap find( tags = mTrack(mResizedGrayscaleInput); mDetect(mResizedGrayscaleInput, tags); mTrack.update(tags); - return scaleBy(mFilter(tags), scaleFactor); + return scaleBy(tags, scaleFactor); case DETECT_PERIODICALLY: mCallsBeforeNextDetection--; @@ -181,7 +174,7 @@ TagCornerMap find( tags = mTrack(mResizedGrayscaleInput); mDetect(mResizedGrayscaleInput, tags); mTrack.update(tags); - return scaleBy(mFilter(tags), scaleFactor); + return scaleBy(tags, scaleFactor); } #ifdef HAS_MULTITHREADING @@ -260,8 +253,6 @@ EnsureGreyscale mEnsureGreyscale; Decode mDecode; -Filter mFilter; - Detect mDetect; Track mTrack; @@ -276,10 +267,6 @@ Chilitags::Chilitags() : { } -void Chilitags::setFilter(int persistence, float gain) { - mImpl->setFilter(persistence, gain); -} - void Chilitags::setPerformance(PerformancePreset preset) { mImpl->setPerformance(preset); } @@ -296,10 +283,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 5c62cf4..52ac6ee 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -20,7 +20,6 @@ #include -#include "Filter.hpp" #include "EstimatePose3D.hpp" #include @@ -40,16 +39,9 @@ Impl(cv::Size cameraResolution) : mEstimatePose3D(cameraResolution), mOmitOtherTags(false), mDefaultTagCorners(), - mId2Configuration(), - 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 { @@ -298,15 +290,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)){ diff --git a/src/Filter.cpp b/src/Filter.cpp deleted file mode 100644 index f6ec4a8..0000000 --- a/src/Filter.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/******************************************************************************* -* 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 "Filter.hpp" - -#include - -namespace chilitags { - -template -FindOutdated::FindOutdated(int persistence) : - mPersistence(persistence), - mDisappearanceTime() -{ -} - -template -template -std::vector FindOutdated::operator()(const std::map &tags){ - - std::vector tagsToForget; - - auto tagIt = tags.cbegin(); - auto ageIt = mDisappearanceTime.begin(); - - // for each tags that are detected in the current frame - while (tagIt != tags.end()) { - - // update all the tags that come before the current tag, - // i.e. that haven't bee detected this time - while (ageIt != mDisappearanceTime.end() - && ageIt->first < tagIt->first) { - - if (ageIt->second >= mPersistence) { - // remove the tags that haven't been seen for too long - tagsToForget.push_back(ageIt->first); - ageIt = mDisappearanceTime.erase(ageIt); - } else { - // mark as older the last update of the others - ++(ageIt->second); - ++ageIt; - } - } - - ageIt = mDisappearanceTime.insert(ageIt, std::make_pair(tagIt->first, 0)); - - ++tagIt; - ++ageIt; - } - - // update the remaining tags that have not been detected in this frame either - while (ageIt != mDisappearanceTime.end()) { - - if (ageIt->second >= mPersistence) { - // remove the tags that haven't been seen for too long - tagsToForget.push_back(ageIt->first); - ageIt = mDisappearanceTime.erase(ageIt); - } else { - // mark as older the last update of the others - ++ageIt->second; - ++ageIt; - } - } - - - return tagsToForget; -} - -template -Filter::Filter(int persistence, float gain): -mFindOutdated(persistence), -mGain(gain), -mFilteredCoordinates() -{} - -template -const std::map & Filter::operator()( - const std::map &tags) { - - for(const auto &tagToForget : mFindOutdated(tags)) { - //TODO lookup can be avoided if Ids are sorted - mFilteredCoordinates.erase(tagToForget); - } - - const float gainComplement = 1.0f - mGain; - - auto filteredIt = mFilteredCoordinates.begin(); - for (const auto &tag : tags) { - while (filteredIt != mFilteredCoordinates.end() - && filteredIt->first < tag.first) { - ++filteredIt; - } - - if (filteredIt != mFilteredCoordinates.end() - && filteredIt->first == tag.first) { - cv::addWeighted(filteredIt->second, mGain, - tag.second, gainComplement, - 0.0f, filteredIt->second); - } - else { - filteredIt = mFilteredCoordinates.insert(filteredIt, tag); - } - } - - 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 deleted file mode 100644 index ad46c72..0000000 --- a/src/Filter.hpp +++ /dev/null @@ -1,82 +0,0 @@ -/******************************************************************************* -* 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 . * -*******************************************************************************/ - -/** This header contains various utilities to paliate with imperfect detection. - */ - -#ifndef Filters_HPP -#define Filters_HPP - -#include -#include - -#include - -namespace chilitags { - -template -class FindOutdated { - -public: - -FindOutdated(int persistence); - -void setPersistence(int persistence) { - mPersistence = persistence; -} - -template -std::vector operator()(const std::map &tags); - -protected: - -int mPersistence; -std::map mDisappearanceTime; - -}; - - - -template -class Filter { -public: - Filter(int persistence, float gain); - - void setPersistence(int persistence) { - mFindOutdated.setPersistence(persistence); - } - - void setGain(float gain) { - mGain = gain; - } - - const std::map & operator()( - const std::map &tags) ; - -protected: - FindOutdated mFindOutdated; - float mGain; - std::map mFilteredCoordinates; -}; - - -} - -#endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 59215ae..22f87dd 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -49,7 +49,6 @@ include_directories(../src) declare_test(TESTNAME drawer) declare_test(TESTNAME codec) -declare_test(TESTNAME Filter) declare_test(TESTNAME integration) declare_test(TESTNAME pose-estimation NEEDS_DATA) declare_test(TESTNAME detection-performance NEEDS_DATA) diff --git a/test/Filter.cpp b/test/Filter.cpp deleted file mode 100644 index 3cacdc9..0000000 --- a/test/Filter.cpp +++ /dev/null @@ -1,179 +0,0 @@ -/******************************************************************************* -* 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 . * -*******************************************************************************/ - -#ifdef OPENCV3 -#include -#else -#include -#endif - -#include -#include - -#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); - - 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); - - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(1, findOutdated(EMPTY_TAG_LIST).size()); -} - -TEST(FindOutdated2D, InvalidateFirst) { - FindOutdated2D findOutdated(2); - - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(1, findOutdated(ONLY_TAG_43).size()); -} - -TEST(FindOutdated2D, ChangePersistence) { - FindOutdated2D findOutdated(2); - - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); - findOutdated.setPersistence(1); - EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); - 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::Quad coordinates { - 1.0f,2.0f, - 3.0f,4.0f, - 5.0f,6.0f, - 7.0f,8.0f, - }; - chilitags::Quad expected; - chilitags::TagCornerMap tags; - chilitags::TagCornerMap results; - - tags = {{0, coordinates}}; - results = filter(tags); - expected = coordinates; - EXPECT_EQ(results.size(), tags.size()); - EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); - - coordinates = cv::Mat(cv::Mat(coordinates) + 9.0f); - tags = {{0, coordinates}}; - results = filter(tags); - expected = coordinates; - EXPECT_EQ(results.size(), tags.size()); - EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); -} - -TEST(Filter, NonZeroGain) { - chilitags::Filter filter(0, 0.1f); - chilitags::Quad coordinates { - 1.0f,2.0f, - 3.0f,4.0f, - 5.0f,6.0f, - 7.0f,8.0f, - }; - chilitags::Quad expected; - chilitags::TagCornerMap tags; - chilitags::TagCornerMap results; - - tags = {{0, coordinates}}; - results = filter(tags); - expected = coordinates; - EXPECT_EQ(results.size(), tags.size()); - EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); - - chilitags::Quad coordinates2 = cv::Mat(cv::Mat(coordinates) + 9.0f); - tags = {{0, coordinates2}}; - results = filter(tags); - expected = cv::Mat(0.1f*cv::Mat(coordinates)+0.9f*cv::Mat(coordinates2)); - EXPECT_EQ(results.size(), tags.size()); - EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); -} - -CV_TEST_MAIN(".") diff --git a/test/detection-performance.cpp b/test/detection-performance.cpp index a25b08e..86e8069 100644 --- a/test/detection-performance.cpp +++ b/test/detection-performance.cpp @@ -93,8 +93,6 @@ TEST(Integration, Snapshots) { chilitags::Chilitags chilitags; // We measure chilitags at its best, and compare optimisations later chilitags.setPerformance(chilitags::Chilitags::ROBUST); - // We do not want any filtering, to measure the raw performances - chilitags.setFilter(0, 0.0f); map resolution; diff --git a/test/float-precision.cpp b/test/float-precision.cpp index ad817c8..8f3c088 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.enableFilter3D(false); chilitags::Chilitags3Dd chilitagsd; chilitagsd.getChilitags().setPerformance(chilitags::Chilitags::ROBUST); - chilitagsd.setFilter(0, 0.0f); + chilitagsd.enableFilter3D(false); for (auto testCase : TestMetadata::all) { std::string path = std::string(cvtest::TS::ptr()->get_data_path())+testCase.filename; From 6a45a5dce27814c93643e22a548f4dabff19ed95 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Thu, 11 Dec 2014 16:27:15 +0100 Subject: [PATCH 14/22] Filter3D: Do not install sample --- samples/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 5eb8621..c696e9b 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -51,5 +51,4 @@ endif() add_executable( filter3d-gui 3dfiltering/filter3d-gui.cpp) target_link_libraries( filter3d-gui chilitags ) target_link_libraries( filter3d-gui ${OpenCV_LIBS} ) -install (TARGETS filter3d-gui RUNTIME DESTINATION bin) From f06995c8aa8639552379788146c91a5b96190a5b Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Fri, 12 Dec 2014 11:42:41 +0100 Subject: [PATCH 15/22] Filter3D: Persistence --- include/chilitags.hpp | 7 +++++++ src/Chilitags3D.cpp | 9 +++++++++ src/EstimatePose3D.cpp | 6 ++++++ src/EstimatePose3D.hpp | 7 +++++++ src/Filter3D.cpp | 33 +++++++++++++++++++++++++++++++-- src/Filter3D.hpp | 16 ++++++++++++++-- 6 files changed, 74 insertions(+), 4 deletions(-) diff --git a/include/chilitags.hpp b/include/chilitags.hpp index 3c6854a..f7de041 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -480,6 +480,13 @@ void setDefaultTagSize(RealT defaultSize); */ void enableFilter3D(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 setPersistence(RealT persistence); + /** For accurate results, Chilitags3D can be provided the calibration data of the camera detecting the chilitags. See diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index 52ac6ee..fed5145 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -143,6 +143,10 @@ void enableFilter3D(bool enabled){ mEstimatePose3D.enableFilter(enabled); } +void setPersistence(RealT persistence){ + mEstimatePose3D.setFilterPersistence(persistence); +} + bool read3DConfiguration(const std::string &filenameOrString, bool omitOtherTags, bool readFromString) { mOmitOtherTags = omitOtherTags; @@ -334,6 +338,11 @@ void Chilitags3D_::enableFilter3D(bool enabled){ mImpl->enableFilter3D(enabled); } +template +void Chilitags3D_::setPersistence(RealT persistence){ + mImpl->setPersistence(persistence); +} + 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 d13d2a4..1917ceb 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -71,6 +71,12 @@ void EstimatePose3D::enableFilter(bool enabled) mFilter3DEnabled = enabled; } +template +void EstimatePose3D::setFilterPersistence(RealT persistence) +{ + mFilter3D.setPersistence(persistence); +} + template void EstimatePose3D::setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX) { diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index e1e45d0..0951016 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -81,6 +81,13 @@ class EstimatePose3D */ 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 Informs the rotation and translation of the current camera frame in the previous camera frame * diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index 35626d9..afe2985 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -43,6 +43,8 @@ Filter3D::Filter3D() : mControl(3, 1, CV_TYPE), mQ(), mR(), + mCovScales(7, 1, CV_TYPE), + mPersistence(10.0f), mTempState(7, 1, CV_TYPE) { @@ -66,6 +68,10 @@ Filter3D::Filter3D() : 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 + for(int i=0;i<7;i++) + mCovScales.at(i) = sqrt(mQ.at(i,i)*mR.at(i,i)); + //Process matrix is 7x7 and is identity as long as there is no camera movement info cv::setIdentity(mF); @@ -76,6 +82,12 @@ Filter3D::Filter3D() : mControl = cv::Mat::zeros(3, 1, CV_TYPE); } +template +void Filter3D::setPersistence(RealT persistence) +{ + mPersistence = persistence; +} + template void Filter3D::setCamDelta(cv::Vec const& q, cv::Vec const& camDeltaX) { @@ -121,8 +133,21 @@ void Filter3D::operator()(typename Chilitags3D_::TagPoseMap& tags) 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 mF.copyTo(filter.transitionMatrix); mB.copyTo(filter.controlMatrix); @@ -155,10 +180,13 @@ void Filter3D::operator()(std::string const& id, cv::Mat& measuredTrans, 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 - if(pair.second) + //Newly inserted or lazy-deleted + if(pair.second || deleted){ + deleted = false; initFilter(filter, prevQuat, measuredTrans, measuredRot); + } //Already existing filter else{ @@ -189,6 +217,7 @@ void Filter3D::initFilter(cv::KalmanFilter& filter, cv::Vec& pre { 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); diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index a2b2a35..01a59a0 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -46,6 +46,13 @@ class Filter3D { */ 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 Informs the rotation and translation of the current camera frame in the previous camera frame * @@ -78,15 +85,17 @@ class Filter3D { const RealT EPSILON; ///< One of FLT_EPSILON, DBL_EPSILON depending on the template type /** - * @brief Describes a Kalman filter and an associated previous quaternion rotation state + * @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() + prevQuat(), + deleted(false) {} }; @@ -99,6 +108,9 @@ class Filter3D { cv::Mat mQ; ///< Process noise covariance matrix, to be tuned cv::Mat mR; ///< Measurement noise covariance matrix, to be tuned + 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) /** From 5127b4d9fede2e4736a92c4b84ff0f396d7e3264 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Fri, 12 Dec 2014 14:14:28 +0100 Subject: [PATCH 16/22] Filter3D: API for setting covariance matrices --- include/chilitags.hpp | 35 +++++++++++++++++- samples/3dfiltering/filter3d-gui.cpp | 55 +++++++++++++++++++++++++--- src/Chilitags3D.cpp | 24 ++++++++++-- src/EstimatePose3D.cpp | 12 ++++++ src/EstimatePose3D.hpp | 14 +++++++ src/Filter3D.cpp | 35 +++++++++++++++++- src/Filter3D.hpp | 21 +++++++++++ test/float-precision.cpp | 4 +- 8 files changed, 187 insertions(+), 13 deletions(-) diff --git a/include/chilitags.hpp b/include/chilitags.hpp index f7de041..6c16957 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -478,7 +478,7 @@ void setDefaultTagSize(RealT defaultSize); * * @param enabled Whether to enable Kalman filtering */ -void enableFilter3D(bool enabled); +void enableFilter(bool enabled); /** * @brief Sets the persistence of tags against being discarded when not observed @@ -487,6 +487,39 @@ void enableFilter3D(bool enabled); */ 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. + * + * @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. + * + * @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 index da63d8c..4b436f6 100644 --- a/samples/3dfiltering/filter3d-gui.cpp +++ b/samples/3dfiltering/filter3d-gui.cpp @@ -96,20 +96,65 @@ int main(int argc, char* argv[]) 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))) { - if(keyPressed == 'f'){ - filterEnabled = !filterEnabled; - chilitags3D.enableFilter3D(filterEnabled); + 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::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; @@ -149,7 +194,7 @@ int main(int argc, char* argv[]) #ifdef OPENCV3 2, cv::LINE_AA, SHIFT); #else - 2, CV_AA, SHIFT); + 2, CV_AA, SHIFT); #endif } } diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index fed5145..a964a6d 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -139,7 +139,7 @@ void setDefaultTagSize(RealT defaultSize){ }; } -void enableFilter3D(bool enabled){ +void enableFilter(bool enabled){ mEstimatePose3D.enableFilter(enabled); } @@ -147,6 +147,14 @@ 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; @@ -334,8 +342,8 @@ void Chilitags3D_::setDefaultTagSize(RealT defaultSize){ } template -void Chilitags3D_::enableFilter3D(bool enabled){ - mImpl->enableFilter3D(enabled); +void Chilitags3D_::enableFilter(bool enabled){ + mImpl->enableFilter(enabled); } template @@ -343,6 +351,16 @@ 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 1917ceb..e977e02 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -77,6 +77,18 @@ 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) { diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index 0951016..d3507d8 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -88,6 +88,20 @@ class EstimatePose3D */ 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 * diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index afe2985..0c34893 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -42,7 +42,9 @@ Filter3D::Filter3D() : 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) @@ -69,8 +71,7 @@ Filter3D::Filter3D() : 0, 0, 0, 0, 0, 0, 1e-5f); //Scale coefficients when calculating the trace of the covariance estimate - for(int i=0;i<7;i++) - mCovScales.at(i) = sqrt(mQ.at(i,i)*mR.at(i,i)); + recalculateCovScales(); //Process matrix is 7x7 and is identity as long as there is no camera movement info cv::setIdentity(mF); @@ -82,12 +83,35 @@ Filter3D::Filter3D() : 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) { @@ -149,6 +173,10 @@ void Filter3D::operator()(typename Chilitags3D_::TagPoseMap& tags) } //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); @@ -167,6 +195,9 @@ void Filter3D::operator()(typename Chilitags3D_::TagPoseMap& tags) 0, 0, 0, 1 }; } + + mQChanged = false; + mRChanged = false; } template diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index 01a59a0..e6615f5 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -53,6 +53,20 @@ class Filter3D { */ 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 * @@ -106,13 +120,20 @@ class Filter3D { 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 * diff --git a/test/float-precision.cpp b/test/float-precision.cpp index 8f3c088..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.enableFilter3D(false); + chilitagsf.enableFilter(false); chilitags::Chilitags3Dd chilitagsd; chilitagsd.getChilitags().setPerformance(chilitags::Chilitags::ROBUST); - chilitagsd.enableFilter3D(false); + chilitagsd.enableFilter(false); for (auto testCase : TestMetadata::all) { std::string path = std::string(cvtest::TS::ptr()->get_data_path())+testCase.filename; From 1744fd1fdf9fbeba4dd01ee26049d06293b04dfa Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Sat, 13 Dec 2014 12:45:37 +0100 Subject: [PATCH 17/22] Filter3D: Kalman filter documentation --- src/Filter3D.cpp | 20 +++++----- src/Filter3D.hpp | 100 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 110 insertions(+), 10 deletions(-) diff --git a/src/Filter3D.cpp b/src/Filter3D.cpp index 0c34893..9cab79b 100644 --- a/src/Filter3D.cpp +++ b/src/Filter3D.cpp @@ -50,7 +50,7 @@ Filter3D::Filter3D() : mTempState(7, 1, CV_TYPE) { - //Process noise covariance is 7x7: cov((x,y,z,qw,qi,qj,qk)) + //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, @@ -60,7 +60,7 @@ Filter3D::Filter3D() : 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,qi,qj,qk)) + //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, @@ -253,7 +253,7 @@ void Filter3D::initFilter(cv::KalmanFilter& filter, cv::Vec& pre //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,qr,qi,qj,qk) + //We make the same measurement as the state: (x,y,z,qw,qx,qy,qz) cv::setIdentity(filter.measurementMatrix); //Set initial state @@ -293,17 +293,17 @@ 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); //qr + output[0] = cos(theta/2); //qw if(theta < EPSILON){ //Use lim( theta -> 0 ){ sin(theta)/theta } - output[1] = (RealT)input[0]; //qi - output[2] = (RealT)input[1]; //qj - output[3] = (RealT)input[2]; //qk + 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; //qi - output[2] = (RealT)input[1]/theta*sTheta2; //qj - output[3] = (RealT)input[2]/theta*sTheta2; //qk + output[1] = (RealT)input[0]/theta*sTheta2; //qx + output[2] = (RealT)input[1]/theta*sTheta2; //qy + output[3] = (RealT)input[2]/theta*sTheta2; //qz } } diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index e6615f5..e1416d8 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -22,6 +22,106 @@ * @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 From d4d739106f4601ea2010ffd7ab9bfcb308ce9ef6 Mon Sep 17 00:00:00 2001 From: Quentin Bonnard Date: Sat, 20 Dec 2014 14:23:02 +0100 Subject: [PATCH 18/22] unremove Filter for 2D detection --- include/chilitags.hpp | 19 ++- samples/detection/detect-live.cpp | 8 + samples/multithreaded/async-detection.cpp | 1 + samples/tracking/tracking.cpp | 2 + src/Chilitags.cpp | 19 ++- src/Chilitags3D.cpp | 1 + src/Filter.cpp | 132 ++++++++++++++++ src/Filter.hpp | 82 ++++++++++ test/CMakeLists.txt | 1 + test/Filter.cpp | 179 ++++++++++++++++++++++ test/detection-performance.cpp | 2 + 11 files changed, 442 insertions(+), 4 deletions(-) create mode 100644 src/Filter.cpp create mode 100644 src/Filter.hpp create mode 100644 test/Filter.cpp diff --git a/include/chilitags.hpp b/include/chilitags.hpp index 6c16957..bac16be 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -61,12 +61,27 @@ class Chilitags /** Constructs a object ready to detect tags. + It sets a default persistence of 5 frames for the tags, and a gain of 0. + Please refer to the documentation of setFilter() for more detail. The default detection performance is balanced between accuracy and processing time (see Chilitags::FAST); it can be changed with setPerformance(). */ Chilitags(); +/** + Parameters to paliate with the imperfections of the detection. + + \param persistence the number of frames in which a tag should be absent + before being removed from the output of find(). 0 means that tags disappear + directly if they are not detected. + + \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 tag is returned. + */ +void setFilter(int persistence, float gain); + /** Values of the parameter to tell find() how to combine tracking and full detection. @@ -347,7 +362,9 @@ typedef std::map TagPoseMap; object, and how big they are. To first detect th tags in the image, Chilitags3D creates a Chilitags - instance, which can be accessed through the getChilitags() accessors. + instance, which can be accessed through the getChilitags() accessors. This + Chilitags instance is set to have a persistence of 0, because Chilitags3D + uses a more advanced Kalman filter. You can also create yourself a separate instance of Chilitagsfor the 2D detection of tags and use it by calling diff --git a/samples/detection/detect-live.cpp b/samples/detection/detect-live.cpp index 85cbfc5..52be44c 100644 --- a/samples/detection/detect-live.cpp +++ b/samples/detection/detect-live.cpp @@ -71,6 +71,14 @@ int main(int argc, char* argv[]) // The tag detection happens in the Chilitags class. chilitags::Chilitags chilitags; + // The detection is not perfect, so if a tag is not detected during one frame, + // the tag will shortly disappears, which results in flickering. + // To address this, Chilitags "cheats" by keeping tags for n frames + // at the same position. When tags disappear for more than 5 frames, + // Chilitags actually removes it. + // Here, we cancel this to show the raw detection results. + chilitags.setFilter(0, 0.0f); + cv::namedWindow("DisplayChilitags"); // Main loop, exiting when 'q is pressed' for (; 'q' != (char) cv::waitKey(1); ) { diff --git a/samples/multithreaded/async-detection.cpp b/samples/multithreaded/async-detection.cpp index 1fc8d3f..3174e72 100644 --- a/samples/multithreaded/async-detection.cpp +++ b/samples/multithreaded/async-detection.cpp @@ -66,6 +66,7 @@ int main(int argc, char* argv[]) cv::Mat inputImage; chilitags::Chilitags chilitags; + chilitags.setFilter(0, 0.); cv::namedWindow("DisplayChilitags"); diff --git a/samples/tracking/tracking.cpp b/samples/tracking/tracking.cpp index ddfb935..a031738 100644 --- a/samples/tracking/tracking.cpp +++ b/samples/tracking/tracking.cpp @@ -75,10 +75,12 @@ int main(int argc, char* argv[]) // This one is the reference Chilitags chilitags::Chilitags detectedChilitags; + detectedChilitags.setFilter(0, 0.0f); // This one will be called with JUST_TRACK when it has previously detected // something chilitags::Chilitags trackedChilitags; + trackedChilitags.setFilter(0, 0.0f); cv::namedWindow("DisplayChilitags"); diff --git a/src/Chilitags.cpp b/src/Chilitags.cpp index 8eb0160..019ad07 100644 --- a/src/Chilitags.cpp +++ b/src/Chilitags.cpp @@ -21,6 +21,7 @@ #include #include "EnsureGreyscale.hpp" +#include "Filter.hpp" #include "Detect.hpp" #include "Track.hpp" @@ -55,6 +56,7 @@ Impl() : mEnsureGreyscale(), mDecode(), + mFilter(5, 0.f), mDetect(), mTrack(), @@ -64,6 +66,11 @@ Impl() : setPerformance(FAST); } +void setFilter(int persistence, float gain) { + mFilter.setPersistence(persistence); + mFilter.setGain(gain); +} + void setPerformance(PerformancePreset preset) { switch (preset) { case FASTER: @@ -148,7 +155,7 @@ TagCornerMap find( case DETECT_ONLY: mDetect(mResizedGrayscaleInput, tags); - return scaleBy(tags, scaleFactor); + return scaleBy(mFilter(tags), scaleFactor); case TRACK_ONLY: return scaleBy(mTrack(mResizedGrayscaleInput), scaleFactor); @@ -159,7 +166,7 @@ TagCornerMap find( tags = mTrack(mResizedGrayscaleInput); mDetect(mResizedGrayscaleInput, tags); mTrack.update(tags); - return scaleBy(tags, scaleFactor); + return scaleBy(mFilter(tags), scaleFactor); case DETECT_PERIODICALLY: mCallsBeforeNextDetection--; @@ -174,7 +181,7 @@ TagCornerMap find( tags = mTrack(mResizedGrayscaleInput); mDetect(mResizedGrayscaleInput, tags); mTrack.update(tags); - return scaleBy(tags, scaleFactor); + return scaleBy(mFilter(tags), scaleFactor); } #ifdef HAS_MULTITHREADING @@ -253,6 +260,8 @@ EnsureGreyscale mEnsureGreyscale; Decode mDecode; +Filter mFilter; + Detect mDetect; Track mTrack; @@ -267,6 +276,10 @@ Chilitags::Chilitags() : { } +void Chilitags::setFilter(int persistence, float gain) { + mImpl->setFilter(persistence, gain); +} + void Chilitags::setPerformance(PerformancePreset preset) { mImpl->setPerformance(preset); } diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index a964a6d..c5d7b9b 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -42,6 +42,7 @@ Impl(cv::Size cameraResolution) : mId2Configuration() { setDefaultTagSize(20.f); + mChilitags.setFilter(0, 0.0f); } const Chilitags &getChilitags() const { diff --git a/src/Filter.cpp b/src/Filter.cpp new file mode 100644 index 0000000..f6ec4a8 --- /dev/null +++ b/src/Filter.cpp @@ -0,0 +1,132 @@ +/******************************************************************************* +* 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 "Filter.hpp" + +#include + +namespace chilitags { + +template +FindOutdated::FindOutdated(int persistence) : + mPersistence(persistence), + mDisappearanceTime() +{ +} + +template +template +std::vector FindOutdated::operator()(const std::map &tags){ + + std::vector tagsToForget; + + auto tagIt = tags.cbegin(); + auto ageIt = mDisappearanceTime.begin(); + + // for each tags that are detected in the current frame + while (tagIt != tags.end()) { + + // update all the tags that come before the current tag, + // i.e. that haven't bee detected this time + while (ageIt != mDisappearanceTime.end() + && ageIt->first < tagIt->first) { + + if (ageIt->second >= mPersistence) { + // remove the tags that haven't been seen for too long + tagsToForget.push_back(ageIt->first); + ageIt = mDisappearanceTime.erase(ageIt); + } else { + // mark as older the last update of the others + ++(ageIt->second); + ++ageIt; + } + } + + ageIt = mDisappearanceTime.insert(ageIt, std::make_pair(tagIt->first, 0)); + + ++tagIt; + ++ageIt; + } + + // update the remaining tags that have not been detected in this frame either + while (ageIt != mDisappearanceTime.end()) { + + if (ageIt->second >= mPersistence) { + // remove the tags that haven't been seen for too long + tagsToForget.push_back(ageIt->first); + ageIt = mDisappearanceTime.erase(ageIt); + } else { + // mark as older the last update of the others + ++ageIt->second; + ++ageIt; + } + } + + + return tagsToForget; +} + +template +Filter::Filter(int persistence, float gain): +mFindOutdated(persistence), +mGain(gain), +mFilteredCoordinates() +{} + +template +const std::map & Filter::operator()( + const std::map &tags) { + + for(const auto &tagToForget : mFindOutdated(tags)) { + //TODO lookup can be avoided if Ids are sorted + mFilteredCoordinates.erase(tagToForget); + } + + const float gainComplement = 1.0f - mGain; + + auto filteredIt = mFilteredCoordinates.begin(); + for (const auto &tag : tags) { + while (filteredIt != mFilteredCoordinates.end() + && filteredIt->first < tag.first) { + ++filteredIt; + } + + if (filteredIt != mFilteredCoordinates.end() + && filteredIt->first == tag.first) { + cv::addWeighted(filteredIt->second, mGain, + tag.second, gainComplement, + 0.0f, filteredIt->second); + } + else { + filteredIt = mFilteredCoordinates.insert(filteredIt, tag); + } + } + + 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 new file mode 100644 index 0000000..ad46c72 --- /dev/null +++ b/src/Filter.hpp @@ -0,0 +1,82 @@ +/******************************************************************************* +* 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 . * +*******************************************************************************/ + +/** This header contains various utilities to paliate with imperfect detection. + */ + +#ifndef Filters_HPP +#define Filters_HPP + +#include +#include + +#include + +namespace chilitags { + +template +class FindOutdated { + +public: + +FindOutdated(int persistence); + +void setPersistence(int persistence) { + mPersistence = persistence; +} + +template +std::vector operator()(const std::map &tags); + +protected: + +int mPersistence; +std::map mDisappearanceTime; + +}; + + + +template +class Filter { +public: + Filter(int persistence, float gain); + + void setPersistence(int persistence) { + mFindOutdated.setPersistence(persistence); + } + + void setGain(float gain) { + mGain = gain; + } + + const std::map & operator()( + const std::map &tags) ; + +protected: + FindOutdated mFindOutdated; + float mGain; + std::map mFilteredCoordinates; +}; + + +} + +#endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 22f87dd..59215ae 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -49,6 +49,7 @@ include_directories(../src) declare_test(TESTNAME drawer) declare_test(TESTNAME codec) +declare_test(TESTNAME Filter) declare_test(TESTNAME integration) declare_test(TESTNAME pose-estimation NEEDS_DATA) declare_test(TESTNAME detection-performance NEEDS_DATA) diff --git a/test/Filter.cpp b/test/Filter.cpp new file mode 100644 index 0000000..3cacdc9 --- /dev/null +++ b/test/Filter.cpp @@ -0,0 +1,179 @@ +/******************************************************************************* +* 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 . * +*******************************************************************************/ + +#ifdef OPENCV3 +#include +#else +#include +#endif + +#include +#include + +#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); + + 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); + + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(1, findOutdated(EMPTY_TAG_LIST).size()); +} + +TEST(FindOutdated2D, InvalidateFirst) { + FindOutdated2D findOutdated(2); + + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(1, findOutdated(ONLY_TAG_43).size()); +} + +TEST(FindOutdated2D, ChangePersistence) { + FindOutdated2D findOutdated(2); + + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + EXPECT_EQ(0, findOutdated(ONLY_TAG_42).size()); + findOutdated.setPersistence(1); + EXPECT_EQ(0, findOutdated(EMPTY_TAG_LIST).size()); + 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::Quad coordinates { + 1.0f,2.0f, + 3.0f,4.0f, + 5.0f,6.0f, + 7.0f,8.0f, + }; + chilitags::Quad expected; + chilitags::TagCornerMap tags; + chilitags::TagCornerMap results; + + tags = {{0, coordinates}}; + results = filter(tags); + expected = coordinates; + EXPECT_EQ(results.size(), tags.size()); + EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); + + coordinates = cv::Mat(cv::Mat(coordinates) + 9.0f); + tags = {{0, coordinates}}; + results = filter(tags); + expected = coordinates; + EXPECT_EQ(results.size(), tags.size()); + EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); +} + +TEST(Filter, NonZeroGain) { + chilitags::Filter filter(0, 0.1f); + chilitags::Quad coordinates { + 1.0f,2.0f, + 3.0f,4.0f, + 5.0f,6.0f, + 7.0f,8.0f, + }; + chilitags::Quad expected; + chilitags::TagCornerMap tags; + chilitags::TagCornerMap results; + + tags = {{0, coordinates}}; + results = filter(tags); + expected = coordinates; + EXPECT_EQ(results.size(), tags.size()); + EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); + + chilitags::Quad coordinates2 = cv::Mat(cv::Mat(coordinates) + 9.0f); + tags = {{0, coordinates2}}; + results = filter(tags); + expected = cv::Mat(0.1f*cv::Mat(coordinates)+0.9f*cv::Mat(coordinates2)); + EXPECT_EQ(results.size(), tags.size()); + EXPECT_EQ(0.0f, cv::norm(cv::Mat(expected) - cv::Mat(results[0]))); +} + +CV_TEST_MAIN(".") diff --git a/test/detection-performance.cpp b/test/detection-performance.cpp index 86e8069..a25b08e 100644 --- a/test/detection-performance.cpp +++ b/test/detection-performance.cpp @@ -93,6 +93,8 @@ TEST(Integration, Snapshots) { chilitags::Chilitags chilitags; // We measure chilitags at its best, and compare optimisations later chilitags.setPerformance(chilitags::Chilitags::ROBUST); + // We do not want any filtering, to measure the raw performances + chilitags.setFilter(0, 0.0f); map resolution; From 33ab3a52e1eef43b73c7bb544f77ca18ea8ce059 Mon Sep 17 00:00:00 2001 From: Quentin Bonnard Date: Sat, 20 Dec 2014 14:36:54 +0100 Subject: [PATCH 19/22] specialize filter for 2D --- src/Chilitags.cpp | 2 +- src/Filter.cpp | 24 ++++------------ src/Filter.hpp | 19 ++++++------- test/Filter.cpp | 70 +++++++---------------------------------------- 4 files changed, 25 insertions(+), 90 deletions(-) diff --git a/src/Chilitags.cpp b/src/Chilitags.cpp index 019ad07..c71c9f2 100644 --- a/src/Chilitags.cpp +++ b/src/Chilitags.cpp @@ -260,7 +260,7 @@ EnsureGreyscale mEnsureGreyscale; Decode mDecode; -Filter mFilter; +Filter mFilter; Detect mDetect; 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/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, From ada9c4204ef06552145b1f6266ae7afb7e72b80e Mon Sep 17 00:00:00 2001 From: Quentin Bonnard Date: Sat, 20 Dec 2014 14:23:24 +0100 Subject: [PATCH 20/22] minor documentation changes --- include/chilitags.hpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/include/chilitags.hpp b/include/chilitags.hpp index bac16be..b98c05a 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -364,7 +364,8 @@ 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, because Chilitags3D - uses a more advanced Kalman filter. + 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 @@ -498,7 +499,8 @@ void setDefaultTagSize(RealT defaultSize); void enableFilter(bool enabled); /** - * @brief Sets the persistence of tags against being discarded when not observed + * @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 */ @@ -515,7 +517,9 @@ void setPersistence(RealT persistence); * 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. + * 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 */ @@ -531,7 +535,9 @@ void setFilterProcessNoiseCovariance(cv::Mat const& covariance); * * 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. + * 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 */ From 19c984d44ac450b4d343d39220cb34214159c53e Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Tue, 16 Dec 2014 16:22:11 +0100 Subject: [PATCH 21/22] Added upside-down screening --- src/ScreenOut.cpp | 72 ++++++++++++++++++++++++++++++++++++++++++++++- src/ScreenOut.hpp | 61 +++++++++++++++++++++++++++++++++++++-- src/Track.cpp | 2 +- src/Track.hpp | 2 ++ 4 files changed, 133 insertions(+), 4 deletions(-) diff --git a/src/ScreenOut.cpp b/src/ScreenOut.cpp index fe0766c..fb8ff86 100644 --- a/src/ScreenOut.cpp +++ b/src/ScreenOut.cpp @@ -24,11 +24,47 @@ * @author Ayberk Özgür */ +#include +#include + #include "ScreenOut.hpp" namespace chilitags{ -bool ScreenOut::isConvex(Quad const& quad) +template +ScreenOut::ScreenOut() : + EPSILON(std::is_same::value ? DBL_EPSILON : FLT_EPSILON), + PI_2(std::acos(std::is_same::value ? 0.0 : 0.0f)), + mFloorVector(0.0f, 0.0f, 0.0f), + mScreenUpsideDown(false) +{ +} + +template +inline void ScreenOut::setFloorVector(cv::Vec floorVector) +{ + if(floorVector(0) == 0.0f && floorVector(1) == 0.0f && floorVector(2) == 0.0f) + mScreenUpsideDown = false; + else{ + mScreenUpsideDown = true; + mFloorVector = floorVector; + } +} + +template +inline bool ScreenOut::operator()(Quad const& quad) +{ + return !isConvex(quad); +} + +template +inline bool ScreenOut::operator()(cv::Mat const& rotation) +{ + return mScreenUpsideDown ? isUpsideDown(rotation) : false; +} + +template +inline bool ScreenOut::isConvex(Quad const& quad) { int vPrevX, vPrevY, vNextX, vNextY; @@ -42,4 +78,38 @@ bool ScreenOut::isConvex(Quad const& quad) return true; } +template +bool ScreenOut::isUpsideDown(cv::Mat const& rotation) +{ + double rot[3]; + memcpy(rot, (double*)rotation.ptr(), 3*sizeof(double)); + RealT theta = (RealT)sqrt(rot[0]*rot[0] + rot[1]*rot[1] + rot[2]*rot[2]); + + cv::Vec zAxis; + if(theta < EPSILON){ + zAxis(0) = 0.0f; + zAxis(1) = 0.0f; + zAxis(2) = 1.0f; + } + else{ + rot[0] /= theta; + rot[1] /= theta; + rot[2] /= theta; + + //Below is the rightmost column of Rodrigues' formula + RealT sTheta = std::sin(theta); + RealT cTheta = std::cos(theta); + RealT cTheta_ = 1.0f - cTheta; + zAxis(0) = (RealT)(rot[0]*rot[2]*cTheta_ + rot[1]*sTheta); + zAxis(1) = (RealT)(rot[1]*rot[2]*cTheta_ - rot[0]*sTheta); + zAxis(2) = (RealT)(rot[2]*rot[2]*cTheta_ + cTheta); + } + + return std::acos(zAxis.dot(mFloorVector)) < PI_2; +} + +//All possible instantiations of ScreenOut +template class ScreenOut; +template class ScreenOut; + } /* namespace chilitags */ diff --git a/src/ScreenOut.hpp b/src/ScreenOut.hpp index 5d37981..9f07f70 100644 --- a/src/ScreenOut.hpp +++ b/src/ScreenOut.hpp @@ -27,15 +27,72 @@ * @author Ayberk Özgür */ -#include +#include + +#include namespace chilitags { +template class ScreenOut { public: - static bool isConvex(Quad const& quad); + /** + * @brief Creates a new screen out object + */ + ScreenOut(); + + /** + * @brief Sets the floor vector against which upside-down tags will be discarded + * + * @param floorVector The latest floor vector + */ + void setFloorVector(cv::Vec floorVector); + + /** + * @brief Calculates whether the tag is invalid + * + * @param quad Quad that describes the tag on the screen + * + * @return Whether the tag is invalid + */ + bool operator()(Quad const& quad); + + /** + * @brief Calculates whether the tag is invalid + * + * @param rotation 3 double precision rotation elements in angle-axis representation denoting the tag's orientation + * + * @return Whether the tag is invalid + */ + bool operator()(cv::Mat const& rotation); + +private: + + /** + * @brief Calculates whether the given quad is convex + * + * @param quad Polygon with 4 consecutive corners + * + * @return Whether the quad is convex + */ + bool isConvex(Quad const& quad); + + /** + * @brief Gets whether the given rotation is upside down w.r.t floor vector + * + * @param rotation 3 double precision rotation elements in angle-axis representation + * + * @return Whether the z axis makes less than 90 degrees with the floor vector + */ + bool isUpsideDown(cv::Mat const& rotation); + + const RealT EPSILON; ///< One of FLT_EPSILON, DBL_EPSILON depending on the template type + const RealT PI_2; ///< Pi/2 + + cv::Vec mFloorVector; ///< Latest floor vector + bool mScreenUpsideDown; ///< Whether to screen out upside-down tags }; } /* namespace chilitags */ diff --git a/src/Track.cpp b/src/Track.cpp index b4056c3..a18debe 100644 --- a/src/Track.cpp +++ b/src/Track.cpp @@ -89,7 +89,7 @@ TagCornerMap Track::operator()(cv::Mat const& grayscaleInputImage) if (cv::sum(cv::Mat(status))[0] == status.size()) { quad = mRefine(grayscaleInputImage, result, 0.5f/10.0f); - if(ScreenOut::isConvex(quad)) + if(!mScreenOut(quad)) trackedTags[tag.first] = quad; } } diff --git a/src/Track.hpp b/src/Track.hpp index ad8541b..a3e2264 100644 --- a/src/Track.hpp +++ b/src/Track.hpp @@ -28,6 +28,7 @@ #include #include "Refine.hpp" +#include "ScreenOut.hpp" namespace chilitags { @@ -44,6 +45,7 @@ TagCornerMap operator()(cv::Mat const& inputImage); protected: Refine mRefine; +ScreenOut mScreenOut; cv::Mat mPrevFrame; TagCornerMap mFromTags; From ff370dc081c99dd48e04653fb1151aee7cb0f100 Mon Sep 17 00:00:00 2001 From: ayberkozgur Date: Tue, 16 Dec 2014 16:42:00 +0100 Subject: [PATCH 22/22] Floor vector API --- include/chilitags.hpp | 17 +++++++++++++++++ src/Chilitags3D.cpp | 9 +++++++++ src/EstimatePose3D.cpp | 9 +++++++++ src/EstimatePose3D.hpp | 23 ++++++++++++++++------- src/Filter3D.hpp | 1 + src/ScreenOut.cpp | 2 +- src/ScreenOut.hpp | 2 +- 7 files changed, 54 insertions(+), 9 deletions(-) diff --git a/include/chilitags.hpp b/include/chilitags.hpp index b98c05a..e084a1a 100644 --- a/include/chilitags.hpp +++ b/include/chilitags.hpp @@ -543,6 +543,23 @@ void setFilterProcessNoiseCovariance(cv::Mat const& covariance); */ void setFilterObservationNoiseCovariance(cv::Mat const& covariance); +/** + * @brief Sets the unit floor vector that will screen out upside down tags + * + * For the next 3D estimation, sets the floor vector against which all upside + * down tag detections will be discarded. The floor vector is defined as the + * inverse of the unit vector that points towards the center of the Earth. This + * means that all tags whose z axes make a smaller than 90 degree angle with + * this vector will be discarded. The floor vector must be in the camera frame. + * + * By default, the internal floor vector is zero, meaning that the upside down + * screening is disabled. The floor vector can be set to zero at any time to + * disable upside-down screening. + * + * @param floorVector Unit floor vector or zero vector to disable upside-down screening + */ +void setFloorVector(cv::Vec const& floorVector); + /** For accurate results, Chilitags3D can be provided the calibration data of the camera detecting the chilitags. See diff --git a/src/Chilitags3D.cpp b/src/Chilitags3D.cpp index c5d7b9b..f80098e 100644 --- a/src/Chilitags3D.cpp +++ b/src/Chilitags3D.cpp @@ -156,6 +156,10 @@ void setFilterObservationNoiseCovariance(cv::Mat const& covariance){ mEstimatePose3D.setFilterObservationNoiseCovariance(covariance); } +void setFloorVector(cv::Vec const& floorVector){ + mEstimatePose3D.setFloorVector(floorVector); +} + bool read3DConfiguration(const std::string &filenameOrString, bool omitOtherTags, bool readFromString) { mOmitOtherTags = omitOtherTags; @@ -362,6 +366,11 @@ void Chilitags3D_::setFilterObservationNoiseCovariance(cv::Mat const& cov mImpl->setFilterObservationNoiseCovariance(covariance); } +template +void Chilitags3D_::setFloorVector(cv::Vec const& floorVector){ + mImpl->setFloorVector(floorVector); +} + 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 e977e02..d9a9cd2 100644 --- a/src/EstimatePose3D.cpp +++ b/src/EstimatePose3D.cpp @@ -96,6 +96,12 @@ void EstimatePose3D::setCamDelta(cv::Vec const& camDeltaR, cv:: mFilter3D.setCamDelta(camDeltaR, camDeltaX); } +template +void EstimatePose3D::setFloorVector(cv::Vec const& floorVector) +{ + mScreenOut.setFloorVector(floorVector); +} + template void EstimatePose3D::operator()(typename Chilitags3D_::TagPoseMap& objects) { @@ -121,6 +127,9 @@ void EstimatePose3D::operator()(std::string const& name, #endif //TODO: Rotation and translation vectors come out of solvePnP as double + if(mScreenOut(mTempRotation)) + return; + if(mFilter3DEnabled) mFilter3D(name, mTempTranslation, mTempRotation); diff --git a/src/EstimatePose3D.hpp b/src/EstimatePose3D.hpp index d3507d8..e3ff3c0 100644 --- a/src/EstimatePose3D.hpp +++ b/src/EstimatePose3D.hpp @@ -110,6 +110,13 @@ class EstimatePose3D */ void setCamDelta(cv::Vec const& camDeltaR, cv::Vec const& camDeltaX); + /** + * @brief Sets the floor vector in the camera frame for upside-down screening + * + * @param floorVector Latest floor vector + */ + void setFloorVector(cv::Vec const& floorVector); + /** * @brief Updates the poses of all known tags via statistical filtering * @@ -132,17 +139,19 @@ class EstimatePose3D protected: - Filter3D mFilter3D; ///< Kalman filter to increase stability of the tag - bool mFilter3DEnabled; ///< Whether to enable pose filtering + Filter3D mFilter3D; ///< Kalman filter to increase stability of the tag + bool mFilter3DEnabled; ///< Whether to enable pose filtering + + ScreenOut mScreenOut; ///< Object that discards upside-down readings - cv::Mat mCameraMatrix; ///< 3x3 camera matrix - cv::Mat mDistCoeffs; ///< Empty or 4x1 or 5x1 or 8x1 Distortion coefficients of the camera + cv::Mat mCameraMatrix; ///< 3x3 camera matrix + cv::Mat mDistCoeffs; ///< Empty or 4x1 or 5x1 or 8x1 Distortion coefficients of the camera - cv::Mat mTempRotation; ///< 3x1 axis-angle: (rx,ry,rz) - cv::Mat mTempTranslation; ///< 3x1 translation: (x,y,z) + cv::Mat mTempRotation; ///< 3x1 axis-angle: (rx,ry,rz) + cv::Mat mTempTranslation; ///< 3x1 translation: (x,y,z) //TODO: This is double because of rodrigues, it doesn't accept float at the time of writing - cv::Matx33d mTempRotMat; ///< 3x3 rotation matrix representation of mTempRotation + cv::Matx33d mTempRotMat; ///< 3x3 rotation matrix representation of mTempRotation }; diff --git a/src/Filter3D.hpp b/src/Filter3D.hpp index e1416d8..b69e4df 100644 --- a/src/Filter3D.hpp +++ b/src/Filter3D.hpp @@ -133,6 +133,7 @@ #include #include "chilitags.hpp" +#include "ScreenOut.hpp" namespace chilitags { diff --git a/src/ScreenOut.cpp b/src/ScreenOut.cpp index fb8ff86..598fecd 100644 --- a/src/ScreenOut.cpp +++ b/src/ScreenOut.cpp @@ -41,7 +41,7 @@ ScreenOut::ScreenOut() : } template -inline void ScreenOut::setFloorVector(cv::Vec floorVector) +inline void ScreenOut::setFloorVector(cv::Vec const& floorVector) { if(floorVector(0) == 0.0f && floorVector(1) == 0.0f && floorVector(2) == 0.0f) mScreenUpsideDown = false; diff --git a/src/ScreenOut.hpp b/src/ScreenOut.hpp index 9f07f70..8583259 100644 --- a/src/ScreenOut.hpp +++ b/src/ScreenOut.hpp @@ -48,7 +48,7 @@ class ScreenOut * * @param floorVector The latest floor vector */ - void setFloorVector(cv::Vec floorVector); + void setFloorVector(cv::Vec const& floorVector); /** * @brief Calculates whether the tag is invalid