RoboticsAcademy/exercises/laser_mapping/cpp_lib/src/Map.cpp at 8187e06247dd9ab5bb6947276300c3680e65c04f · JdeRobot/RoboticsAcademy · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#include "Map.hpp"
#include <cmath>

Map::Map(std::function<Pose3d()> pose_getter, std::function<Pose3d()> noisy_pose_getter)
    : pose_getter_(pose_getter), noisy_pose_getter_(noisy_pose_getter)
{
}

cv::Mat Map::rtx(double angle, double tx, double ty, double tz)
{
    return (cv::Mat_<double>(4, 4) <<
        1, 0, 0, tx,
        0, std::cos(angle), -std::sin(angle), ty,
        0, std::sin(angle), std::cos(angle), tz,
        0, 0, 0, 1);
}

cv::Mat Map::rty(double angle, double tx, double ty, double tz)
{
    return (cv::Mat_<double>(4, 4) <<
        std::cos(angle), 0, std::sin(angle), tx,
        0, 1, 0, ty,
        -std::sin(angle), 0, std::cos(angle), tz,
        0, 0, 0, 1);
}

cv::Mat Map::rtz(double angle, double tx, double ty, double tz)
{
    return (cv::Mat_<double>(4, 4) <<
        std::cos(angle), -std::sin(angle), 0, tx,
        std::sin(angle), std::cos(angle), 0, ty,
        0, 0, 1, tz,
        0, 0, 0, 1);
}

cv::Mat Map::rt_vacuum()
{
    return rtz(CV_PI / 2.0, 50, 70, 0);
}

std::tuple<double, double, double> Map::get_robot_coordinates()
{
    Pose3d pose = pose_getter_();

    double y = -23.53 * (-31.95 - pose.y);
    double x = -23.58 * (-20.36 - pose.x);

    return {x, y, pose.yaw};
}

std::tuple<double, double, double> Map::get_robot_coordinates_with_noise()
{
    Pose3d pose = noisy_pose_getter_();

    double y = -23.53 * (-31.95 - pose.y);
    double x = -23.58 * (-20.36 - pose.x);

    return {x, y, pose.yaw};
}

void Map::reset()
{
}