Initial Matrix Follow Commit

src/aadcUser/LITD_MatrixFollower/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.10.0)
+
+project(matrixfollower_filter)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+set (SOURCEFILES    	stdafx.h
+                        FineLocator.h
+                        FineLocator.cpp
+						MatrixFollower.h
+						MatrixFollower.cpp)
+
+adtf_add_filter(${PROJECT_NAME}
+				${SOURCEFILES}
+)
+
+# Specify where it should be installed to
+adtf_install_target(${PROJECT_NAME} bin)
+
+#install dlls for opencv to binary folder 
+if(WIN32)
+    install(FILES "${OpenCV_DIR}/${OpenCV_ARCH}/${OpenCV_RUNTIME}/bin/opencv_world${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}d.dll" DESTINATION ${CMAKE_INSTALL_BINARY}/debug CONFIGURATIONS Debug)
+    install(FILES "${OpenCV_DIR}/${OpenCV_ARCH}/${OpenCV_RUNTIME}/bin/opencv_world${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll" DESTINATION ${CMAKE_INSTALL_BINARY} CONFIGURATIONS RelWithDebInfo Release)
+endif(WIN32)
+
+adtf_create_plugindescription(
+    TARGET
+        ${PROJECT_NAME}
+    PLUGIN_SUBDIR
+        "bin"
+ ) 
+
+target_link_libraries(${PROJECT_NAME} LINK_PUBLIC ${OpenCV_LIBS})
+
+#this is to add a folder within the ADTF solution
+set_property(TARGET ${PROJECT_NAME} PROPERTY FOLDER user)
+set_property(TARGET ${PROJECT_NAME}_pdgen PROPERTY FOLDER user)

src/aadcUser/LITD_MatrixFollower/FineLocator.cpp

@@ -0,0 +1,47 @@
+//
+// Created by Stefan on 23.08.2018.
+//
+
+#include "FineLocator.h"
+
+FineLocator::FineLocator(char *pathToScaledMap) {
+    scaledMap = imread(pathToScaledMap);
+}
+
+FineLocator::~FineLocator() {
+    scaledMap.release();
+}
+
+Point2i FineLocator::localize(Mat img_bv, float theta, Point2i pos, int size) {
+    // left upper corner of map -> car location
+    Mat car_coord_shift = Mat::eye(3,3, CV_32F);
+    car_coord_shift.at<float>(0, 2) = -pos.x;
+    car_coord_shift.at<float>(1, 2) = -pos.y;
+    // map rotation -> car rotation
+    Mat car_rot = Mat::eye(3,3, CV_32F);
+    Mat rot = getRotationMatrix2D(Point2f(0, 0), -theta, 1.0f);
+    rot.copyTo(car_rot(Rect_<int>(0,0,1,2)));
+    // car location -> picture location
+    Mat offset = Mat::eye(3,3, CV_32F);
+    offset.at<float>(0, 2) += img_bv.size[0]/2.f + size/2.f;
+    offset.at<float>(1, 2) += img_bv.size[1] + size/2.f;  //TODO check if size[1] == 192 etc.
+    // combine in reverse order
+    Mat combined = offset*car_rot*car_coord_shift;
+    combined = combined(Rect_<int>(0,0,1,2)); // only select the Affine Part of the Transformation
+    Mat search_space;
+    warpAffine(scaledMap, search_space ,combined, Size(img_bv.size[0] + size, img_bv.size[1] + size), INTER_LINEAR, BORDER_REPLICATE);
+    Mat search_result;
+    matchTemplate(search_space, img_bv, search_result, TM_CCOEFF_NORMED);
+    double mi, ma;
+    Point mil, mal;
+    minMaxLoc(search_result, &mi, &ma, &mil, &mal);
+    Mat reverse;
+    invertAffineTransform(combined, reverse);
+    Mat location_global = reverse*Mat(Vec3f(mal.x + img_bv.size[0]/2.f, mal.y + img_bv.size[1], 1));
+    return Point2i(location_global.at<float>(0,0),location_global.at<float>(0,1));
+
+}
+
+float rad2grad(float x){
+    return (float)(x*M_PI/180.0f);
+}

src/aadcUser/LITD_MatrixFollower/FineLocator.h

@@ -0,0 +1,29 @@
+//
+// Created by Stefan on 23.08.2018.
+//
+
+#ifndef AADC_USER_FINELOCATOR_H
+#include <opencv2/opencv.hpp>
+#include <math.h>
+#define AADC_USER_FINELOCATOR_H
+
+using namespace cv;
+
+class FineLocator {
+private:
+    /*! The scaled image of the Map*/
+    Mat scaledMap;
+
+public:
+    /*! Constructor*/
+    FineLocator(char* pathToScaledMap);
+
+    /*! Destructor*/
+    ~FineLocator();
+
+    /*! call localisation*/
+    Point2i localize(Mat img_bv, float theta, Point2i pos, int size=20);
+};
+
+
+#endif //AADC_USER_FINELOCATOR_H

src/aadcUser/LITD_MatrixFollower/MatrixFollower.cpp

@@ -0,0 +1,121 @@
+/*********************************************************************
+Copyright (c) 2018
+Audi Autonomous Driving Cup. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+3.  All advertising materials mentioning features or use of this software must display the following acknowledgement: ?This product includes software developed by the Audi AG and its contributors for Audi Autonomous Driving Cup.?
+4.  Neither the name of Audi nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL AUDI AG OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+**********************************************************************/
+
+#include "stdafx.h"
+#include "MatrixFollower.h"
+#include "ADTF3_OpenCV_helper.h"
+
+
+ADTF_TRIGGER_FUNCTION_FILTER_PLUGIN(CID_COPENCVTEMPLATE_DATA_TRIGGERED_FILTER,
+                                    "MatrixFollower_cf",
+                                    cMatrixFollower,
+                                    adtf::filter::pin_trigger({ "input" }));
+
+cMatrixFollower::cMatrixFollower()
+{
+
+    //create and set inital input format type
+    m_sImageFormat.m_strFormatName = ADTF_IMAGE_FORMAT(RGB_24);
+    const adtf::ucom::object_ptr<IStreamType> pType = adtf::ucom::make_object_ptr<cStreamType>(stream_meta_type_image());
+    set_stream_type_image_format(*pType, m_sImageFormat);
+
+    //Register input pin
+    Register(m_oReader, "i_BirdsEyeImg", pType);
+
+    //Register output pins
+    Register(m_oSpeedWriter, "o_Speed", pType);
+    Register(m_oSteeringWriter, "o_Steering", pType);
+
+    //register callback for type changes
+    m_oReader.SetAcceptTypeCallback([this](const adtf::ucom::ant::iobject_ptr<const adtf::streaming::ant::IStreamType>& pType) -> tResult
+    {
+        return ChangeType(m_oReader, m_sImageFormat, *pType.Get());
+    });
+
+    // generating bitmask path
+    m_Path = imread(PATH_TO_PATH);
+
+}
+
+tResult cMatrixFollower::Configure()
+{
+    //get clock object
+    RETURN_IF_FAILED(_runtime->GetObject(m_pClock));
+
+    RETURN_NOERROR;
+}
+
+tResult cMatrixFollower::Process(tTimeStamp tmTimeOfTrigger)
+{
+    object_ptr<const ISample> pReadSample;
+
+    while (IS_OK(m_oReader.GetNextSample(pReadSample)))
+    {
+        object_ptr_shared_locked<const ISampleBuffer> pReadBuffer;
+        //lock read buffer
+        if (IS_OK(pReadSample->Lock(pReadBuffer)))
+        {
+            //create a opencv matrix from the media sample buffer
+            Mat bvImage = Mat(cv::Size(m_sImageFormat.m_ui32Width, m_sImageFormat.m_ui32Height),
+                                   CV_8UC3, const_cast<unsigned char*>(static_cast<const unsigned char*>(pReadBuffer->GetPtr())));
+
+            //Do the localization
+            Point2i newLoc = m_locator.localize(bvImage, Point2i(x + int(vel*sin(heading)), y +int(vel*cos(heading))), SEARCH_SPACE_SIZE);
+            int x_new = newLoc.x ,y_new = newLoc.y;
+
+            // Estimate new Velocity and Heading
+            int x_diff = x_new - x;
+            int y_diff = y_new - y;
+
+            vel = sqrt(x_diff*x_diff + y_diff*y_diff);
+            heading += atan2(y_diff, x_diff);
+            //Set new position
+            x = x_new;
+            y = y_new;
+
+            //cutout new steering information
+            Mat car_coord_shift = Mat::eye(3,3, CV_32F);
+            car_coord_shift.at<float>(0, 2) = -x;
+            car_coord_shift.at<float>(1, 2) = -y;
+            // map rotation -> car rotation
+            Mat car_rot = Mat::eye(3,3, CV_32F);
+            Mat rot = getRotationMatrix2D(Point2f(0, 0), -heading, 1.0f);
+            rot.copyTo(car_rot(Rect_<int>(0,0,1,2)));
+            // car location -> picture location
+            Mat offset = Mat::eye(3,3, CV_32F);
+            offset.at<float>(0, 2) += CUTOUT_X / 2.f;
+            offset.at<float>(1, 2) += CUTOUT_Y;
+            Mat combined = offset*car_rot*car_coord_shift;
+            combined = combined(Rect_<int>(0,0,1,2)); // only select the Affine Part of the Transformation
+            //CutOut path for steering
+            Mat m_PathCutOut;
+            warpAffine(m_Path, m_PathCutOut ,combined, Size(CUTOUT_X, CUTOUT_Y), INTER_LINEAR, BORDER_REPLICATE);
+            // Convert to binary
+            Mat m_PathBitMask;
+            threshold(m_PathCutOut, m_PathBitMask, 1, 1, THRESH_BINARY_INV);
+
+            vector<int> mult_vec;
+            for (int i = 0; i < CUTOUT_X; ++i) mult_vec.push_back(i-CUTOUT_X/2);
+
+            Mat mult_mat = Mat(mult_vec);
+
+            double value = sum(m_PathCut_Out*mult_mat);
+            LOG_INFO(value);
+
+        }
+    }
+
+    RETURN_NOERROR;
+}

src/aadcUser/LITD_MatrixFollower/MatrixFollower.h

@@ -0,0 +1,93 @@
+/*********************************************************************
+Copyright (c) 2018
+Audi Autonomous Driving Cup. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+3.  All advertising materials mentioning features or use of this software must display the following acknowledgement: ?This product includes software developed by the Audi AG and its contributors for Audi Autonomous Driving Cup.?
+4.  Neither the name of Audi nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL AUDI AG OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+**********************************************************************/
+
+
+#pragma once
+
+//*************************************************************************************************
+#define CID_COPENCVTEMPLATE_DATA_TRIGGERED_FILTER "litd_matrixfollower.filter.user.aadc.cid"
+#define PATH_TO_MAP "~/share/adtf/data/scaledMap.png"
+#define PATH_TO_PATH "~/share/adtf/data/scaledMapssw.png"
+#define SEARCH_SPACE_SIZE 30
+#define CUTOUT_X 50
+#define CUTOUT_Y 20
+
+using namespace adtf_util;
+using namespace ddl;
+using namespace adtf::ucom;
+using namespace adtf::base;
+using namespace adtf::streaming;
+using namespace adtf::mediadescription;
+using namespace adtf::filter;
+using namespace std;
+using namespace cv;
+
+
+/*! the main class of the open cv template. */
+class cMatrixFollower : public cTriggerFunction
+{
+private:
+
+    //Pins
+    /*! Reader of an InPin. */
+    cPinReader m_oReader;
+    /*! Writer to an OutPin. */
+    cPinWriter m_oSteeringWriter;
+    cPinWriter m_oSpeedWriter;
+
+    //Stream Formats
+        /*! The input format */
+    adtf::streaming::tStreamImageFormat m_sImageFormat;
+
+    /*! The clock */
+    object_ptr<adtf::services::IReferenceClock> m_pClock;
+
+    //Localization
+    FineLocator m_locator = FineLocator(PATH_TO_MAP);
+
+    //Path
+    Mat m_Path;
+
+    //Positioning
+    int x = 515, y = 583;
+    double heading = 0.0;
+    double vel = 0.0 ;
+
+public:
+
+    /*! Default constructor. */
+    cMatrixFollower();
+
+
+    /*! Destructor. */
+    virtual ~cMatrixFollower() = default;
+
+    /**
+    * Overwrites the Configure
+    * This is to Read Properties prepare your Trigger Function
+    */
+    tResult Configure() override;
+    /**
+    * Overwrites the Process
+    * You need to implement the Reading and Writing of Samples within this function
+    * MIND: Do Reading until the Readers queues are empty or use the IPinReader::GetLastSample()
+    * This FUnction will be called if the Run() of the TriggerFunction was called.
+    */
+    tResult Process(tTimeStamp tmTimeOfTrigger) override;
+
+};
+
+
+//*************************************************************************************************

src/aadcUser/LITD_MatrixFollower/stdafx.h

@@ -0,0 +1,37 @@
+#pragma once
+/**********************************************************************
+Copyright (c)
+Audi Autonomous Driving Cup. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+3.  All advertising materials mentioning features or use of this software must display the following acknowledgement: “This product includes software developed by the Audi AG and its contributors for Audi Autonomous Driving Cup.”
+4.  Neither the name of Audi nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL AUDI AG OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+**********************************************************************
+* $Author:: hart#$  $Date:: 2017-05-19 08:12:10#$ $Rev:: 63515   $
+**********************************************************************/
+
+
+
+#ifdef WIN32
+#include <windows.h>
+#endif
+
+//adtf
+#include <adtf3.h>
+#include <adtf_platform_inc.h>
+#include <a_utils_platform_inc.h>
+#include <adtf_systemsdk.h>
+
+#include <opencv2/opencv.hpp>
+#include <stdlib.h>
+#include <math.h>
+
+using namespace std;
+using namespace cv;