tools.cpp

Go to the documentation of this file.

/*
 * tools.cpp
 *
 *  Created on: 13.07.2011
 *      Author: hess
 */

#include <coverage_3d_tools/tools.h>

#include <opencv2/ml/ml.hpp>
#include <opencv2/highgui/highgui.hpp>


float mahalanobis(const cv::Mat& v1, const cv::Mat& v2, const cv::Mat& invCov) {

        cv::Mat_<float>& ptr_v1 = (cv::Mat_<float>&) v1;
        cv::Mat_<float>& ptr_v2 = (cv::Mat_<float>&) v2;
        cv::Mat_<float>& ptr_cov = (cv::Mat_<float>&) invCov;

        float diff0 = 0;
        float diff1 = 0;
        float diff2 = 0;

        //take the difference
        for (int i = 0; i < 3; ++i) {
                diff0 = ptr_v1(0, 0) - ptr_v2(0, 0);
                diff1 = ptr_v1(0, 1) - ptr_v2(0, 1);
                diff2 = ptr_v1(0, 2) - ptr_v2(0, 2);
        }

        float tmp0 = 0;
        float tmp1 = 0;
        float tmp2 = 0;

        //left-multiply
        for (int i = 0; i < 3; ++i) {
                tmp0 += diff0 * ptr_cov(0, i);
                tmp1 += diff1 * ptr_cov(1, i);
                tmp2 += diff2 * ptr_cov(2, i);
                //        cout << ptr
        }

        //right-multiply
        float result = 0;
        for (int i = 0; i < 3; ++i) {
                result += tmp0 * diff0;
                result += tmp1 * diff1;
                result += tmp2 * diff2;
        }

        return result;
}

void computeCornersFromCell(const tf::Point center, const tf::Point v1,
                const tf::Point v2, tf::Point* rect_points) {

        tf::Point curr_corner;
        rect_points[0] = center - v1 - v2;
        rect_points[1] = center + v1 - v2;
        rect_points[2] = center + v1 + v2;
        rect_points[3] = center - v1 + v2;
}

void computeCornersFromCell(const tf::Point center, const tf::Point v1,
                const tf::Point v2, std::vector<tf::Point>& rect_points) {

        tf::Point curr_corner;
        rect_points.clear();
        rect_points.resize(4);
        rect_points[0] = center - v1 - v2;
        rect_points[1] = center + v1 - v2;
        rect_points[2] = center + v1 + v2;
        rect_points[3] = center - v1 + v2;
}

void computeRectPixel(const IplImage *ipl_image,
                const std::vector<cv::Point>& points, std::vector<cv::Point>& pixel) {

        std::map<int, std::map<int, bool> > rect;

        //iterate over all four side of the rectangle
        for (int i = 0; i < 4; i++) {

                //compute all points belonging to line
                CvLineIterator iterator;
                int pixel_count = cvInitLineIterator(ipl_image, points[i],
                                points[(i + 1) % 4], &iterator, 8, 0);
                //          ROS_INFO("pixel_count %i", pixel_count);
                for (int j = 0; j < pixel_count; j++) {
                        int x = (iterator.ptr - (uchar*) ipl_image->imageData)
                                        / ipl_image->widthStep;
                        int y = ((iterator.ptr - (uchar*) ipl_image->imageData)
                                        % ipl_image->widthStep) / ipl_image->nChannels;
                        rect[x][y] = true;

                        CV_NEXT_LINE_POINT(iterator);
                }
        }

        //collect all points belonging to the rectangle
        std::map<int, std::map<int, bool> >::iterator rect_it;
        pixel.clear();
        for (rect_it = rect.begin(); rect_it != rect.end(); rect_it++) {
                int x = rect_it->first;
                int y1 = rect_it->second.begin()->first;
                int y2 = rect_it->second.rbegin()->first;
                for (int y = y1; y <= y2; y++) {

                        //in case of wrong table detection: only push pixels inside the image
                        if ((x < ipl_image->height) && (y < ipl_image->width) && (x >= 0)
                                        && (y >= 0))
                                pixel.push_back(cv::Point(x, y)); //TODO: check: that is really weird c and c++ different representation of x and y?
                }
        }
}

void computeRectPixel(const cv::Mat& image,
                const std::vector<cv::Point>& points, std::vector<cv::Point>& pixel) {

        std::map<int, std::map<int, bool> > rect;

        //iterate over all four sides of the rectangle
        for (int i = 0; i < 4; i++) {
                //compute all points belonging to line
//              std::cout << "point " << i << " " << points[i].x << " " << points[i].y
//                              << std::endl;
                cv::LineIterator it(image, points[i], points[(i + 1) % 4], 8, 0);

                for (int j = 0; j < it.count; j++, ++it) {

                        rect[it.pos().x][it.pos().y] = true;
                }
                //      cvLine(image, points[i], points[(i + 1) % 4], color, 1, 8);
        }

        //collect all points belonging to the rectangle
        std::map<int, std::map<int, bool> >::iterator rect_it;
        pixel.clear();
        for (rect_it = rect.begin(); rect_it != rect.end(); rect_it++) {
                int x = rect_it->first;
                int y1 = rect_it->second.begin()->first;
                int y2 = rect_it->second.rbegin()->first;
                for (int y = y1; y <= y2; y++) {

                        //in case of wrong table detection: only push pixels inside the image
                        if ((x < image.rows) && (y < image.cols) && (x >= 0) && (y >= 0))
                                pixel.push_back(cv::Point(y, x)); //TODO: check: that is really weird c and c++ different representation of x and y?
                }
        }
}

bool linePlaneIntersection(const cv::Point3d& plane_normal,
                const cv::Point3d& point_in_plane, const cv::Point3d& line_unit_vector,
                const cv::Point3d& line_origin, cv::Point3d& intersection) {

        cv::Point3d tmp = (point_in_plane - line_origin);
        double numerator = tmp.dot(plane_normal);
        double denominator = line_unit_vector.dot(plane_normal);

        //if the line intersects the plane:
        if (std::fabs(numerator) > 0.00001 && std::fabs(denominator) > 0.00001) {

                double distance_from_origin = numerator / denominator;
                intersection = distance_from_origin * line_unit_vector;
                return true;
        } else { //otherwise it is parallel or inside the plane, but we don't check the case
                return false;
        }
}

bool getTransform(const std::string& source, const std::string& target, tf::TransformListener& listener, tf::StampedTransform& transform){
        bool received =false;
        int iter=0;
        while (!received && iter < 50) {
                iter++;
                ROS_DEBUG_STREAM("Looking up transform from " << source << " to " << target);
                ros::Time lookup_time = ros::Time::now();
                try {
                        ros::Duration timeout(0.1);
                        listener.waitForTransform(source,
                                        target, lookup_time, timeout);
                        listener.lookupTransform(source,
                                        target, lookup_time, transform);
                        received = true;
                } catch (tf::LookupException& e) {
                        ROS_WARN_STREAM(e.what());
                        ROS_INFO("Continue waiting for transform.");
                } catch (tf::ConnectivityException& e) {
                        ROS_WARN_STREAM("coverage::tools::getTransform: " << e.what());
                } catch (std::exception& e) {
                        ROS_WARN_STREAM("other execption: " << e.what());
                }
                ros::Rate r(10);
                r.sleep();
        }
        if (received){
                return true;
        } else {
                return false;
        }
}

bool getTransform(const std::string& source, const std::string& target, tf::StampedTransform& transform){
        tf::TransformListener listener;
        return (getTransform(source, target, listener, transform));
}