Kalman filter prototype

include/chilitags.hpp

@@ -363,10 +363,9 @@ typedef std::map<std::string, TransformMatrix> TagPoseMap;
 
     To first detect th tags in the image, Chilitags3D creates a Chilitags
     instance, which can be accessed through the getChilitags() accessors. This
-    Chilitags instance is set to have a persistence of 0, but Chilitags3D sets
-    a default persistence of 5 frames for poses it estimates. It also sets a
-    gain of .5 to avoid the detected objects to "shake". Please refer to the
-    documentation of Chilitags3D::setFilter() for more detail.
+    Chilitags instance is set to have a persistence of 0, because Chilitags3D
+    uses a more advanced Kalman filter. See enableFilter() and setPersistence()
+    for more details.
 
     You can also create yourself a separate instance of Chilitagsfor the 2D
     detection of tags and use it by calling
@@ -384,28 +383,6 @@ typedef std::map<std::string, TransformMatrix> TagPoseMap;
  */
 Chilitags3D_(cv::Size cameraResolution = cv::Size(640, 480));
 
-/**
-    Parameters to paliate with the imperfections of the detection.
-
-    \param persistence the number of frames in which a 3d object, i.e. a tag or
-    a rigid object containing several tags, should be absent before being
-    removed from the output of estimate().
-
-    \param gain a value between 0 and 1 corresponding to the weight of the
-    previous (filtered) position in the new filtered position. 0 means that the
-    latest position of the object is returned.
-
-    Chilitags3D manages persistence separately from the 2D detection of
-    Chilitags, because an object can be composed of several tags. If one or
-    more of these tags are not detected, the pose of the object is estimated
-    with the remaining, detected tags. In this case, it would hurt the pose
-    estimation to make the individual tags persistent. Chilitags3D thus sets
-    the 2D detection to have no persistence, and applies its persistence
-    mechanism only after the estimation of the pose. The same goes for the
-    position filtering. Note that for independent tags, it should not matter.
- */
-void setFilter(int persistence, float gain);
-
 /** Accessor to the underlying (2D) Chilitags detection. */
 const Chilitags &getChilitags() const;
 
@@ -424,8 +401,18 @@ Chilitags &getChilitags();
         0 ,   0 ,   0 ,  1 }
     \endverbatim
     \param tags a list of tags, as returned by Chilitags::find().
+
+    \param camDeltaR Rotation from the previous camera frame to
+    the current camera frame, i.e rotation of the current camera frame in the
+    last camera frame. Quaternion format (scalar, vx, vy, vz).
+
+    \param camDeltaX Translation from the previous camera frame
+    to the current camera frame, i.e position of the current camera frame in
+    the last camera frame.
  */
-TagPoseMap estimate(const TagCornerMap & tags);
+TagPoseMap estimate(const TagCornerMap & tags,
+    cv::Vec<RealT, 4> const& camDeltaR = cv::Vec<RealT, 4>(1,0,0,0),
+    cv::Vec<RealT, 3> const& camDeltaX = cv::Vec<RealT, 3>(0,0,0));
 
 /**
     This is a convenience variant of estimate() which also takes care of the
@@ -449,10 +436,20 @@ TagPoseMap estimate(const TagCornerMap & tags);
     best return tags previously found; it won't find new ones, but
     can lose some. See Chilitags::DetectionTrigger for a description of the
     possible values.
+
+    \param camDeltaR Rotation from the previous camera frame to
+    the current camera frame, i.e rotation of the current camera frame in the
+    last camera frame. Quaternion format (scalar, vx, vy, vz).
+
+    \param camDeltaX Translation from the previous camera frame
+    to the current camera frame, i.e position of the current camera frame in
+    the last camera frame.
  */
 TagPoseMap estimate(
     const cv::Mat &inputImage,
-    Chilitags::DetectionTrigger detectionTrigger = Chilitags::DETECT_ONLY);
+    Chilitags::DetectionTrigger detectionTrigger = Chilitags::DETECT_ONLY,
+    cv::Vec<RealT, 4> const& camDeltaR = cv::Vec<RealT, 4>(1,0,0,0),
+    cv::Vec<RealT, 3> const& camDeltaX = cv::Vec<RealT, 3>(0,0,0));
 
 /**
     Chilitags3D can also detect rigid assemblies of tags. This allows for a
@@ -494,6 +491,58 @@ bool readTagConfiguration(
  */
 void setDefaultTagSize(RealT defaultSize);
 
+/**
+ * @brief Enables/disables Kalman filtering on tag pose (enabled by default)
+ *
+ * @param enabled Whether to enable Kalman filtering
+ */
+void enableFilter(bool enabled);
+
+/**
+ * @brief Sets the persistence of tags against being discarded when not
+ * observed (10 by default)
+ *
+ * @param persistence Persistence value, roughly correponds to number of frames
+ */
+void setPersistence(RealT persistence);
+
+/**
+ * @brief Sets the process noise covariance matrix a.k.a Q for the Kalman filter
+ *
+ * The state is described by (x,y,z,qw,qx,qy,qz) where x,y,z is the tag
+ * position and qw,qx,qy,qz is the tag orientation in quaternion
+ * representation. Therefore, the input matrix should correspond to
+ * cov((x,y,z,qw,qx,qy,qz)).
+ *
+ * When IMU data is present, the process moves the tag according to camera
+ * movement. When IMU data is not present, the process tries to keep the tag
+ * still. Smaller values in the process covariance matrix causes the tags to
+ * resist motion more. By default, this matrix has
+ * 1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4, 1e-4
+ * on its diagonal.
+ *
+ * @param covariance 7x7 covariance matrix
+ */
+void setFilterProcessNoiseCovariance(cv::Mat const& covariance);
+
+/**
+ * @brief Sets the observation noise covariance matrix a.k.a R for the Kalman filter
+ *
+ * The observation (done by image processing on the camera image) is described
+ * by (x,y,z,qw,qx,qy,qz) where x,y,z is the tag position and qw,qx,qy,qz is
+ * the tag orientation in quaternion representation. Therefore, the input
+ * matrix should correspond to cov((x,y,z,qw,qx,qy,qz)).
+ *
+ * Larger values in the observation noise covariance matrix indicates noisier
+ * measurements and causes observations to affect the state less, making tags
+ * more resistant to motion. By default, this matrix has
+ * 1e-3, 1e-3, 1e-1, 1e-3, 1e-2, 1e-2, 1e-5
+ * on its diagonal.
+ *
+ * @param covariance 7x7 covariance matrix
+ */
+void setFilterObservationNoiseCovariance(cv::Mat const& covariance);
+
 /**
     For accurate results, Chilitags3D can be provided the calibration data of
     the camera detecting the chilitags.  See

samples/3dfiltering/filter3d-gui.cpp

@@ -0,0 +1,209 @@
+/*******************************************************************************
+ *   Copyright 2013-2014 EPFL                                                   *
+ *   Copyright 2013-2014 Quentin Bonnard                                        *
+ *                                                                              *
+ *   This file is part of chilitags.                                            *
+ *                                                                              *
+ *   Chilitags is free software: you can redistribute it and/or modify          *
+ *   it under the terms of the Lesser GNU General Public License as             *
+ *   published by the Free Software Foundation, either version 3 of the         *
+ *   License, or (at your option) any later version.                            *
+ *                                                                              *
+ *   Chilitags is distributed in the hope that it will be useful,               *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of             *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
+ *   GNU Lesser General Public License for more details.                        *
+ *                                                                              *
+ *   You should have received a copy of the GNU Lesser General Public License   *
+ *   along with Chilitags.  If not, see <http://www.gnu.org/licenses/>.         *
+ *******************************************************************************/
+
+#include <iostream>
+
+#include <chilitags.hpp>
+
+#include <opencv2/core/core.hpp> // for cv::Mat
+#include <opencv2/core/core_c.h> // CV_AA
+#include <opencv2/highgui/highgui.hpp> // for camera capture
+#include <opencv2/imgproc/imgproc.hpp> // for camera capture
+
+using namespace std;
+using namespace cv;
+
+int main(int argc, char* argv[])
+{
+    cout
+        << "Usage: "<< argv[0]
+        << " [-c <tag configuration (YAML)>] [-i <camera calibration (YAML)>]\n";
+
+    const char* intrinsicsFilename = 0;
+    const char* configFilename = 0;
+
+    for( int i = 1; i < argc; i++ )
+    {
+        if( strcmp(argv[i], "-c") == 0 )
+            configFilename = argv[++i];
+        else if( strcmp(argv[i], "-i") == 0 )
+            intrinsicsFilename = argv[++i];
+    }
+
+    /*****************************/
+    /*    Init camera capture    */
+    /*****************************/
+    int cameraIndex = 0;
+    cv::VideoCapture capture(cameraIndex);
+    if (!capture.isOpened())
+    {
+        cerr << "Unable to initialise video capture.\n";
+        return 1;
+    }
+
+    /******************************/
+    /* Setting up pose estimation */
+    /******************************/
+#ifdef OPENCV3
+    double inputWidth  = capture.get(cv::CAP_PROP_FRAME_WIDTH);
+    double inputHeight = capture.get(cv::CAP_PROP_FRAME_HEIGHT);
+#else
+    double inputWidth  = capture.get(CV_CAP_PROP_FRAME_WIDTH);
+    double inputHeight = capture.get(CV_CAP_PROP_FRAME_HEIGHT);
+#endif
+
+    chilitags::Chilitags3D chilitags3D(Size(inputWidth, inputHeight));
+
+    if (configFilename) chilitags3D.readTagConfiguration(configFilename);
+
+    if (intrinsicsFilename) {
+        Size calibratedImageSize = chilitags3D.readCalibration(intrinsicsFilename);
+#ifdef OPENCV3
+        capture.set(cv::CAP_PROP_FRAME_WIDTH, calibratedImageSize.width);
+        capture.set(cv::CAP_PROP_FRAME_HEIGHT, calibratedImageSize.height);
+#else
+        capture.set(CV_CAP_PROP_FRAME_WIDTH, calibratedImageSize.width);
+        capture.set(CV_CAP_PROP_FRAME_HEIGHT, calibratedImageSize.height);
+#endif
+    }
+
+    cv::Mat projectionMat = cv::Mat::zeros(4,4,CV_64F);
+    chilitags3D.getCameraMatrix().copyTo(projectionMat(cv::Rect(0,0,3,3)));
+    cv::Matx44d projection = projectionMat;
+    projection(3,2) = 1;
+
+    /*****************************/
+    /*             Go!           */
+    /*****************************/
+    cv::namedWindow("Pose Estimation");
+
+    char keyPressed;
+    bool filterEnabled = true;
+
+    cv::Mat Q = (cv::Mat_<float>(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_<float>(7,7) <<
+            1e-3f,  0,      0,      0,      0,      0,      0,
+            0,      1e-3f,  0,      0,      0,      0,      0,
+            0,      0,      1e-1f,  0,      0,      0,      0,
+            0,      0,      0,      1e-3f,  0,      0,      0,
+            0,      0,      0,      0,      1e-2f,  0,      0,
+            0,      0,      0,      0,      0,      1e-2f,  0,
+            0,      0,      0,      0,      0,      0,      1e-5f);
+    float alphaR = 1.0f;
+
+    while ('q' != (keyPressed = (char) cv::waitKey(1))) {
+
+        switch(keyPressed){
+            case 'f':
+                filterEnabled = !filterEnabled;
+                chilitags3D.enableFilter(filterEnabled);
+                break;
+            case 'w':
+                alphaQ *= 10.0f;
+                chilitags3D.setFilterProcessNoiseCovariance(alphaQ*Q);
+                break;
+            case 's':
+                alphaQ /= 10.0f;
+                chilitags3D.setFilterProcessNoiseCovariance(alphaQ*Q);
+                break;
+            case 'e':
+                alphaR *= 10.0f;
+                chilitags3D.setFilterObservationNoiseCovariance(alphaR*R);
+                break;
+            case 'd':
+                alphaR /= 10.0f;
+                chilitags3D.setFilterObservationNoiseCovariance(alphaR*R);
+                break;
+        }
+
+        cv::Mat inputImage;
+        capture.read(inputImage);
+        cv::Mat outputImage = inputImage.clone();
+
+        cv::putText(outputImage, cv::format("Filtering %s, press 'f' to toggle",filterEnabled ? "ENABLED" : "DISABLED"),
+                cv::Point(8,20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255));
+
+        cv::putText(outputImage, cv::format("Process covariance multiplier: %f, press 'w' or 's' to modify", alphaQ),
+                cv::Point(8,36), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255));
+
+        cv::putText(outputImage, cv::format("Observation covariance multiplier: %f, press 'e' or 'd' to modify", alphaR),
+                cv::Point(8,52), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255));
+
+        for (auto& kv : chilitags3D.estimate(inputImage, chilitags::Chilitags::DETECT_PERIODICALLY)) {
+
+            static const float DEFAULT_SIZE = 20.f;
+            static const cv::Vec4d UNITS[4] {
+                {0.f, 0.f, 0.f, 1.f},
+                    {DEFAULT_SIZE, 0.f, 0.f, 1.f},
+                    {0.f, DEFAULT_SIZE, 0.f, 1.f},
+                    {0.f, 0.f, DEFAULT_SIZE, 1.f},
+            };
+
+            cv::Matx44d transformation = kv.second;
+            cv::Vec4f referential[4] = {
+                projection*transformation*UNITS[0],
+                projection*transformation*UNITS[1],
+                projection*transformation*UNITS[2],
+                projection*transformation*UNITS[3],
+            };
+
+            std::vector<cv::Point2f> 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<<SHIFT;
+            static const std::string AXIS_NAMES[3] = { "x", "y", "z" };
+            static const cv::Scalar AXIS_COLORS[3] = {
+                {0,0,255},{0,255,0},{255,0,0},
+            };
+            for (int i : {1,2,3}) {
+                cv::line(
+                        outputImage,
+                        PRECISION*t2DPoints[0],
+                        PRECISION*t2DPoints[i],
+                        AXIS_COLORS[i-1],
+#ifdef OPENCV3
+                        2, cv::LINE_AA, SHIFT);
+#else
+                2, CV_AA, SHIFT);
+#endif
+            }
+        }
+        cv::imshow("Pose Estimation", outputImage);
+    }
+
+    cv::destroyWindow("Pose Estimation");
+    capture.release();
+
+    return 0;
+}
+

samples/CMakeLists.txt

@@ -48,3 +48,7 @@ if(WITH_PTHREADS)
     target_link_libraries( async-detection ${OpenCV_LIBS} )
 endif()
 
+add_executable( filter3d-gui 3dfiltering/filter3d-gui.cpp)
+target_link_libraries( filter3d-gui chilitags )
+target_link_libraries( filter3d-gui ${OpenCV_LIBS} )
+

src/Chilitags.cpp

@@ -60,8 +60,8 @@ Impl() :
     mDetect(),
     mTrack(),
 
-    mCallsBeforeDetection(15),
-    mCallsBeforeNextDetection(0)
+    mCallsBeforeNextDetection(0),
+    mCallsBeforeDetection(15)
 {
     setPerformance(FAST);
 }
@@ -260,7 +260,7 @@ EnsureGreyscale mEnsureGreyscale;
 
 Decode mDecode;
 
-Filter<int, Quad> mFilter;
+Filter mFilter;
 
 Detect mDetect;
 
@@ -296,10 +296,7 @@ void Chilitags::setMinInputWidth(int minWidth) {
     mImpl->setMinInputWidth(minWidth);
 }
 
-TagCornerMap Chilitags::find(
-    const cv::Mat &inputImage,
-    DetectionTrigger trigger
-    ) {
+TagCornerMap Chilitags::find(const cv::Mat &inputImage, DetectionTrigger trigger) {
     return mImpl->find(inputImage, trigger);
 }