物理轨迹与图像轨迹之间的转换

物理坐标系下转换为图像坐标系

/*
 * @Author: philtell [email protected]
 * @LastEditors: philtell [email protected]
 * @FilePath: 
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#ifndef NAV2_MAP_SERVER__MAP_2_MAT_HPP_
#define NAV2_MAP_SERVER__MAP_2_MAT_HPP_

#include <opencv2/core.hpp>
#include <string>
#include <vector>

namespace map_convert
{
    
    
class MapUtils
{
    
    
public:
  MapUtils();
  MapUtils(cv::Point2f origin,float resolution,float height,float width);
  void map2Mat(const cv::Point2f pt1, cv::Point2f & pt2);
private:
  std::string reference_frame_; // 参考坐标系
  std::string map_frame_; // 地图坐标系
  std::string image_frame_; // 图像坐标系
  cv::Point2f origin_; // 原点
  float resolution_; //分辨率
  float height_; // 图像高度
  float width_; // 图像宽度
  cv::Point2f translateMatix_; // 偏移
};
}  // namespace map_convert

#endif  // NAV2_MAP_SERVER__MAP_2_MAT_HPP_

/*
 * @Author: philtell [email protected]
 * @LastEditors: philtell [email protected]
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "nav2_map_server/map2mat.hpp"
#include <rclcpp/logging.hpp>
#include <iostream>


namespace map_convert
{
    
    
    void MapUtils::map2Mat(const cv::Point2f pt1, cv::Point2f & pt2)
    {
    
    
        float grid_orign_x = this->origin_.x;
        float grid_orign_y = this->origin_.y;
        float grid_height = this->height_;
        // float grid_width = this->width_;
        float res = this->resolution_;
        pt2.x = (pt1.x - grid_orign_x) / res;
        pt2.y = grid_height - (pt1.y  - grid_orign_y) / res;
    }

    MapUtils::MapUtils(cv::Point2f origin,float resolution,float height,float width)
    {
    
    
        this->origin_ = origin;
        this->resolution_ = resolution;
        this->height_ = height;
        this->width_ = width;

        cv::Point2f reference_point_; // 像素坐标系下的参考系坐标点
        reference_point_.x = static_cast<int>(this->width_/2);
        reference_point_.y = static_cast<int>(this->height_/2);

        cv::Point2f map_point_; // 像素坐标系下地图点。

        translateMatix_.x = map_point_.x = reference_point_.x - ((this->origin_.x)/this->resolution_);
        translateMatix_.y = map_point_.y = reference_point_.y - ((this->origin_.y)/this->resolution_);
    }
}