PerspectiveHypothesisUpdater.cpp

Go to the documentation of this file.

#include <boost/foreach.hpp>
#include "next_best_view/hypothesis_updater/impl/PerspectiveHypothesisUpdater.hpp"

#include "next_best_view/helper/MathHelper.hpp"
#include "typedef.hpp"
#include <ros/ros.h>

namespace next_best_view {
    PerspectiveHypothesisUpdater::PerspectiveHypothesisUpdater() {
        mDebugHelperPtr = DebugHelper::getInstance();
    }

    PerspectiveHypothesisUpdater::~PerspectiveHypothesisUpdater() { }

    unsigned int PerspectiveHypothesisUpdater::update(const ObjectTypeSetPtr &objectTypeSetPtr, const ViewportPoint &viewportPoint) {
        mDebugHelperPtr->writeNoticeably("STARTING UPDATE METHOD", DebugHelper::HYPOTHESIS_UPDATER);

        unsigned int counter = 0;

        mDebugHelperPtr->write(std::stringstream() << "Viewport: " << viewportPoint, DebugHelper::HYPOTHESIS_UPDATER);

        BOOST_FOREACH(int index, *viewportPoint.child_indices) {
                        ObjectPoint &objectPoint = viewportPoint.child_point_cloud->at(index);

            if (objectTypeSetPtr->find(objectPoint.type) == objectTypeSetPtr->end()) {
                continue;
            }

                        Indices::iterator begin = objectPoint.active_normal_vectors->begin();
                        Indices::iterator end = objectPoint.active_normal_vectors->end();
                        for (Indices::iterator iter = begin; iter != end;) {
                                int normalIndex = *iter;
                                SimpleVector3 normalVector = objectPoint.normal_vectors->at(normalIndex);
                SimpleVector3 objectPosition = objectPoint.getPosition();

                // rate the angle between the camera and the object normal
                float rating = mDefaultRatingModulePtr->getNormalUtility(viewportPoint, normalVector, objectPosition, mNormalAngleThreshold);

                mDebugHelperPtr->write(std::stringstream() << "Normal utility: " << rating, DebugHelper::HYPOTHESIS_UPDATER);

                                if (rating != 0.0) {
                    end--;
                    std::iter_swap(iter, end);
                    counter++;
                    continue;
                                }

                                iter++;
                        }

            std::size_t iteratorDistance = std::distance(begin, end);
            //ROS_INFO("Resizing active normal vectors from %d to %d at index %d", objectPoint.active_normal_vectors->size(), iteratorDistance, index);
            objectPoint.active_normal_vectors->resize(iteratorDistance);

                        // RGB
                        double ratio = double(objectPoint.active_normal_vectors->size()) / double(objectPoint.normal_vectors->size());

            objectPoint.color.r = ratio > .5 ? int((2.0 - 2 * ratio) * 255) : 255;
            objectPoint.color.g = ratio > .5 ? 255 : int(2.0  * ratio * 255);
            objectPoint.color.b = 0;
                }

        mDebugHelperPtr->writeNoticeably("ENDING UPDATE METHOD", DebugHelper::HYPOTHESIS_UPDATER);

        return counter;
        }

    void PerspectiveHypothesisUpdater::setDefaultRatingModule(DefaultRatingModulePtr defaultRatingModulePtr) {
                mDefaultRatingModulePtr = defaultRatingModulePtr;
        }

        DefaultRatingModulePtr PerspectiveHypothesisUpdater::getDefaultRatingModule() {
                return mDefaultRatingModulePtr;
        }

    void PerspectiveHypothesisUpdater::setNormalAngleThreshold(double angle) {
        mNormalAngleThreshold = angle;
    }

    double PerspectiveHypothesisUpdater::getNormalAngleThreshold() {
        return mNormalAngleThreshold;
    }
}