icp_2d.h

Go to the documentation of this file.

/*
 * Copyright 2015 Fetch Robotics Inc.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef FETCH_AUTO_DOCK_ICP_2D_H
#define FETCH_AUTO_DOCK_ICP_2D_H

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Transform.h>

#include <vector>

namespace icp_2d
{

double thetaFromQuaternion(const geometry_msgs::Quaternion& q);

std::vector<geometry_msgs::Point>
transform(const std::vector<geometry_msgs::Point>& points,
          double x,
          double y,
          double theta);

geometry_msgs::Point
getCentroid(const std::vector<geometry_msgs::Point> points);

bool alignPCA(const std::vector<geometry_msgs::Point> source,
              const std::vector<geometry_msgs::Point> target,
              geometry_msgs::Transform & transform);

bool alignSVD(const std::vector<geometry_msgs::Point> source,
              const std::vector<geometry_msgs::Point> target,
              geometry_msgs::Transform & transform);

double alignICP(const std::vector<geometry_msgs::Point> source,
                const std::vector<geometry_msgs::Point> target,
                geometry_msgs::Transform & transform,
                size_t max_iterations = 10,
                double min_delta_rmsd = 0.000001);

}  // namespace icp_2d

#endif  // FETCH_AUTO_DOCK_ICP_2D_H