Allow for passing non-uint8 images into navigation algos

examples/perfect_memory/perfect_memory.cc

@@ -30,12 +30,15 @@ void trainRoute(PM &pm, const std::vector<cv::Mat> &images)
 }
 }
 
-int bobMain(int, char **)
+int bobMain(int argc, char **argv)
 {
     const cv::Size imSize(180, 50);
     units::angle::degree_t heading;
 
-    const ImageDatabase imdb{ Path::getRepoPath() / "tools/ant_world_db_creator/ant1_route1" };
+    BOB_ASSERT(argc <= 2);
+
+    filesystem::path dbPath = (argc == 2) ? argv[1] : (Path::getRepoPath() / "tools/ant_world_db_creator/ant1_route1");
+    const ImageDatabase imdb{ std::move(dbPath) };
     const auto snapshots = imdb.readImages(imSize);
     LOGI << "Loaded " << snapshots.size() << " snapshots";
 
@@ -147,13 +150,9 @@ int bobMain(int, char **)
         // Time testing phase
         Timer<> t{ "Time taken for testing: " };
 
-        // Treat snapshot #10 as test data
-        cv::Mat snap = cv::imread((Path::getRepoPath() / "tools/ant_world_db_creator/ant1_route1/image_00010.png").str(), cv::IMREAD_GRAYSCALE);
-        BOB_ASSERT(!snap.empty());
-        cv::resize(snap, snap, imSize);
         size_t snapshot;
         float difference;
-        std::tie(heading, snapshot, difference, std::ignore) = pm.getHeading(snap);
+        std::tie(heading, snapshot, difference, std::ignore) = pm.getHeading(snapshots[10]);
         LOGI << "Heading: " << heading;
         LOGI << "Best-matching snapshot: #" << snapshot;
         LOGI << "Difference score: " << difference;

include/imgproc/convert_scale.h

@@ -0,0 +1,11 @@
+#pragma once
+
+// OpenCV
+#include <opencv2/opencv.hpp>
+
+namespace BoBRobotics {
+namespace ImgProc {
+void
+convertScale(const cv::Mat &in, cv::Mat &out, int targetType);
+} // ImgProc
+} // BoBRobotics

include/navigation/infomax.h

@@ -2,6 +2,7 @@
 
 // BoB robotics includes
 #include "common/macros.h"
+#include "imgproc/convert_scale.h"
 #include "imgproc/mask.h"
 #include "navigation/insilico_rotater.h"
 
@@ -173,14 +174,12 @@ class InfoMax
 
     void calculateUY(const cv::Mat &image)
     {
-        BOB_ASSERT(image.type() == CV_8UC1);
-
         const cv::Size &unwrapRes = getUnwrapResolution();
         BOB_ASSERT(image.cols == unwrapRes.width);
         BOB_ASSERT(image.rows == unwrapRes.height);
 
-        // Convert image to vector of floats
-        m_U = m_Weights * getNetInputs(image) * m_TanhScalingFactor;
+        convertImage(image);
+        m_U = m_Weights * getNetInputs() * m_TanhScalingFactor;
         m_Y = tanh(m_U.array());
     }
 
@@ -192,7 +191,8 @@ class InfoMax
 
     VectorType getNetOutputs(const cv::Mat &image) const
     {
-        return m_Weights * getNetInputs(image);
+        convertImage(image);
+        return m_Weights * getNetInputs();
     }
 
 private:
@@ -203,7 +203,7 @@ class InfoMax
     VectorType m_U, m_Y;
 
     template<class T>
-    static auto toZScore(const T &vec)
+    static VectorType toZScore(const T &vec)
     {
         const FloatType mean = vec.mean();
 
@@ -213,14 +213,14 @@ class InfoMax
     }
 
     //! Converts image to VectorType and normalises
-    VectorType getNetInputs(const cv::Mat &image) const
+    VectorType getNetInputs() const
     {
-        Eigen::Map<Eigen::Matrix<uint8_t, Eigen::Dynamic, 1>> map(image.data, image.cols * image.rows);
-        const auto vec = map.cast<FloatType>();
+        Eigen::Map<VectorType> vec(reinterpret_cast<FloatType *>(m_ScratchImage.data),
+                                   m_ScratchImage.cols * m_ScratchImage.rows);
 
         switch (m_Normalisation) {
         case Normalisation::None:
-            return vec / 255.0;
+            return vec;
         case Normalisation::ZScore:
             return toZScore(vec);
         default:
@@ -234,6 +234,19 @@ class InfoMax
     {
         return (mat.array() * mat.array()).rowwise().mean().sqrt();
     }
+
+protected:
+    mutable cv::Mat m_ScratchImage;
+
+    void convertImage(const cv::Mat &image) const
+    {
+        BOB_ASSERT(image.channels() == 1);
+        const auto &unwrapRes = getUnwrapResolution();
+        BOB_ASSERT(image.cols == unwrapRes.width);
+        BOB_ASSERT(image.rows == unwrapRes.height);
+
+        ImgProc::convertScale(image, m_ScratchImage, cv::DataType<FloatType>::type);
+    }
 }; // InfoMax
 
 //------------------------------------------------------------------------
@@ -272,8 +285,17 @@ class InfoMaxRotater : public InfoMax<FloatType>
     {
         using radian_t = units::angle::radian_t;
 
+        /*
+         * We don't *need* to do this conversion here, as
+         * PerfectMemory<>::test() will accept non-float images as inputs, but
+         * by doing so we only have to do the conversion once as opposed to once
+         * per rotation.
+         */
+        this->convertImage(image);
         const cv::Size unwrapRes = this->getUnwrapResolution();
-        auto rotater = InSilicoRotater::create(unwrapRes, mask, image, std::forward<Ts>(args)...);
+        auto rotater = InSilicoRotater::create(unwrapRes, mask,
+                                               this->m_ScratchImage,
+                                               std::forward<Ts>(args)...);
         calcImageDifferences(rotater);
 
         // Find index of lowest difference

include/navigation/insilico_rotater.h

@@ -46,8 +46,6 @@ struct InSilicoRotater
         {
             BOB_ASSERT(image.cols == unwrapRes.width);
             BOB_ASSERT(image.rows == unwrapRes.height);
-            BOB_ASSERT(image.type() == CV_8UC1);
-            BOB_ASSERT(image.isContinuous());
             BOB_ASSERT(beginRoll < endRoll);
             BOB_ASSERT((distance(endRoll, beginRoll) % scanStep) == 0);
         }

include/navigation/perfect_memory.h

@@ -2,9 +2,10 @@
 
 // BoB robotics includes
 #include "common/macros.h"
-#include "differencers.h"
-#include "insilico_rotater.h"
-#include "perfect_memory_store_raw.h"
+#include "imgproc/convert_scale.h"
+#include "navigation/differencers.h"
+#include "navigation/insilico_rotater.h"
+#include "navigation/perfect_memory_store_raw.h"
 
 // Third-party includes
 #include "third_party/units.h"
@@ -46,6 +47,7 @@ class PerfectMemory
     PerfectMemory(const cv::Size &unwrapRes, Ts &&... args)
       : m_UnwrapRes(unwrapRes)
       , m_Store(unwrapRes, std::forward<Ts>(args)...)
+      , m_ScratchImage(unwrapRes, CV_8UC1)
     {}
 
     //------------------------------------------------------------------------
@@ -58,13 +60,10 @@ class PerfectMemory
     //------------------------------------------------------------------------
     void train(const cv::Mat &image, const ImgProc::Mask &mask = ImgProc::Mask{})
     {
-        const auto &unwrapRes = getUnwrapResolution();
-        BOB_ASSERT(image.cols == unwrapRes.width);
-        BOB_ASSERT(image.rows == unwrapRes.height);
-        BOB_ASSERT(image.type() == CV_8UC1);
+        convertImage(image);
 
         // Add snapshot
-        m_Store.addSnapshot(image, mask);
+        m_Store.addSnapshot(m_ScratchImage, mask);
     }
 
     float test(const cv::Mat &image, const ImgProc::Mask &mask, const Window &window) const
@@ -116,16 +115,6 @@ class PerfectMemory
     //! Get the resolution of images
     const cv::Size &getUnwrapResolution() const { return m_UnwrapRes; }
 
-protected:
-    //------------------------------------------------------------------------
-    // Protected API
-    //------------------------------------------------------------------------
-    float calcSnapshotDifference(const cv::Mat &image,
-                                 const ImgProc::Mask &mask, size_t snapshot) const
-    {
-        return m_Store.calcSnapshotDifference(image, mask, snapshot);
-    }
-
 private:
     //------------------------------------------------------------------------
     // Private members
@@ -136,11 +125,8 @@ class PerfectMemory
 
     void testInternal(const cv::Mat &image, const ImgProc::Mask &mask, const Window &window) const
     {
-        const auto &unwrapRes = getUnwrapResolution();
-        BOB_ASSERT(image.cols == unwrapRes.width);
-        BOB_ASSERT(image.rows == unwrapRes.height);
-        BOB_ASSERT(image.type() == CV_8UC1);
-        BOB_ASSERT(mask.isValid(unwrapRes));
+        convertImage(image);
+        BOB_ASSERT(mask.isValid(getUnwrapResolution()));
 
         BOB_ASSERT(window.first < getNumSnapshots());
         BOB_ASSERT(window.second <= getNumSnapshots());
@@ -152,10 +138,32 @@ class PerfectMemory
         tbb::parallel_for(tbb::blocked_range<size_t>(window.first, window.second),
             [&](const auto &r) {
                 for (size_t s = r.begin(); s != r.end(); ++s) {
-                    m_Differences[s - window.first] = calcSnapshotDifference(image, mask, s);
+                    m_Differences[s - window.first] = calcSnapshotDifference(m_ScratchImage, mask, s);
                 }
             });
     }
+
+protected:
+    //------------------------------------------------------------------------
+    // Protected API
+    //------------------------------------------------------------------------
+    mutable cv::Mat m_ScratchImage;
+
+    float calcSnapshotDifference(const cv::Mat &image,
+                                 const ImgProc::Mask &mask, size_t snapshot) const
+    {
+        return m_Store.calcSnapshotDifference(image, mask, snapshot);
+    }
+
+    void convertImage(const cv::Mat &image) const
+    {
+        BOB_ASSERT(image.channels() == 1);
+        const auto &unwrapRes = getUnwrapResolution();
+        BOB_ASSERT(image.cols == unwrapRes.width);
+        BOB_ASSERT(image.rows == unwrapRes.height);
+
+        ImgProc::convertScale(image, m_ScratchImage, CV_8U);
+    }
 };
 
 //------------------------------------------------------------------------
@@ -223,7 +231,16 @@ class PerfectMemoryRotater : public PerfectMemory<Store>
     template<class... Ts>
     auto getHeading(const cv::Mat &image, ImgProc::Mask mask, typename PerfectMemory<Store>::Window window, Ts &&... args) const
     {
-        auto rotater = InSilicoRotater::create(this->getUnwrapResolution(), mask, image, std::forward<Ts>(args)...);
+        /*
+         * We don't *need* to do this conversion here, as
+         * PerfectMemory<>::test() will accept non-uint8 images as inputs, but
+         * by doing so we only have to do the conversion once as opposed to once
+         * per rotation.
+         */
+        this->convertImage(image);
+        auto rotater = InSilicoRotater::create(this->getUnwrapResolution(),
+                                               mask, this->m_ScratchImage,
+                                               std::forward<Ts>(args)...);
         calcImageDifferences(window, rotater);
 
         // Now get the minimum for each snapshot and the column this corresponds to

python/navigation/src/navigation.cc

@@ -51,17 +51,31 @@ type_caster<cv::Mat>::load(handle src, bool)
 
     auto *arr = reinterpret_cast<PyArrayObject *>(obj);
 
-    /*
-     * It would be totally trivial to add other types, but we're only
-     * interested in uint8-type images for now.
-     */
     int cvType;
     switch (PyArray_TYPE(arr)) {
     case NPY_UINT8:
         cvType = CV_8U;
         break;
+    case NPY_INT8:
+        cvType = CV_8S;
+        break;
+    case NPY_UINT16:
+        cvType = CV_16U;
+        break;
+    case NPY_INT16:
+        cvType = CV_16S;
+        break;
+    case NPY_INT32:
+        cvType = CV_32S;
+        break;
+    case NPY_FLOAT32:
+        cvType = CV_32F;
+        break;
+    case NPY_FLOAT64:
+        cvType = CV_64F;
+        break;
     default:
-        throw std::runtime_error{ "Only uint8-type arrays are allowed" };
+        throw std::runtime_error{ "Numpy array could not be converted to OpenCV matrix (invalid type)" };
     }
 
     /*

src/imgproc/CMakeLists.txt

@@ -3,7 +3,8 @@ include(../../cmake/BoBRobotics.cmake)
 project(imgproc)
 
 find_package(BoBRobotics REQUIRED COMPONENTS common)
-BoB_module(bee_eye.cc mask.cc opencv_optical_flow.cc opencv_unwrap_360.cc roll.cc)
+BoB_module(bee_eye.cc convert_scale.cc mask.cc opencv_optical_flow.cc
+           opencv_unwrap_360.cc roll.cc)
 
 find_package(OpenCV REQUIRED)
 target_include_directories(${BOB_MODULE_TARGET} PUBLIC ${OpenCV_INCLUDE_DIRS})

src/imgproc/convert_scale.cc

@@ -0,0 +1,62 @@
+// BoB robotics includes
+#include "common/macros.h"
+#include "imgproc/convert_scale.h"
+
+// Standard C++ includes
+#include <limits>
+#include <stdexcept>
+#include <type_traits>
+
+// Standard C includes
+#include <cstdint>
+
+// Anonymous namespace
+namespace {
+template<class T, typename = void>
+struct ImageMax;
+
+template<class T>
+struct ImageMax<T, std::enable_if_t<std::is_unsigned<T>::value>> {
+    static constexpr T value = std::numeric_limits<T>::max();
+};
+
+template<class T>
+struct ImageMax<T, std::enable_if_t<std::is_floating_point<T>::value>> {
+    static constexpr T value{ 1 };
+};
+
+#define BOB_GET_MAX(TYPE)          \
+    case cv::DataType<TYPE>::type: \
+        return ImageMax<TYPE>::value
+
+inline double
+getMax(int type)
+{
+    switch (type) {
+        BOB_GET_MAX(uchar);
+        BOB_GET_MAX(uint16_t);
+        BOB_GET_MAX(float);
+        BOB_GET_MAX(double);
+    default:
+        throw std::invalid_argument("Unsupported matrix type");
+    }
+}
+}
+
+namespace BoBRobotics {
+namespace ImgProc {
+void
+convertScale(const cv::Mat &in, cv::Mat &out, int targetType)
+{
+    BOB_ASSERT(in.channels() == 1);
+
+    if (in.type() == targetType) {
+        out = in;
+        return;
+    }
+
+    in.convertTo(out, targetType, getMax(targetType) / getMax(in.type()));
+}
+
+} // ImgProc
+} // BoBRobotics