Program Listing for File transform_util.h
↰ Return to documentation for file (/tmp/ws/src/marti_common/swri_transform_util/include/swri_transform_util/transform_util.h
)
// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Southwest Research Institute® (SwRI®) nor the
// names of its contributors may be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************
#ifndef TRANSFORM_UTIL_TRANSFORM_UTIL_H_
#define TRANSFORM_UTIL_TRANSFORM_UTIL_H_
#include <string>
#include <array>
#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
namespace swri_transform_util
{
tf2::Transform GetRelativeTransform(
double latitude,
double longitude,
double yaw,
double reference_latitude,
double reference_longitude,
double reference_yaw);
double GreatCircleDistance(
double src_latitude,
double src_longitude,
double dst_latitude,
double dst_longitude);
double GreatCircleDistance(const tf2::Vector3& src, const tf2::Vector3& dst);
double GetBearing(
double source_latitude,
double source_longitude,
double destination_latitude,
double destination_longitude);
void GetMidpointLatLon(
double latitude1,
double longitude1,
double latitude2,
double longitude2,
double& mid_latitude,
double& mid_longitude);
double GetHeading(double src_x, double src_y, double dst_x, double dst_y);
double ToHeading(double yaw);
double ToYaw(double heading);
tf2::Quaternion SnapToRightAngle(const tf2::Quaternion& rotation);
tf2::Vector3 GetPrimaryAxis(const tf2::Vector3& vector);
bool IsRotation(tf2::Matrix3x3 matrix);
tf2::Matrix3x3 GetUpperLeft(const std::array<double, 36>& matrix);
tf2::Matrix3x3 GetLowerRight(const std::array<double, 36>& matrix);
tf2::Matrix3x3 Get3x3Cov(const std::array<double, 9>& matrix);
void Set3x3Cov(const tf2::Matrix3x3& matrix_in,
std::array<double, 9>& matrix_out);
void SetUpperLeft(
const tf2::Matrix3x3& sub_matrix,
std::array<double, 36>& matrix);
void SetLowerRight(
const tf2::Matrix3x3& sub_matrix,
std::array<double, 36>& matrix);
double LongitudeDegreesFromMeters(
double latitude,
double altitude,
double arc_length);
double LatitudeDegreesFromMeters(
double altitude,
double arc_length);
std::string NormalizeFrameId(const std::string& frame_id);
bool FrameIdsEqual(const std::string& frame1, const std::string& frame2);
}
#endif // TRANSFORM_UTIL_TRANSFORM_UTIL_H_