VisionUtils.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
 
#ifndef __IPA_VISIONUTILS_H__
#define __IPA_VISIONUTILS_H__

#ifdef __LINUX__
        #include "cob_vision_utils/GlobalDefines.h"
#else
        #include "cob_perception_common/cob_vision_utils/common/include/cob_vision_utils/GlobalDefines.h"
#endif

#include <opencv2/core/core.hpp>
#include <iostream>

namespace ipa_Utils {


cv::Mat vstack(const std::vector<cv::Mat> &mats);

void InitUndistortMap( const cv::Mat& A, const cv::Mat& dist_coeffs,
                                        cv::Mat& mapxarr, cv::Mat& mapyarr );

unsigned long ConvertToShowImage(const cv::Mat& source, cv::Mat& dest, int channel = 1, double min = -1, double max = -1);

unsigned long MaskImage(const cv::Mat& source, cv::Mat& dest, const cv::Mat& mask, cv::Mat& destMask, float minMaskThresh = 0,
                                                float maxMaskTresh = 20000, int sourceChannel = 1, double sourceMin = -1, double sourceMax = -1);

unsigned long EvaluatePolynomial(double x, int degree, double* coefficients, double* y);

unsigned long FilterByAmplitude(cv::Mat& xyzImage, const cv::Mat& greyImage, cv::Mat* mask, cv::Mat* maskColor, float minMaskThresh, float maxMaskThresh);

unsigned long FilterTearOffEdges(cv::Mat& xyzImage, cv::Mat* mask, float piHalfFraction = 6);

unsigned long FilterSpeckles( cv::Mat& img, int maxSpeckleSize, double _maxDiff, cv::Mat& _buf );

cv::Vec3b GrayColorMap(double value, double min, double max);

cv::Mat GetColorcoded(const cv::Mat& img_32F);

cv::Mat GetColorcoded(const cv::Mat& img_32F, double min, double max);

unsigned long SaveMat(cv::Mat& mat, std::string filename, int type=CV_32F);

unsigned long LoadMat(cv::Mat& mat, std::string filename, int type=CV_32F);


template <typename Distance>
void ClusteringKMeanspp(int k, cv::Mat& dataset, int* indices, int indices_length, int* centers, int& centers_length, int numLocalTries=1)
{
        typedef typename Distance::ElementType ElementType;
        typedef typename Distance::ResultType DistanceType;

        Distance distance = Distance();

        int n = indices_length;

        double currentPot = 0;
        DistanceType* closestDistSq = new DistanceType[n];

        // Choose one random center and set the closestDistSq values
        //int index = cv::flann::rand_int(n);
        int index = rand() % n;
        assert(index >=0 && index < n);
        centers[0] = indices[index];

        for (int i = 0; i < n; i++) {
                closestDistSq[i] = distance(dataset.ptr(indices[i]), 
                        dataset.ptr(indices[index]), dataset.cols);
                currentPot += closestDistSq[i];
        }

        // Choose each center
        int centerCount;
        for (centerCount = 1; centerCount < k; centerCount++) {

                // Repeat several trials
                double bestNewPot = -1;
                int bestNewIndex = 0;
                for (int localTrial = 0; localTrial < numLocalTries; localTrial++) {

                        // Choose our center - have to be slightly careful to return a valid answer even accounting
                        // for possible rounding errors
                        //double randVal = cv::flann::rand_double(currentPot);
                        double randVal = (double)rand() / RAND_MAX;
                        double fMin = 0;
                        double fMax = currentPot;
                        randVal = fMin + randVal * (fMax - fMin);
                        for (index = 0; index < n-1; index++) {
                                if (randVal <= closestDistSq[index]) break;
                                else randVal -= closestDistSq[index];
                        }

                        // Compute the new potential
                        double newPot = 0;
                        for (int i = 0; i < n; i++) 
                                newPot += std::min( distance(dataset.ptr(indices[i]), 
                                        dataset.ptr(indices[index]), dataset.cols), closestDistSq[i] );

                        // Store the best result
                        if ((bestNewPot < 0)||(newPot < bestNewPot)) {
                                bestNewPot = newPot;
                                bestNewIndex = index;
                        }
                }

                // Add the appropriate center
                centers[centerCount] = indices[bestNewIndex];
                currentPot = bestNewPot;
                for (int i = 0; i < n; i++) 
                        closestDistSq[i] = std::min( distance(dataset.ptr(indices[i]),
                                dataset.ptr(indices[bestNewIndex]), dataset.cols), closestDistSq[i] );
        }

        centers_length = centerCount;

        delete[] closestDistSq;
}

// Generator that yields an increasing sequence of integers
class UniqueNumber {
public:
  int current;
  UniqueNumber();
  int operator()();
};

// Returns random subset of input vector. Will not create duplicates.
// @param v Input vector
// @param n Number of items to return
// @return Vector of same type as input vector (length n)
template<class T> std::vector<T> takeRandomN(const std::vector<T> &v, int n)
{
        int current = 0;
        std::vector<int> indices(v.size());
        std::generate(indices.begin(), indices.end(), UniqueNumber());
        std::random_shuffle(indices.begin(), indices.end());
        
        std::vector<T> ret;
        for (int i = 0; i < n; i++)
        {
                ret.push_back(v[indices[i]]);
        }

        return ret;
}

} // end namespace __IPA_VISIONUTILS_H__


#endif