model_visualizer_rviz.cpp

Go to the documentation of this file.

//Local include
#include <asr_ism_visualizations/model_visualizer_rviz.hpp>

//ISM includes
#include <ISM/utility/GeometryHelper.hpp>

//#define USE_LINES
//#define VOTE_DIFFERENCE

namespace VIZ
{
void ModelVisualizerRVIZ::addVisualization(const std::string pattern_name, const ISM::TracksPtr tracks, const ISM::TracksPtr ism_object_tracks,
                                           const std::map<int, std::vector<ISM::VoteSpecifierPtr>> track_index_to_votes)
{
    initObjectToColorMap(tracks, ism_object_tracks);
    ISM::TracksPtr overall_tracks = ISM::TracksPtr(new ISM::Tracks(tracks->tracks));
    overall_tracks->tracks.insert(overall_tracks->tracks.end(), ism_object_tracks->tracks.begin(), ism_object_tracks->tracks.end());


    visualization_msgs::MarkerArray current_marker_array = generateModelMarkers(pattern_name, overall_tracks, track_index_to_votes);

    addMarker(current_marker_array);
    publishCollectedMarkers();
}

visualization_msgs::MarkerArray ModelVisualizerRVIZ::generateModelMarkers(std::string pattern_name, const ISM::TracksPtr tracks, const std::map<int, std::vector<ISM::VoteSpecifierPtr>> track_index_to_votes)
{
    visualization_msgs::MarkerArray return_markers;

    for (const std::pair<int, std::vector<ISM::VoteSpecifierPtr>>& votes : track_index_to_votes)
    {
        int index = votes.first;
        if (index < 0)
        {
            continue;
        }

        /* find reference */
        ISM::ObjectPtr ref;
        for (ISM::VoteSpecifierPtr vote : votes.second)
        {
            if (ISM::GeometryHelper::isSelfVote(vote))
            {
                ISM::TrackPtr track = tracks->getTrackByTypeAndId(vote->objectType, vote->observedId);
                if (track != nullptr)
                {
                    if(static_cast<size_t>(index) >= track->objects.size())
                    {
                        std::cout << "Index: " << index << "exceeds objects size" << std::endl;
                        continue;
                    }
                    ref = track->objects[index];
                }

                break;
            }
        }

        if (ref == nullptr)
        {
            std::cout << "no reference for " << pattern_name
                      << " at the track-index " << votes.first << "\n";
            continue;
        }

        /* find corresponding vote/object pair and generate marker */
        for (const ISM::VoteSpecifierPtr& vote : votes.second)
        {
            ISM::ObjectPtr obj;
            ISM::TrackPtr track = tracks->getTrackByTypeAndId(vote->objectType, vote->observedId);
            if (track == nullptr)
            {
                continue;
            }

            if(static_cast<size_t>(index) >= track->objects.size())
            {
                std::cout << "Index: " << index << "exceeds objects size" << std::endl;
                continue;
            }

            obj = track->objects[index];
            if (ref == nullptr)
            {
                continue;
            }

            ISM::PosePtr expected_object_pose = ISM::GeometryHelper::getSourcePose(ref->pose, ISM::GeometryHelper::getSourcePoint(ref->pose, vote->refToObjectQuat, vote->radius), vote->refToObjectPoseQuat);

            if (ISM::GeometryHelper::poseEqual(expected_object_pose, obj->pose))
            {
                #ifdef USE_LINES
                    return_markers.markers.push_back(VizHelperRVIZ::createLine(obj->pose->point, ref->pose->point, 0.002, index, base_frame_, prefix_ + pattern_name + "_vote_from_" + obj->type + "_" + obj->observedId,
                                                                               object_to_color_map_[obj->type][obj->observedId], marker_lifetime_));
                #else
                    return_markers.markers.push_back(VizHelperRVIZ::createArrowMarkerToPoint(obj->pose->point, ref->pose->point, base_frame_, prefix_ + pattern_name + "_vote_from_" + obj->type + "_" + obj->observedId,
                                                                                             index, 0.002, 0.007, 0.015, object_to_color_map_[obj->type][obj->observedId], marker_lifetime_));
                #endif

            }
            else
            {
                std::cout << "Vote from: " << vote->objectType  << " with ID: " << vote->observedId
                          << " at track-index: " << index << " couldn't be drawn!\n";

                #ifdef VOTE_DIFFERENCE
                    double position_difference = ISM::GeometryHelper::getDistanceBetweenPoints(expected_object_pose->point, obj->pose->point);
                    double orientation_difference = ISM::GeometryHelper::getAngleBetweenQuats(expected_object_pose->quat, obj->pose->quat);
                    std::cout << "VOTE_DEBUG_INFO  -position deviation: " << position_difference
                              << "   -angle deviation: " << orientation_difference << std::endl << std::endl;

                #endif
            }
        }
    }

    return return_markers;
}

void ModelVisualizerRVIZ::initObjectToColorMap(ISM::TracksPtr tracks, ISM::TracksPtr ism_object_tracks)
{
    double value = 0.6;
    double color_step_size = 240.0 / (tracks->tracks.size() + 1);
    for (size_t i = 0; i < tracks->tracks.size(); i++)
    {
        object_to_color_map_[tracks->tracks[i]->type][tracks->tracks[i]->observedId] = VizHelperRVIZ::hsvToRGBA(color_step_size * (i + 1), 1.0, value);
    }

    color_step_size = 120.0 / (ism_object_tracks->tracks.size());
    for (size_t i = 0; i < ism_object_tracks->tracks.size(); i++)
    {
        object_to_color_map_[ism_object_tracks->tracks[i]->type][ism_object_tracks->tracks[i]->observedId] = VizHelperRVIZ::hsvToRGBA((color_step_size * i) + 240, 1.0, value);
    }
}
}