wykobi_utils.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <algorithm>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include "wykobi_ext.h"
#include "wykobi_types.h"
#include <wykobi.hpp>
 
namespace armarx::wykobi
{
 
    inline pcl::PointXYZ
    fromWykobi(const wykobi::Point& ptWykobi)
    {
        pcl::PointXYZ pt;
 
        pt.x = ptWykobi.x;
        pt.y = ptWykobi.y;
        pt.z = 0.F;
 
        return pt;
    }
 
    template <typename T = float>
    void
    project(pcl::PointCloud<pcl::PointXYZ>& cloud, wykobi::Polygon polygon)
    {
        const auto toWykobi = [](const pcl::PointXYZ& pt) -> ::wykobi::point2d<T>
        { return ::wykobi::make_point(pt.x, pt.y); };
 
        // project in-place
        const auto projectPoint = [&](pcl::PointXYZ& pt)
        {
            const auto ptWykobi = toWykobi(pt);
 
            const ::wykobi::point2d<T> closestPoint =
                wykobi::ext::closest_point_on_polygon_from_point(polygon, ptWykobi);
 
            pt = fromWykobi(closestPoint);
        };
 
        std::for_each(cloud.points.begin(), cloud.points.end(), projectPoint);
    }
 
} // namespace armarx::wykobi