-
Notifications
You must be signed in to change notification settings - Fork 0
/
Bridge.cpp
68 lines (60 loc) · 2.81 KB
/
Bridge.cpp
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
64
65
66
67
68
#include "Bridge.h"
Position Bridge::convertToPosition(const std::tuple<float, float, float> pos,
const VisionSensor::Resolution &resolution) {
return Position{(std::get<0>(pos) + 2.5f) * resolution.height / 5.0f, (std::get<1>(pos) + 2.5f) * resolution.width / 5.0f};
}
std::tuple<float, float, float> Bridge::convertToVREPCoordinate(const Position &pos,
const VisionSensor::Resolution &resolution) {
float x = (pos.X() * 5.0f) / resolution.height - 2.5f;
float y = (pos.Y() * 5.0f) / resolution.width - 2.5f;
float z = 0;
std::cout << x << " " << y << " " << z << " " << std::endl;
return std::make_tuple(x, y, z);
}
std::vector<std::tuple<float, float, float> > Bridge::convertToVREPPath(const CoordinateList_sptr &pathCoordList,
const VisionSensor::Resolution &resolution) {
std::vector<std::tuple<float, float, float> > vrepPath;
for (auto it = pathCoordList->cbegin(); it != pathCoordList->cend(); ++it) {
vrepPath.push_back(convertToVREPCoordinate(*it, resolution));
}
return vrepPath;
}
void Bridge::coverObject(cv::Mat &ground, const Position &pos, const unsigned int radius, const cv::Scalar &color) {
cv::circle(ground, cv::Point(pos.X(), pos.Y()), radius, color, -1);
}
void Bridge::enlargeBorder(cv::Mat& image, const unsigned int kernel) {
cv::dilate(image, image, 255 * cv::getStructuringElement(cv::MORPH_RECT, cv::Size(kernel, kernel)));
}
boost::numeric::ublas::matrix<STATE> Bridge::createGroundMap(const cv::Mat &ground) {
cv::Size size = ground.size();
boost::numeric::ublas::matrix<STATE> groundMap;
groundMap.resize(size.height, size.width);
for (unsigned int i = 0; i < size.height; ++i) {
for (unsigned int j = 0; j < size.width; ++j) {
if (ground.at<uchar>(j, i) == 0) {
groundMap(i, j) = EMPTY;
} else {
groundMap(i, j) = WALL;
}
}
}
return groundMap;
}
cv::Mat Bridge::createImgMap(const boost::numeric::ublas::matrix<STATE> &groundMap) {
cv::Mat ground(groundMap.size1(), groundMap.size2(), CV_8UC3, cv::Scalar(0, 0, 0));
for (unsigned int i = 0; i < groundMap.size1(); ++i) {
for (unsigned int j = 0; j < groundMap.size2(); ++j) {
if (groundMap(i, j) == 0) {
ground.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0);
} else {
ground.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 255, 255);
}
}
}
return ground;
}
void Bridge::drawPath(cv::Mat &ground, CoordinateList_sptr &pathCoordList) {
for (auto it = pathCoordList->cbegin(); it != pathCoordList->cend(); ++it) {
ground.at<uchar>(it->Y(), it->X()) = 255;
}
}