Skip to content

Q
qgroundcontrol
Commit ed47290e authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

libGeographic dependency removed, snake namespace renamed to geometry,... 

libGeographic dependency removed, snake namespace renamed to geometry, geometry.h and geometry.cpp cleared up
parent ec7b37e1
Hide whitespace changes
Inline Side-by-side
Showing with 1337 additions and 240 deletions
QGCCommon.pri
View file @ ed47290e
......@@ -35,6 +35,8 @@ linux {
QMAKE_CXXFLAGS_WARN_ON += -Werror \
-Wno-deprecated-copy \ # These come from mavlink headers
-Wno-unused-parameter \ # gst_plugins-good has these errors
-Wno-ignored-qualifiers\ # or-tools has these errors
-Wno-sign-compare\ # or-tools has these errors
-Wno-implicit-fallthrough # gst_plugins-good has these errors
}
} else : linux-rasp-pi2-g++ {
......
qgroundcontrol.pro
View file @ ed47290e
......@@ -445,6 +445,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
#
HEADERS += \
src/MeasurementComplexItem/geometry/geometry.h \
src/QmlControls/QmlUnitsConversion.h \
src/MeasurementComplexItem/geometry/GeoArea.h \
src/MeasurementComplexItem/geometry/MeasurementArea.h \
......@@ -479,7 +480,6 @@ HEADERS += \
src/MeasurementComplexItem/geometry/mapbox/recursive_wrapper.hpp \
src/MeasurementComplexItem/geometry/mapbox/variant.hpp \
src/MeasurementComplexItem/geometry/mapbox/variant_io.hpp \
src/MeasurementComplexItem/geometry/snake.h \
src/MeasurementComplexItem/geometry/GenericPolygon.h \
src/MeasurementComplexItem/geometry/GenericPolygonArray.h \
src/MeasurementComplexItem/geometry/GeoPoint3D.h \
......@@ -524,6 +524,7 @@ SOURCES += \
src/MeasurementComplexItem/geometry/GeoArea.cc \
src/MeasurementComplexItem/geometry/MeasurementArea.cc \
src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/MeasurementComplexItem/AreaData.cc \
src/api/QGCCorePlugin.cc \
......@@ -537,7 +538,6 @@ SOURCES += \
src/MeasurementComplexItem/GeneratorBase.cc \
src/MeasurementComplexItem/LinearGenerator.cpp \
src/MeasurementComplexItem/geometry/clipper/clipper.cpp \
src/MeasurementComplexItem/geometry/snake.cpp \
src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
src/MeasurementComplexItem/NemoInterface.cpp \
src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc \
......
src/MeasurementComplexItem/AreaData.cc
View file @ ed47290e
......@@ -2,7 +2,7 @@
#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/snake.h"
#include "geometry/geometry.h"
#include "JsonHelper.h"
#include "QGCApplication.h"
......@@ -116,10 +116,10 @@ bool AreaData::isCorrect(bool showError) {
return false;
}
const auto &origin = this->origin();
snake::FPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
snake::FPolygon measurementAreaENU;
snake::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU);
geometry::FPolygon safeAreaENU;
geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
geometry::FPolygon measurementAreaENU;
geometry::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU);
// qDebug() << "origin" << origin;
// std::stringstream ss;
// ss << "measurementAreaENU: " << bg::wkt(measurementAreaENU) << std::endl;
......@@ -218,14 +218,14 @@ void AreaData::intersection(bool showError) {
// convert to ENU
const auto origin = this->origin();
snake::FPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
snake::FPolygon measurementAreaENU;
snake::areaToEnu(origin, measurementArea->pathModel(),
geometry::FPolygon safeAreaENU;
geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
geometry::FPolygon measurementAreaENU;
geometry::areaToEnu(origin, measurementArea->pathModel(),
measurementAreaENU);
// do intersection
std::deque<snake::FPolygon> outputENU;
std::deque<geometry::FPolygon> outputENU;
boost::geometry::intersection(measurementAreaENU, safeAreaENU, outputENU);
if (outputENU.size() < 1 || outputENU[0].outer().size() < 4) {
......@@ -246,9 +246,9 @@ void AreaData::intersection(bool showError) {
// Shrink the result if safeAreaENU doesn't cover it.
auto large = std::move(outputENU[0]);
snake::FPolygon small;
geometry::FPolygon small;
while (!bg::covered_by(large, safeAreaENU)) {
snake::offsetPolygon(large, small, -0.1);
geometry::offsetPolygon(large, small, -0.1);
large = std::move(small);
}
......@@ -259,7 +259,7 @@ void AreaData::intersection(bool showError) {
for (auto it = large.outer().begin(); it != large.outer().end() - 1;
++it) {
QGeoCoordinate c;
snake::fromENU(origin, *it, c);
geometry::fromENU(origin, *it, c);
measurementArea->appendVertex(c);
}
}
......
src/MeasurementComplexItem/CircularGenerator.cpp
View file @ ed47290e
......@@ -31,11 +31,12 @@ const char *minLengthKey = "MinLength";
const char *referenceKey = "ReferencePoint";
} // namespace
bool circularTransects(const snake::FPoint &reference,
const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects);
bool circularTransects(const geometry::FPoint &reference,
const geometry::FPolygon &polygon,
const std::vector<geometry::FPolygon> &tiles,
geometry::Length deltaR, geometry::Angle deltaAlpha,
geometry::Length minLength,
geometry::LineStringArray &transects);
const char *CircularGenerator::settingsGroup = "CircularGenerator";
const char *CircularGenerator::typeString = "CircularGenerator";
......@@ -76,7 +77,7 @@ QString CircularGenerator::abbreviation() const { return tr("C. Gen."); }
QString CircularGenerator::type() const { return typeString; }
bool CircularGenerator::get(Generator &generator) {
bool CircularGenerator::get(Work &work) {
if (this->_d) {
if (this->_d->isCorrect()) {
// Prepare data.
......@@ -93,8 +94,8 @@ bool CircularGenerator::get(Generator &generator) {
qCDebug(CircularGeneratorLog) << "get(): reference invalid." << ref;
return false;
}
snake::FPoint reference;
snake::toENU(origin, ref, reference);
geometry::FPoint reference;
geometry::toENU(origin, ref, reference);
auto measurementArea =
getGeoArea<const MeasurementArea *>(*this->_d->areaList());
......@@ -114,13 +115,13 @@ bool CircularGenerator::get(Generator &generator) {
return false;
}
}
auto pPolygon = std::make_shared<snake::FPolygon>();
snake::areaToEnu(origin, geoPolygon, *pPolygon);
auto pPolygon = std::make_shared<geometry::FPolygon>();
geometry::areaToEnu(origin, geoPolygon, *pPolygon);
// Progress and tiles.
const auto &progress = measurementArea->progress();
const auto *tiles = measurementArea->tiles();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
auto pTiles = std::make_shared<std::vector<geometry::FPolygon>>();
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
......@@ -128,8 +129,8 @@ bool CircularGenerator::get(Generator &generator) {
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
geometry::FPolygon tileENU;
geometry::areaToEnu(origin, tile->coordinateList(), tileENU);
pTiles->push_back(std::move(tileENU));
} else {
qCDebug(CircularGeneratorLog)
......@@ -154,22 +155,22 @@ bool CircularGenerator::get(Generator &generator) {
qCDebug(CircularGeneratorLog) << "get(): depot invalid." << geoDepot;
return false;
}
snake::FPoint depot;
snake::toENU(origin, geoDepot, depot);
geometry::FPoint depot;
geometry::toENU(origin, geoDepot, depot);
// Fetch transect parameter.
auto distance =
snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto alpha = snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
bu::degree::degree);
generator = [reference, depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool {
auto distance = geometry::Length(this->_distance.rawValue().toDouble() *
bu::si::meter);
auto minLength = geometry::Length(this->_minLength.rawValue().toDouble() *
bu::si::meter);
auto alpha = geometry::Angle(this->_deltaAlpha.rawValue().toDouble() *
bu::degree::degree);
work = [reference, depot, pPolygon, pTiles, distance, alpha,
minLength](geometry::LineStringArray &transects) -> bool {
bool value = circularTransects(reference, *pPolygon, *pTiles, distance,
alpha, minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot});
transects.insert(transects.begin(), geometry::FLineString{depot});
return value;
};
return true;
......@@ -413,11 +414,12 @@ void CircularGenerator::setMeasurementArea(MeasurementArea *area) {
}
}
bool circularTransects(const snake::FPoint &reference,
const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects) {
bool circularTransects(const geometry::FPoint &reference,
const geometry::FPolygon &polygon,
const std::vector<geometry::FPolygon> &tiles,
geometry::Length deltaR, geometry::Angle deltaAlpha,
geometry::Length minLength,
geometry::LineStringArray &transects) {
auto s1 = std::chrono::high_resolution_clock::now();
// Check preconitions
......@@ -435,15 +437,16 @@ bool circularTransects(const snake::FPoint &reference,
qCDebug(CircularGeneratorLog) << ss.str().c_str();
} else {
// Calculate polygon distances and angles.
std::vector<snake::Length> distances;
std::vector<geometry::Length> distances;
distances.reserve(polygon.outer().size());
std::vector<snake::Angle> angles;
std::vector<geometry::Angle> angles;
angles.reserve(polygon.outer().size());
// qCDebug(CircularGeneratorLog) << "circularTransects():";
for (const auto &p : polygon.outer()) {
snake::Length distance = bg::distance(reference, p) * si::meter;
geometry::Length distance = bg::distance(reference, p) * si::meter;
distances.push_back(distance);
snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
geometry::Angle alpha =
(std::atan2(p.get<1>(), p.get<0>())) * si::radian;
alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
angles.push_back(alpha);
// qCDebug(CircularGeneratorLog) << "distances, angles,
......@@ -456,8 +459,8 @@ bool circularTransects(const snake::FPoint &reference,
}
auto rMin = deltaR; // minimal circle radius
snake::Angle alpha1(0 * degree::degree);
snake::Angle alpha2(360 * degree::degree);
geometry::Angle alpha1(0 * degree::degree);
geometry::Angle alpha2(360 * degree::degree);
// Determine r_min by successive approximation
if (!bg::within(reference, polygon.outer())) {
rMin = bg::distance(reference, polygon) * si::meter;
......@@ -478,7 +481,7 @@ bool circularTransects(const snake::FPoint &reference,
// Generate circle sectors.
auto rScaled = rMinScaled;
const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
std::vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
const auto nSectors =
long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
// qCDebug(CircularGeneratorLog) << "circularTransects(): sector
......@@ -504,8 +507,8 @@ bool circularTransects(const snake::FPoint &reference,
}
// Clip sectors to polygonENU.
ClipperLib::Path polygonClipper;
snake::FPolygon shrinked;
snake::offsetPolygon(polygon, shrinked, -0.3);
geometry::FPolygon shrinked;
geometry::offsetPolygon(polygon, shrinked, -0.3);
auto &outer = shrinked.outer();
polygonClipper.reserve(outer.size());
for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
......@@ -522,7 +525,7 @@ bool circularTransects(const snake::FPoint &reference,
// Subtract holes.
if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles;
std::vector<ClipperLib::Path> processedTiles;
for (const auto &tile : tiles) {
ClipperLib::Path path;
for (const auto &v : tile.outer()) {
......@@ -546,12 +549,12 @@ bool circularTransects(const snake::FPoint &reference,
// Extract transects from PolyTree and convert them to
// BoostLineString
for (const auto &child : transectsClipper.Childs) {
snake::FLineString transect;
geometry::FLineString transect;
transect.reserve(child->Contour.size());
for (const auto &vertex : child->Contour) {
auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
transect.push_back(snake::FPoint(x, y));
transect.push_back(geometry::FPoint(x, y));
}
transects.push_back(transect);
}
......@@ -562,7 +565,7 @@ bool circularTransects(const snake::FPoint &reference,
for (auto iti = ito + 1; iti < transects.end(); ++iti) {
auto dist1 = bg::distance(ito->front(), iti->front());
if (dist1 < th) {
snake::FLineString temp;
geometry::FLineString temp;
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
......@@ -573,7 +576,7 @@ bool circularTransects(const snake::FPoint &reference,
}
auto dist2 = bg::distance(ito->front(), iti->back());
if (dist2 < th) {
snake::FLineString temp;
geometry::FLineString temp;
temp.insert(temp.end(), iti->begin(), iti->end());
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
......@@ -582,7 +585,7 @@ bool circularTransects(const snake::FPoint &reference,
}
auto dist3 = bg::distance(ito->back(), iti->front());
if (dist3 < th) {
snake::FLineString temp;
geometry::FLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
temp.insert(temp.end(), iti->begin(), iti->end());
*ito = temp;
......@@ -591,7 +594,7 @@ bool circularTransects(const snake::FPoint &reference,
}
auto dist4 = bg::distance(ito->back(), iti->back());
if (dist4 < th) {
snake::FLineString temp;
geometry::FLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
......
src/MeasurementComplexItem/CircularGenerator.h
View file @ ed47290e
......@@ -26,7 +26,7 @@ public:
virtual QString abbreviation() const override;
virtual QString type() const override;
virtual bool get(Generator &generator) override;
virtual bool get(Work &work) override;
QGeoCoordinate reference() const;
Fact *distance();
......
src/MeasurementComplexItem/GeneratorBase.h
View file @ ed47290e
......@@ -7,7 +7,7 @@
#include <functional>
#include <memory>
#include "geometry/snake.h"
#include "geometry/geometry.h"
#include "AreaData.h"
......@@ -17,7 +17,7 @@ class GeneratorBase : public QObject {
Q_OBJECT
public:
using Data = AreaData *;
using Generator = std::function<bool(snake::Transects &)>;
using Work = std::function<bool(geometry::LineStringArray &)>;
explicit GeneratorBase(QObject *parent = nullptr);
explicit GeneratorBase(Data d, QObject *parent = nullptr);
......@@ -39,7 +39,7 @@ public:
virtual QString abbreviation() const = 0;
virtual QString type() const = 0;
virtual bool get(Generator &generator) = 0;
virtual bool get(Work &work) = 0;
Data data() const;
void setData(Data d);
......
src/MeasurementComplexItem/LinearGenerator.cpp
View file @ ed47290e
......@@ -23,10 +23,11 @@ const char *minLengthKey = "MinLength";
QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog")
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects);
bool linearTransects(const geometry::FPolygon &polygon,
const std::vector<geometry::FPolygon> &tiles,
geometry::Length distance, geometry::Angle angle,
geometry::Length minLength,
geometry::LineStringArray &transects);
const char *LinearGenerator::settingsGroup = "LinearGenerator";
const char *LinearGenerator::typeString = "LinearGenerator";
......@@ -67,7 +68,7 @@ QString LinearGenerator::abbreviation() const {
QString LinearGenerator::type() const { return typeString; }
bool LinearGenerator::get(Generator &generator) {
bool LinearGenerator::get(Work &generator) {
if (_d != nullptr) {
if (this->_d->isCorrect()) {
// Prepare data.
......@@ -95,13 +96,13 @@ bool LinearGenerator::get(Generator &generator) {
return false;
}
}
auto pPolygon = std::make_shared<snake::FPolygon>();
snake::areaToEnu(origin, geoPolygon, *pPolygon);
auto pPolygon = std::make_shared<geometry::FPolygon>();
geometry::areaToEnu(origin, geoPolygon, *pPolygon);
// Progress and tiles.
const auto &progress = measurementArea->progress();
const auto *tiles = measurementArea->tiles();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
auto pTiles = std::make_shared<std::vector<geometry::FPolygon>>();
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
......@@ -109,8 +110,8 @@ bool LinearGenerator::get(Generator &generator) {
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
geometry::FPolygon tileENU;
geometry::areaToEnu(origin, tile->coordinateList(), tileENU);
pTiles->push_back(std::move(tileENU));
} else {
qCDebug(LinearGeneratorLog) << "get(): tile == nullptr";
......@@ -134,21 +135,21 @@ bool LinearGenerator::get(Generator &generator) {
qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot;
return false;
}
snake::FPoint depot;
snake::toENU(origin, geoDepot, depot);
geometry::FPoint depot;
geometry::toENU(origin, geoDepot, depot);
// Fetch transect parameter.
auto distance =
snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto alpha =
snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
auto distance = geometry::Length(this->_distance.rawValue().toDouble() *
bu::si::meter);
auto minLength = geometry::Length(this->_minLength.rawValue().toDouble() *
bu::si::meter);
auto alpha = geometry::Angle(this->_alpha.rawValue().toDouble() *
bu::degree::degree);
generator = [depot, pPolygon, pTiles, distance, alpha,
minLength](snake::Transects &transects) -> bool {
minLength](geometry::LineStringArray &transects) -> bool {
bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
minLength, transects);
transects.insert(transects.begin(), snake::FLineString{depot});
transects.insert(transects.begin(), geometry::FLineString{depot});
return value;
};
return true;
......@@ -326,10 +327,11 @@ void LinearGenerator::setMeasurementArea(MeasurementArea *area) {
}
}
bool linearTransects(const snake::FPolygon &polygon,
const std::vector<snake::FPolygon> &tiles,
snake::Length distance, snake::Angle angle,
snake::Length minLength, snake::Transects &transects) {
bool linearTransects(const geometry::FPolygon &polygon,
const std::vector<geometry::FPolygon> &tiles,
geometry::Length distance, geometry::Angle angle,
geometry::Length minLength,
geometry::LineStringArray &transects) {
namespace tr = bg::strategy::transform;
auto s1 = std::chrono::high_resolution_clock::now();
......@@ -349,9 +351,9 @@ bool linearTransects(const snake::FPolygon &polygon,
tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
180 / M_PI);
// Rotate polygon by angle and calculate bounding box.
snake::FPolygon polygonENURotated;
geometry::FPolygon polygonENURotated;
bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
snake::FBox box;
geometry::FBox box;
boost::geometry::envelope(polygonENURotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
......@@ -360,17 +362,17 @@ bool linearTransects(const snake::FPolygon &polygon,
// Generate transects and convert them to clipper path.
size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects
vector<ClipperLib::Path> transectsClipper;
std::vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) {
// calculate transect
snake::FPoint v1{x0, y0 + i * distance.value()};
snake::FPoint v2{x1, y0 + i * distance.value()};
snake::FLineString transect;
geometry::FPoint v1{x0, y0 + i * distance.value()};
geometry::FPoint v2{x1, y0 + i * distance.value()};
geometry::FLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
snake::FLineString temp_transect;
geometry::FLineString temp_transect;
tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-angle.value() * 180 / M_PI);
bg::transform(transect, temp_transect, rotate_back);
......@@ -397,8 +399,8 @@ bool linearTransects(const snake::FPolygon &polygon,
}
// Convert measurement area to clipper path.
snake::FPolygon shrinked;
snake::offsetPolygon(polygon, shrinked, -0.2);
geometry::FPolygon shrinked;
geometry::offsetPolygon(polygon, shrinked, -0.2);
auto &outer = shrinked.outer();
ClipperLib::Path polygonClipper;
for (auto vertex : outer) {
......@@ -418,7 +420,7 @@ bool linearTransects(const snake::FPolygon &polygon,
// Subtract holes.
if (tiles.size() > 0) {
vector<ClipperLib::Path> processedTiles;
std::vector<ClipperLib::Path> processedTiles;
for (const auto &tile : tiles) {
ClipperLib::Path path;
for (const auto &v : tile.outer()) {
......@@ -442,14 +444,14 @@ bool linearTransects(const snake::FPolygon &polygon,
// Extract transects from PolyTree and convert them to BoostLineString
for (const auto &child : clippedTransecs.Childs) {
const auto &clipperTransect = child->Contour;
snake::FPoint v1{
geometry::FPoint v1{
static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
snake::FPoint v2{
geometry::FPoint v2{
static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
snake::FLineString transect{v1, v2};
geometry::FLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value()) {
transects.push_back(transect);
}
......
src/MeasurementComplexItem/LinearGenerator.h
View file @ ed47290e
......@@ -25,7 +25,7 @@ public:
virtual QString abbreviation() const override;
virtual QString type() const override;
virtual bool get(Generator &generator) override;
virtual bool get(Work &generator) override;
//!
//! \brief save Saves the generator.
......
src/MeasurementComplexItem/MeasurementComplexItem.cc
View file @ ed47290e
......@@ -7,7 +7,7 @@
#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/clipper/clipper.hpp"
#include "geometry/snake.h"
#include "geometry/geometry.h"
#include "nemo_interface/SnakeTile.h"
// QGC
......@@ -365,13 +365,13 @@ bool MeasurementComplexItem::load(const QJsonObject &complexObject,
auto safeArea = safeAreaArray[0];
QGeoCoordinate origin =
safeArea->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
snake::FPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU);
geometry::FPolygon safeAreaENU;
geometry::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU);
for (const auto &variant : variantVector) {
snake::FLineString varENU;
geometry::FLineString varENU;
for (const auto &vertex : variant) {
snake::FPoint vertexENU;
snake::toENU(origin, vertex.value<QGeoCoordinate>(), vertexENU);
geometry::FPoint vertexENU;
geometry::toENU(origin, vertex.value<QGeoCoordinate>(), vertexENU);
varENU.push_back(vertexENU);
}
......@@ -833,11 +833,11 @@ void MeasurementComplexItem::_updateRoute() {
RoutingParameter par;
par.numSolutions = 5;
auto &safeAreaENU = par.safeArea;
snake::areaToEnu(origin, geoSafeArea, safeAreaENU);
geometry::areaToEnu(origin, geoSafeArea, safeAreaENU);
// Create generator.
if (this->_pGenerator != nullptr) {
routing::GeneratorBase::Generator g; // Transect generator.
routing::GeneratorBase::Work g; // Transect generator.
if (this->_pGenerator->get(g)) {
// Start/Restart routing worker.
this->_pWorker->route(par, g);
......@@ -1182,7 +1182,7 @@ void MeasurementComplexItem::_storeRoutingData(
auto ori = this->_pAreaData->origin();
ori.setAltitude(0);
QVector<Variant> variantVector;
const auto nSolutions = pRoute->solutionVector.size();
const std::size_t nSolutions = pRoute->solutionVector.size();
for (std::size_t j = 0; j < nSolutions; ++j) {
Variant var;
......@@ -1195,7 +1195,7 @@ void MeasurementComplexItem::_storeRoutingData(
for (const auto &vertex : path) {
QGeoCoordinate c;
snake::fromENU(ori, vertex, c);
geometry::fromENU(ori, vertex, c);
var.append(QVariant::fromValue(c));
}
} else {
......
src/MeasurementComplexItem/MeasurementComplexItem.h
View file @ ed47290e
......@@ -12,7 +12,7 @@
#include "AreaData.h"
class RoutingThread;
class RoutingData;
class RoutingResult;
namespace routing {
class GeneratorBase;
......@@ -23,7 +23,7 @@ class MeasurementComplexItem : public ComplexMissionItem {
using PtrGenerator = routing::GeneratorBase *;
using PtrAreaData = AreaData *;
using PtrRoutingData = std::shared_ptr<RoutingData>;
using PtrRoutingData = std::shared_ptr<RoutingResult>;
using PtrWorker = RoutingThread *;
using Variant = QVariantList;
......
src/MeasurementComplexItem/NemoInterface.cpp
View file @ ed47290e
......@@ -14,7 +14,7 @@
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/snake.h"
#include "geometry/geometry.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/QNemoProgress.h"
#include "nemo_interface/SnakeTile.h"
......@@ -170,7 +170,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
geometry::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
......
src/MeasurementComplexItem/RoutingThread.cpp
View file @ ed47290e
......@@ -3,10 +3,35 @@
#include <chrono>
// Qt
#include <QDebug>
// or-tools
#include "QGCLoggingCategory.h"
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
QGC_LOGGING_CATEGORY(RoutingThreadLog, "RoutingThreadLog")
using namespace geometry;
using namespace operations_research;
// Aux struct and functions.
struct InternalParameters {
InternalParameters()
: numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::size_t minNumTransectsPerRun;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool getRoute(const FPolygon &area, const LineStringArray &transects,
std::vector<Solution> &solutionVector,
const InternalParameters &par = InternalParameters());
// class RoutingThread
RoutingThread::RoutingThread(QObject *parent)
: QThread(parent), _calculating(false), _stop(false), _restart(false) {
......@@ -26,12 +51,11 @@ RoutingThread::~RoutingThread() {
bool RoutingThread::calculating() const { return this->_calculating; }
void RoutingThread::route(const RoutingParameter &par,
const Generator &generator) {
void RoutingThread::route(const RoutingParameter &par, const Work &work) {
// Sample input.
Lock lk(this->_mutex);
this->_par = par;
this->_generator = generator;
this->_work = work;
lk.unlock();
if (!this->isRunning()) {
......@@ -55,16 +79,16 @@ void RoutingThread::run() {
emit calculatingChanged();
Lock lk(this->_mutex);
auto par = this->_par;
auto generator = this->_generator;
auto work = this->_work;
lk.unlock();
auto safeAreaENU = par.safeArea;
auto numRuns = par.numRuns;
auto numSolutionsPerRun = par.numSolutions;
PtrRoutingData pRouteData(new RoutingData());
PtrRoutingData pRouteData(new RoutingResult());
auto &transectsENU = pRouteData->transects;
// Generate transects.
if (generator(transectsENU)) {
if (work(transectsENU)) {
// Check if generation was successful.
if (transectsENU.size() == 0) {
qCDebug(RoutingThreadLog) << "run(): "
......@@ -73,29 +97,29 @@ void RoutingThread::run() {
// Prepare data for routing.
auto &solutionVector = pRouteData->solutionVector;
snake::RouteParameter snakePar;
snakePar.numSolutionsPerRun = numSolutionsPerRun;
snakePar.numRuns = numRuns;
InternalParameters routePar;
routePar.numSolutionsPerRun = numSolutionsPerRun;
routePar.numRuns = numRuns;
// Set time limit to 10 min.
const auto maxRoutingTime = std::chrono::minutes(10);
const auto routingEnd =
std::chrono::high_resolution_clock::now() + maxRoutingTime;
const auto &restart = this->_restart;
snakePar.stop = [&restart, routingEnd] {
routePar.stop = [&restart, routingEnd] {
bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
return restart || expired;
};
// Route transects.
bool success =
snake::route(safeAreaENU, transectsENU, solutionVector, snakePar);
getRoute(safeAreaENU, transectsENU, solutionVector, routePar);
// Check if routing was successful.
if ((!success || solutionVector.size() < 1) && !this->_restart) {
qCDebug(RoutingThreadLog) << "run(): "
"routing failed. "
<< snakePar.errorString.c_str();
<< routePar.errorString.c_str();
} else if (this->_restart) {
qCDebug(RoutingThreadLog) << "run(): "
"restart requested.";
......@@ -127,3 +151,402 @@ void RoutingThread::run() {
} // main loop
qCDebug(RoutingThreadLog) << "run(): thread end.";
}
bool getRoute(const FPolygon &area, const LineStringArray &transects,
std::vector<Solution> &solutionVector,
const InternalParameters &par) {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
//================================================================
// Create routing model.
//================================================================
// Use integer polygons to increase numerical robustness.
// Convert area;
IPolygon intArea;
for (const auto &v : area.outer()) {
auto p = float2Int(v);
intArea.outer().push_back(p);
}
for (const auto &ring : area.inners()) {
IRing intRing;
for (const auto &v : ring) {
auto p = float2Int(v);
intRing.push_back(p);
}
intArea.inners().push_back(std::move(intRing));
}
// Helper classes.
struct VirtualNode {
VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {}
std::size_t fromIndex; // index for leaving node
std::size_t toIndex; // index for entering node
};
struct NodeToTransect {
NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {}
std::size_t transectsIndex; // transects index
bool reversed; // transect reversed?
};
// Create vertex and node list
std::vector<IPoint> vertices;
std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
std::vector<VirtualNode> nodeList;
std::vector<NodeToTransect> nodeToTransectList;
for (std::size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
// Copy line edges only.
if (t.size() == 1 || i == 0) {
auto p = float2Int(t.back());
vertices.push_back(p);
nodeToTransectList.emplace_back(i, false);
auto idx = vertices.size() - 1;
nodeList.emplace_back(idx, idx);
} else if (t.size() > 1) {
auto p1 = float2Int(t.front());
auto p2 = float2Int(t.back());
vertices.push_back(p1);
vertices.push_back(p2);
nodeToTransectList.emplace_back(i, false);
nodeToTransectList.emplace_back(i, true);
auto fromIdx = vertices.size() - 1;
auto toIdx = fromIdx - 1;
nodeList.emplace_back(fromIdx, toIdx);
nodeList.emplace_back(toIdx, fromIdx);
disjointNodes.emplace_back(toIdx, fromIdx);
} else { // transect empty
std::cout << "ignoring empty transect with index " << i << std::endl;
}
}
#ifdef SNAKE_DEBUG
// Print.
std::cout << "nodeToTransectList:" << std::endl;
std::cout << "node:transectIndex:reversed" << std::endl;
std::size_t c = 0;
for (const auto &n2t : nodeToTransectList) {
std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed
<< std::endl;
}
std::cout << "nodeList:" << std::endl;
std::cout << "node:fromIndex:toIndex" << std::endl;
c = 0;
for (const auto &n : nodeList) {
std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl;
}
std::cout << "disjoint nodes:" << std::endl;
std::cout << "number:nodes" << std::endl;
c = 0;
for (const auto &d : disjointNodes) {
std::cout << c++ << ":" << d.first << "," << d.second << std::endl;
}
#endif
// Add polygon vertices.
for (auto &v : intArea.outer()) {
vertices.push_back(v);
}
for (auto &ring : intArea.inners()) {
for (auto &v : ring) {
vertices.push_back(v);
}
}
// Create connection graph (inf == no connection between vertices).
// Note: graph is not symmetric.
auto n = vertices.size();
// Matrix must be double since integers don't have infinity and nan
Matrix<double> connectionGraph(n, n);
for (std::size_t i = 0; i < n; ++i) {
auto &fromVertex = vertices[i];
for (std::size_t j = 0; j < n; ++j) {
auto &toVertex = vertices[j];
ILineString line{fromVertex, toVertex};
if (bg::covered_by(line, intArea)) {
connectionGraph(i, j) = bg::length(line);
} else {
connectionGraph(i, j) = std::numeric_limits<double>::infinity();
}
}
}
#ifdef SNAKE_DEBUG
std::cout << "connection grah:" << std::endl;
std::cout << connectionGraph << std::endl;
#endif
// Create distance matrix.
auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double {
return connectionGraph(i, j);
};
auto nNodes = nodeList.size();
Matrix<IntType> distanceMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
distanceMatrix(i, i) = 0;
for (std::size_t j = i + 1; j < nNodes; ++j) {
auto dist = connectionGraph(i, j);
if (std::isinf(dist)) {
std::vector<std::size_t> route;
if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) {
std::stringstream ss;
ss << "Distance matrix calculation failed. connection graph: "
<< connectionGraph << std::endl;
ss << "area: " << bg::wkt(area) << std::endl;
ss << "transects:" << std::endl;
for (const auto &t : transects) {
ss << bg::wkt(t) << std::endl;
}
par.errorString = ss.str();
return false;
}
(void)route;
}
distanceMatrix(i, j) = dist;
distanceMatrix(j, i) = dist;
}
}
#ifdef SNAKE_DEBUG
std::cout << "distance matrix:" << std::endl;
std::cout << distanceMatrix << std::endl;
#endif
// Create (asymmetric) routing matrix.
Matrix<IntType> routingMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
auto fromNode = nodeList[i];
for (std::size_t j = 0; j < nNodes; ++j) {
auto toNode = nodeList[j];
routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex);
}
}
// Insert max for disjoint nodes.
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
routingMatrix(i, j) = std::numeric_limits<IntType>::max();
routingMatrix(j, i) = std::numeric_limits<IntType>::max();
}
#ifdef SNAKE_DEBUG
std::cout << "routing matrix:" << std::endl;
std::cout << routingMatrix << std::endl;
#endif
// Create Routing Index Manager.
auto minNumTransectsPerRun =
std::max<std::size_t>(1, par.minNumTransectsPerRun);
auto maxRuns = std::max<std::size_t>(
1, std::floor(double(transects.size()) / minNumTransectsPerRun));
auto numRuns = std::max<std::size_t>(1, par.numRuns);
numRuns = std::min<std::size_t>(numRuns, maxRuns);
RoutingIndexManager::NodeIndex depot(0);
// std::vector<RoutingIndexManager::NodeIndex> depots(numRuns, depot);
// RoutingIndexManager manager(nNodes, numRuns, depots, depots);
RoutingIndexManager manager(nNodes, numRuns, depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 {
// Convert from routing variable Index to distance matrix NodeIndex.
auto from_node = manager.IndexToNode(from_index).value();
auto to_node = manager.IndexToNode(to_index).value();
return routingMatrix(from_node, to_node);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Add distance dimension.
if (numRuns > 1) {
routing.AddDimension(transitCallbackIndex, 0, 300000000,
true, // start cumul to zero
"Distance");
routing.GetMutableDimension("Distance")
->SetGlobalSpanCostCoefficient(100000000);
}
// Define disjunctions.
#ifdef SNAKE_DEBUG
std::cout << "disjunctions:" << std::endl;
#endif
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
#ifdef SNAKE_DEBUG
std::cout << i << "," << j << std::endl;
#endif
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j));
std::vector<int64> disj{idx0, idx1};
routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/);
}
// Set first solution heuristic.
auto searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
// Number of solutions.
auto numSolutionsPerRun = std::max<std::size_t>(1, par.numSolutionsPerRun);
searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun);
// Set costume limit.
auto *solver = routing.solver();
auto *limit = solver->MakeCustomLimit(par.stop);
routing.AddSearchMonitor(limit);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "create routing model: " << delta.count() << " ms" << endl;
#endif
//================================================================
// Solve model.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
auto pSolutions = std::make_unique<std::vector<const Assignment *>>();
(void)routing.SolveWithParameters(searchParameters, pSolutions.get());
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "solve routing model: " << delta.count() << " ms" << endl;
#endif
if (par.stop()) {
par.errorString = "User terminated.";
return false;
}
if (pSolutions->size() == 0) {
std::stringstream ss;
ss << "No solution found." << std::endl;
par.errorString = ss.str();
return false;
}
//================================================================
// Construc route.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
long long counter = -1;
// Note: route number 0 corresponds to the best route which is the last entry
// of *pSolutions.
for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin();
--solution) {
++counter;
if (!*solution || (*solution)->Size() <= 1) {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << "invalid."
<< std::endl;
par.errorString = ss.str();
continue;
}
// Iterate over all routes.
Solution routeVector;
for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) {
if (!routing.IsVehicleUsed(**solution, vehicle))
continue;
// Create index list.
auto index = routing.Start(vehicle);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
index = (*solution)->Value(routing.NextVar(index));
route_idx.push_back(manager.IndexToNode(index).value());
}
#ifdef SNAKE_DEBUG
// Print route.
std::cout << "route " << counter
<< " route_idx.size() = " << route_idx.size() << std::endl;
std::cout << "route: ";
for (const auto &idx : route_idx) {
std::cout << idx << ", ";
}
std::cout << std::endl;
#endif
if (route_idx.size() < 2) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
// Assemble route.
Route r;
auto &path = r.path;
auto &info = r.info;
for (size_t i = 0; i < route_idx.size() - 1; ++i) {
size_t nodeIndex0 = route_idx[i];
size_t nodeIndex1 = route_idx[i + 1];
const auto &n2t0 = nodeToTransectList[nodeIndex0];
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
// Copy transect to route.
const auto &t = transects[n2t0.transectsIndex];
if (n2t0.reversed) { // transect reversal needed?
for (auto it = t.end() - 1; it > t.begin(); --it) {
path.push_back(*it);
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
path.push_back(*it);
}
}
// Connect transects.
std::vector<size_t> idxList;
if (!shortestPathFromGraph(connectionGraph,
nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
for (auto idx : idxList) {
auto p = int2Float(vertices[idx]);
path.push_back(p);
}
}
// Append last transect info.
const auto &n2t0 = nodeToTransectList.back();
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
if (path.size() < 2 || info.size() < 2) {
std::stringstream ss;
ss << par.errorString << "Route empty (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
routeVector.push_back(std::move(r));
}
if (routeVector.size() > 0) {
solutionVector.push_back(std::move(routeVector));
} else {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << " empty." << std::endl;
par.errorString = ss.str();
}
}
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
if (solutionVector.size() > 0) {
return true;
} else {
return false;
}
}
src/MeasurementComplexItem/RoutingThread.h
View file @ ed47290e
......@@ -4,24 +4,40 @@
#include <QSharedPointer>
#include <QThread>
#include "geometry/snake.h"
#include "geometry/geometry.h"
#include <atomic>
#include <condition_variable>
#include <functional>
#include <mutex>
struct RoutingData {
snake::Transects transects;
std::vector<snake::Solution> solutionVector;
// Aux structs
struct TransectInfo {
TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
struct Route {
geometry::FLineString path;
std::vector<TransectInfo> info;
};
typedef std::vector<Route>
Solution; // Every route corresponds to one run/vehicle
struct RoutingResult {
geometry::LineStringArray transects;
std::vector<Solution> solutionVector;
std::string errorString;
};
struct RoutingParameter {
RoutingParameter() : numSolutions(1), numRuns(1) {}
snake::FPolygon safeArea;
geometry::FPolygon safeArea;
std::size_t numSolutions;
std::size_t numRuns;
};
//!
//! \brief The CSWorker class
//! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker
......@@ -31,9 +47,8 @@ class RoutingThread : public QThread {
using Lock = std::unique_lock<std::mutex>;
public:
using PtrRoutingData = shared_ptr<RoutingData>;
using Generator = std::function<bool(snake::Transects &)>;
using Consumer = std::function<void(const RoutingData &)>;
using PtrRoutingData = std::shared_ptr<RoutingResult>;
using Work = std::function<bool(geometry::LineStringArray &)>;
RoutingThread(QObject *parent = nullptr);
~RoutingThread() override;
......@@ -41,7 +56,7 @@ public:
bool calculating() const;
public slots:
void route(const RoutingParameter &par, const Generator &generator);
void route(const RoutingParameter &par, const Work &work);
signals:
void result(PtrRoutingData pTransects);
......@@ -55,7 +70,7 @@ private:
mutable std::condition_variable _cv;
// Internal data
RoutingParameter _par;
Generator _generator; // transect generator
Work _work; // transect worker
// State
std::atomic_bool _calculating;
std::atomic_bool _stop;
......
src/MeasurementComplexItem/geometry/GeoArea.cc
View file @ ed47290e
#include "GeoArea.h"
#include "snake.h"
#include "geometry.h"
#include "QGCLoggingCategory.h"
#include "QGCQGeoCoordinate.h"
......@@ -48,17 +48,19 @@ bool GeoArea::loadFromJson(const QJsonObject &json, QString &errorString) {
bool GeoArea::isCorrect() {
if (this->pathModel().count() >= 3) {
auto origin = this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
snake::FPolygon polygonENU;
snake::areaToEnu(origin, this->pathModel(), polygonENU);
geometry::FPolygon polygonENU;
geometry::areaToEnu(origin, this->pathModel(), polygonENU);
std::string msg;
if (bg::is_valid(polygonENU, msg)) {
return true;
} else {
qCWarning(GeoAreaLog) << msg.c_str();
qCWarning(GeoAreaLog) << "origin: " << origin;
qCWarning(GeoAreaLog) << "isCorrect(): " << msg.c_str();
qCWarning(GeoAreaLog) << "isCorrect(): "
<< "origin: " << origin;
std::stringstream ss;
ss << bg::wkt(polygonENU);
qCWarning(GeoAreaLog) << "polygonENU: " << ss.str().c_str();
qCWarning(GeoAreaLog) << "isCorrect(): "
<< "polygonENU: " << ss.str().c_str();
setErrorString(this->objectName() + tr(" must be a simple polygon."));
}
}
......@@ -70,10 +72,10 @@ QString GeoArea::errorString() const { return this->_errorString; }
bool GeoArea::covers(const QGeoCoordinate &c) {
if (GeoArea::isCorrect()) {
auto origin = this->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
snake::FPolygon polygonENU;
snake::areaToEnu(origin, this->pathModel(), polygonENU);
snake::FPoint cENU;
snake::toENU(origin, c, cENU);
geometry::FPolygon polygonENU;
geometry::areaToEnu(origin, this->pathModel(), polygonENU);
geometry::FPoint cENU;
geometry::toENU(origin, c, cENU);
return bg::covered_by(cENU, polygonENU);
} else {
return false;
......@@ -100,7 +102,7 @@ void GeoArea::setErrorString(const QString &str) {
emit errorStringChanged();
}
void GeoArea::setErrorString(const string &str) {
void GeoArea::setErrorString(const std::string &str) {
this->_errorString = str.c_str();
emit errorStringChanged();
}
......
src/MeasurementComplexItem/geometry/MeasurementArea.cc
View file @ ed47290e
#include "MeasurementArea.h"
#include "QtConcurrentRun"
#include "geometry.h"
#include "nemo_interface/SnakeTile.h"
#include "snake.h"
#include <ctime>
#include <boost/units/systems/si.hpp>
......@@ -15,6 +15,14 @@
#define SNAKE_MAX_TILES 1000
#endif
using namespace geometry;
namespace trans = bg::strategy::transform;
// Aux function
bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles,
BoundingBox &bbox);
QGC_LOGGING_CATEGORY(MeasurementAreaLog, "MeasurementAreaLog")
namespace {
......@@ -460,7 +468,7 @@ void MeasurementArea::resetProgress() {
//! \pre MeasurementArea::deferUpdate must be called first, don't call
//! this function directly!
void MeasurementArea::doUpdate() {
using namespace snake;
using namespace geometry;
using namespace boost::units;
auto start = std::chrono::high_resolution_clock::now();
......@@ -493,10 +501,8 @@ void MeasurementArea::doUpdate() {
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
if (getTiles(polygonENU, height, width, minArea, tilesENU, bbox)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
......@@ -507,8 +513,8 @@ void MeasurementArea::doUpdate() {
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
geometry::FPoint center;
geometry::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
......@@ -625,3 +631,106 @@ void MeasurementArea::setHoldProgress(bool holdProgress) {
emit holdProgressChanged();
}
}
bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles,
BoundingBox &bbox) {
if (area.outer().empty() || area.outer().size() < 4) {
qCDebug(MeasurementAreaLog) << "Area has to few vertices.";
return false;
}
if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter ||
minTileArea < 0 * bu::si::meter * bu::si::meter) {
std::stringstream ss;
ss << "Parameters tileWidth (" << tileWidth << "), tileHeight ("
<< tileHeight << "), minTileArea (" << minTileArea
<< ") must be positive.";
qCDebug(MeasurementAreaLog) << ss.str().c_str();
return false;
}
if (bbox.corners.outer().size() != 5) {
bbox.corners.clear();
minimalBoundingBox(area, bbox);
}
if (bbox.corners.outer().size() < 5)
return false;
double bboxWidth = bbox.width;
double bboxHeight = bbox.height;
FPoint origin = bbox.corners.outer()[0];
// cout << "Origin: " << origin[0] << " " << origin[1] << endl;
// Transform _mArea polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(
bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
-origin.get<1>());
FPolygon translated_polygon;
FPolygon rotated_polygon;
boost::geometry::transform(area, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
// cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t iMax = ceil(bboxWidth / tileWidth.value());
size_t jMax = ceil(bboxHeight / tileHeight.value());
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight
<< ") to large for measurement area.";
qCDebug(MeasurementAreaLog) << ss.str().c_str();
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
-bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
origin.get<1>());
for (size_t i = 0; i < iMax; ++i) {
double x_min = tileWidth.value() * i;
double x_max = x_min + tileWidth.value();
for (size_t j = 0; j < jMax; ++j) {
double y_min = tileHeight.value() * j;
double y_max = y_min + tileHeight.value();
FPolygon tile_unclipped;
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
std::deque<FPolygon> boost_tiles;
if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
boost_tiles))
continue;
for (FPolygon t : boost_tiles) {
if (bg::area(t) > minTileArea.value()) {
// Transform boost_tile to world coordinate system.
FPolygon rotated_tile;
FPolygon translated_tile;
boost::geometry::transform(t, rotated_tile, rotate_back);
boost::geometry::transform(rotated_tile, translated_tile,
translate_back);
// Store tile and calculate center point.
tiles.push_back(translated_tile);
}
}
}
}
if (tiles.size() < 1) {
std::stringstream ss;
ss << "No tiles calculated. Is the minTileArea (" << minTileArea
<< ") parameter large enough?";
qCDebug(MeasurementAreaLog) << ss.str().c_str();
return false;
}
return true;
}
src/MeasurementComplexItem/geometry/snake.cpp → src/MeasurementComplexItem/geometry/geometry.cpp
View file @ ed47290e
#include <algorithm>
#include <iostream>
#include "snake.h"
#include "geometry.h"
#include <mapbox/geometry.hpp>
#include <mapbox/polylabel.hpp>
......@@ -14,13 +14,6 @@
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 1000000
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using namespace operations_research;
#ifndef NDEBUG
//#define SNAKE_SHOW_TIME
#endif
......@@ -30,7 +23,7 @@ namespace trans = bg::strategy::transform;
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian)
namespace snake {
namespace geometry {
static const IntType stdScale = 1000000;
//=========================================================================
// Geometry stuff.
......@@ -40,15 +33,15 @@ void polygonCenter(const FPolygon &polygon, FPoint &center) {
using namespace mapbox;
if (polygon.outer().empty())
return;
geometry::polygon<double> p;
geometry::linear_ring<double> lr1;
mapbox::geometry::polygon<double> p;
mapbox::geometry::linear_ring<double> lr1;
for (size_t i = 0; i < polygon.outer().size(); ++i) {
geometry::point<double> vertex(polygon.outer()[i].get<0>(),
polygon.outer()[i].get<1>());
mapbox::geometry::point<double> vertex(polygon.outer()[i].get<0>(),
polygon.outer()[i].get<1>());
lr1.push_back(vertex);
}
p.push_back(lr1);
geometry::point<double> c = polylabel(p);
mapbox::geometry::point<double> c = polylabel(p);
center.set<0>(c.x);
center.set<1>(c.y);
......@@ -270,109 +263,6 @@ bool toDistanceMatrix(Matrix<double> &graph) {
return true;
}
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
string &errorString) {
if (area.outer().empty() || area.outer().size() < 4) {
errorString = "Area has to few vertices.";
return false;
}
if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter ||
minTileArea < 0 * bu::si::meter * bu::si::meter) {
std::stringstream ss;
ss << "Parameters tileWidth (" << tileWidth << "), tileHeight ("
<< tileHeight << "), minTileArea (" << minTileArea
<< ") must be positive.";
errorString = ss.str();
return false;
}
if (bbox.corners.outer().size() != 5) {
bbox.corners.clear();
minimalBoundingBox(area, bbox);
}
if (bbox.corners.outer().size() < 5)
return false;
double bboxWidth = bbox.width;
double bboxHeight = bbox.height;
FPoint origin = bbox.corners.outer()[0];
// cout << "Origin: " << origin[0] << " " << origin[1] << endl;
// Transform _mArea polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(
bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
-origin.get<1>());
FPolygon translated_polygon;
FPolygon rotated_polygon;
boost::geometry::transform(area, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
// cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t iMax = ceil(bboxWidth / tileWidth.value());
size_t jMax = ceil(bboxHeight / tileHeight.value());
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight
<< ") to large for measurement area.";
errorString = ss.str();
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
-bbox.angle * 180 / M_PI);
trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
origin.get<1>());
for (size_t i = 0; i < iMax; ++i) {
double x_min = tileWidth.value() * i;
double x_max = x_min + tileWidth.value();
for (size_t j = 0; j < jMax; ++j) {
double y_min = tileHeight.value() * j;
double y_max = y_min + tileHeight.value();
FPolygon tile_unclipped;
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_max});
tile_unclipped.outer().push_back(FPoint{x_max, y_min});
tile_unclipped.outer().push_back(FPoint{x_min, y_min});
std::deque<FPolygon> boost_tiles;
if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
boost_tiles))
continue;
for (FPolygon t : boost_tiles) {
if (bg::area(t) > minTileArea.value()) {
// Transform boost_tile to world coordinate system.
FPolygon rotated_tile;
FPolygon translated_tile;
boost::geometry::transform(t, rotated_tile, rotate_back);
boost::geometry::transform(rotated_tile, translated_tile,
translate_back);
// Store tile and calculate center point.
tiles.push_back(translated_tile);
}
}
}
}
if (tiles.size() < 1) {
std::stringstream ss;
ss << "No tiles calculated. Is the minTileArea (" << minTileArea
<< ") parameter large enough?";
errorString = ss.str();
return false;
}
return true;
}
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
const FPolygon &corridor, FPolygon &jArea,
std::string &errorString) {
......@@ -489,541 +379,6 @@ void BoundingBox::clear() {
corners.clear();
}
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString) {
// Rotate measurement area by angle and calculate bounding box.
FPolygon mAreaRotated;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
180 / M_PI);
bg::transform(mArea, mAreaRotated, rotate);
FBox box;
boost::geometry::envelope(mAreaRotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
double x1 = box.max_corner().get<0>();
double y1 = box.max_corner().get<1>();
// Generate transects and convert them to clipper path.
size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i = 0; i < num_t; ++i) {
// calculate transect
FPoint v1{x0, y0 + i * distance.value()};
FPoint v2{x1, y0 + i * distance.value()};
FLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
FLineString temp_transect;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-angle.value() * 180 / M_PI);
bg::transform(transect, temp_transect, rotate_back);
// to clipper
ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
temp_transect[0].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[0].get<1>() * CLIPPER_SCALE)};
ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
temp_transect[1].get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(
temp_transect[1].get<1>() * CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
if (transectsClipper.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: distance = " << distance
<< std::endl;
errorString = ss.str();
return false;
}
// Convert measurement area to clipper path.
ClipperLib::Path mAreaClipper;
for (auto vertex : mArea.outer()) {
mAreaClipper.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
const auto *transects = &clippedTransecs;
bool ignoreProgress = p.size() != tiles.size();
ClipperLib::PolyTree clippedTransecs2;
if (!ignoreProgress) {
// Calculate processed tiles (_progress[i] == 100) and subtract them from
// measurement area.
size_t numTiles = p.size();
vector<FPolygon> processedTiles;
for (size_t i = 0; i < numTiles; ++i) {
if (p[i] == 100) {
processedTiles.push_back(tiles[i]);
}
}
if (processedTiles.size() != numTiles) {
vector<ClipperLib::Path> processedTilesClipper;
for (const auto &t : processedTiles) {
ClipperLib::Path path;
for (const auto &vertex : t.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
}
processedTilesClipper.push_back(path);
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper.Clear();
for (const auto &child : clippedTransecs.Childs) {
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
}
clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
clipper.Execute(ClipperLib::ctDifference, clippedTransecs2,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
transects = &clippedTransecs2;
} else {
// All tiles processed (t.size() not changed).
return true;
}
}
// Extract transects from PolyTree and convert them to BoostLineString
for (const auto &child : transects->Childs) {
const auto &clipperTransect = child->Contour;
FPoint v1{static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
FPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
FLineString transect{v1, v2};
if (bg::length(transect) >= minLength.value()) {
t.push_back(transect);
}
}
if (t.size() == 0) {
std::stringstream ss;
ss << "Not able to generate transects. Parameter: minLength = " << minLength
<< std::endl;
errorString = ss.str();
return false;
}
return true;
}
bool route(const FPolygon &area, const Transects &transects,
std::vector<Solution> &solutionVector, const RouteParameter &par) {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
//================================================================
// Create routing model.
//================================================================
// Use integer polygons to increase numerical robustness.
// Convert area;
IPolygon intArea;
for (const auto &v : area.outer()) {
auto p = float2Int(v);
intArea.outer().push_back(p);
}
for (const auto &ring : area.inners()) {
IRing intRing;
for (const auto &v : ring) {
auto p = float2Int(v);
intRing.push_back(p);
}
intArea.inners().push_back(std::move(intRing));
}
// Helper classes.
struct VirtualNode {
VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {}
std::size_t fromIndex; // index for leaving node
std::size_t toIndex; // index for entering node
};
struct NodeToTransect {
NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {}
std::size_t transectsIndex; // transects index
bool reversed; // transect reversed?
};
// Create vertex and node list
std::vector<IPoint> vertices;
std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
std::vector<VirtualNode> nodeList;
std::vector<NodeToTransect> nodeToTransectList;
for (std::size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
// Copy line edges only.
if (t.size() == 1 || i == 0) {
auto p = float2Int(t.back());
vertices.push_back(p);
nodeToTransectList.emplace_back(i, false);
auto idx = vertices.size() - 1;
nodeList.emplace_back(idx, idx);
} else if (t.size() > 1) {
auto p1 = float2Int(t.front());
auto p2 = float2Int(t.back());
vertices.push_back(p1);
vertices.push_back(p2);
nodeToTransectList.emplace_back(i, false);
nodeToTransectList.emplace_back(i, true);
auto fromIdx = vertices.size() - 1;
auto toIdx = fromIdx - 1;
nodeList.emplace_back(fromIdx, toIdx);
nodeList.emplace_back(toIdx, fromIdx);
disjointNodes.emplace_back(toIdx, fromIdx);
} else { // transect empty
std::cout << "ignoring empty transect with index " << i << std::endl;
}
}
#ifdef SNAKE_DEBUG
// Print.
std::cout << "nodeToTransectList:" << std::endl;
std::cout << "node:transectIndex:reversed" << std::endl;
std::size_t c = 0;
for (const auto &n2t : nodeToTransectList) {
std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed
<< std::endl;
}
std::cout << "nodeList:" << std::endl;
std::cout << "node:fromIndex:toIndex" << std::endl;
c = 0;
for (const auto &n : nodeList) {
std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl;
}
std::cout << "disjoint nodes:" << std::endl;
std::cout << "number:nodes" << std::endl;
c = 0;
for (const auto &d : disjointNodes) {
std::cout << c++ << ":" << d.first << "," << d.second << std::endl;
}
#endif
// Add polygon vertices.
for (auto &v : intArea.outer()) {
vertices.push_back(v);
}
for (auto &ring : intArea.inners()) {
for (auto &v : ring) {
vertices.push_back(v);
}
}
// Create connection graph (inf == no connection between vertices).
// Note: graph is not symmetric.
auto n = vertices.size();
// Matrix must be double since integers don't have infinity and nan
Matrix<double> connectionGraph(n, n);
for (std::size_t i = 0; i < n; ++i) {
auto &fromVertex = vertices[i];
for (std::size_t j = 0; j < n; ++j) {
auto &toVertex = vertices[j];
ILineString line{fromVertex, toVertex};
if (bg::covered_by(line, intArea)) {
connectionGraph(i, j) = bg::length(line);
} else {
connectionGraph(i, j) = std::numeric_limits<double>::infinity();
}
}
}
#ifdef SNAKE_DEBUG
std::cout << "connection grah:" << std::endl;
std::cout << connectionGraph << std::endl;
#endif
// Create distance matrix.
auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double {
return connectionGraph(i, j);
};
auto nNodes = nodeList.size();
Matrix<IntType> distanceMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
distanceMatrix(i, i) = 0;
for (std::size_t j = i + 1; j < nNodes; ++j) {
auto dist = connectionGraph(i, j);
if (std::isinf(dist)) {
std::vector<std::size_t> route;
if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) {
std::stringstream ss;
ss << "Distance matrix calculation failed. connection graph: "
<< connectionGraph << std::endl;
ss << "area: " << bg::wkt(area) << std::endl;
ss << "transects:" << std::endl;
for (const auto &t : transects) {
ss << bg::wkt(t) << std::endl;
}
par.errorString = ss.str();
return false;
}
(void)route;
}
distanceMatrix(i, j) = dist;
distanceMatrix(j, i) = dist;
}
}
#ifdef SNAKE_DEBUG
std::cout << "distance matrix:" << std::endl;
std::cout << distanceMatrix << std::endl;
#endif
// Create (asymmetric) routing matrix.
Matrix<IntType> routingMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
auto fromNode = nodeList[i];
for (std::size_t j = 0; j < nNodes; ++j) {
auto toNode = nodeList[j];
routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex);
}
}
// Insert max for disjoint nodes.
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
routingMatrix(i, j) = std::numeric_limits<IntType>::max();
routingMatrix(j, i) = std::numeric_limits<IntType>::max();
}
#ifdef SNAKE_DEBUG
std::cout << "routing matrix:" << std::endl;
std::cout << routingMatrix << std::endl;
#endif
// Create Routing Index Manager.
auto minNumTransectsPerRun =
std::max<std::size_t>(1, par.minNumTransectsPerRun);
auto maxRuns = std::max<std::size_t>(
1, std::floor(double(transects.size()) / minNumTransectsPerRun));
auto numRuns = std::max<std::size_t>(1, par.numRuns);
numRuns = std::min<std::size_t>(numRuns, maxRuns);
RoutingIndexManager::NodeIndex depot(0);
// std::vector<RoutingIndexManager::NodeIndex> depots(numRuns, depot);
// RoutingIndexManager manager(nNodes, numRuns, depots, depots);
RoutingIndexManager manager(nNodes, numRuns, depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 {
// Convert from routing variable Index to distance matrix NodeIndex.
auto from_node = manager.IndexToNode(from_index).value();
auto to_node = manager.IndexToNode(to_index).value();
return routingMatrix(from_node, to_node);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Add distance dimension.
if (numRuns > 1) {
routing.AddDimension(transitCallbackIndex, 0, 300000000,
true, // start cumul to zero
"Distance");
routing.GetMutableDimension("Distance")
->SetGlobalSpanCostCoefficient(100000000);
}
// Define disjunctions.
#ifdef SNAKE_DEBUG
std::cout << "disjunctions:" << std::endl;
#endif
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
#ifdef SNAKE_DEBUG
std::cout << i << "," << j << std::endl;
#endif
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j));
std::vector<int64> disj{idx0, idx1};
routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/);
}
// Set first solution heuristic.
auto searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
// Number of solutions.
auto numSolutionsPerRun = std::max<std::size_t>(1, par.numSolutionsPerRun);
searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun);
// Set costume limit.
auto *solver = routing.solver();
auto *limit = solver->MakeCustomLimit(par.stop);
routing.AddSearchMonitor(limit);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "create routing model: " << delta.count() << " ms" << endl;
#endif
//================================================================
// Solve model.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
auto pSolutions = std::make_unique<std::vector<const Assignment *>>();
(void)routing.SolveWithParameters(searchParameters, pSolutions.get());
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "solve routing model: " << delta.count() << " ms" << endl;
#endif
if (par.stop()) {
par.errorString = "User terminated.";
return false;
}
if (pSolutions->size() == 0) {
std::stringstream ss;
ss << "No solution found." << std::endl;
par.errorString = ss.str();
return false;
}
//================================================================
// Construc route.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
long long counter = -1;
// Note: route number 0 corresponds to the best route which is the last entry
// of *pSolutions.
for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin();
--solution) {
++counter;
if (!*solution || (*solution)->Size() <= 1) {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << "invalid."
<< std::endl;
par.errorString = ss.str();
continue;
}
// Iterate over all routes.
Solution routeVector;
for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) {
if (!routing.IsVehicleUsed(**solution, vehicle))
continue;
// Create index list.
auto index = routing.Start(vehicle);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
index = (*solution)->Value(routing.NextVar(index));
route_idx.push_back(manager.IndexToNode(index).value());
}
#ifdef SNAKE_DEBUG
// Print route.
std::cout << "route " << counter
<< " route_idx.size() = " << route_idx.size() << std::endl;
std::cout << "route: ";
for (const auto &idx : route_idx) {
std::cout << idx << ", ";
}
std::cout << std::endl;
#endif
if (route_idx.size() < 2) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
// Assemble route.
Route r;
auto &path = r.path;
auto &info = r.info;
for (size_t i = 0; i < route_idx.size() - 1; ++i) {
size_t nodeIndex0 = route_idx[i];
size_t nodeIndex1 = route_idx[i + 1];
const auto &n2t0 = nodeToTransectList[nodeIndex0];
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
// Copy transect to route.
const auto &t = transects[n2t0.transectsIndex];
if (n2t0.reversed) { // transect reversal needed?
for (auto it = t.end() - 1; it > t.begin(); --it) {
path.push_back(*it);
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
path.push_back(*it);
}
}
// Connect transects.
std::vector<size_t> idxList;
if (!shortestPathFromGraph(connectionGraph,
nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
for (auto idx : idxList) {
auto p = int2Float(vertices[idx]);
path.push_back(p);
}
}
// Append last transect info.
const auto &n2t0 = nodeToTransectList.back();
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
if (path.size() < 2 || info.size() < 2) {
std::stringstream ss;
ss << par.errorString << "Route empty (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
routeVector.push_back(std::move(r));
}
if (routeVector.size() > 0) {
solutionVector.push_back(std::move(routeVector));
} else {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << " empty." << std::endl;
par.errorString = ss.str();
}
}
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
if (solutionVector.size() > 0) {
return true;
} else {
return false;
}
}
FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); }
FPoint int2Float(const IPoint &ip, IntType scale) {
......@@ -1151,19 +506,23 @@ bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
return true;
}
} // namespace snake
} // namespace geometry
bool boost::geometry::model::operator==(snake::FPoint &p1, snake::FPoint &p2) {
bool boost::geometry::model::operator==(::geometry::FPoint &p1,
::geometry::FPoint &p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::FPoint &p1, snake::FPoint &p2) {
bool boost::geometry::model::operator!=(::geometry::FPoint &p1,
::geometry::FPoint &p2) {
return !(p1 == p2);
}
bool boost::geometry::model::operator==(snake::IPoint &p1, snake::IPoint &p2) {
bool boost::geometry::model::operator==(::geometry::IPoint &p1,
::geometry::IPoint &p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::IPoint &p1, snake::IPoint &p2) {
bool boost::geometry::model::operator!=(::geometry::IPoint &p1,
::geometry::IPoint &p2) {
return !(p1 == p2);
}
src/MeasurementComplexItem/geometry/snake.h → src/MeasurementComplexItem/geometry/geometry.h
View file @ ed47290e
......@@ -3,6 +3,7 @@
#include <array>
#include <atomic>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
......@@ -23,26 +24,25 @@ namespace bu = boost::units;
#include "QGCQGeoCoordinate.h"
#include "QmlObjectListModel.h"
using namespace std;
namespace snake {
namespace geometry {
//=========================================================================
// Geometry stuff.
//=========================================================================
// Double geometry.
using FloatType = double;
typedef double FloatType;
typedef bg::model::point<double, 2, bg::cs::cartesian> FPoint;
typedef bg::model::polygon<FPoint> FPolygon;
typedef bg::model::linestring<FPoint> FLineString;
typedef bg::model::box<FPoint> FBox;
typedef std::vector<FLineString> LineStringArray;
// Integer geometry.
using IntType = long long;
using IPoint = bg::model::point<IntType, 2, bg::cs::cartesian>;
using IPolygon = bg::model::polygon<IPoint>;
using IRing = bg::model::ring<IPoint>;
using ILineString = bg::model::linestring<IPoint>;
typedef long long IntType;
typedef bg::model::point<IntType, 2, bg::cs::cartesian> IPoint;
typedef bg::model::polygon<IPoint> IPolygon;
typedef bg::model::ring<IPoint> IRing;
typedef bg::model::linestring<IPoint> ILineString;
FPoint int2Float(const IPoint &ip);
FPoint int2Float(const IPoint &ip, IntType scale);
......@@ -52,7 +52,7 @@ IPoint float2Int(const FPoint &ip, IntType scale);
template <class T> class Matrix;
template <class DataType>
ostream &operator<<(ostream &os, const Matrix<DataType> &matrix) {
std::ostream &operator<<(std::ostream &os, const Matrix<DataType> &matrix) {
for (std::size_t i = 0; i < matrix.m(); ++i) {
for (std::size_t j = 0; j < matrix.n(); ++j) {
os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl;
......@@ -85,7 +85,8 @@ public:
std::size_t m() const { return _n; }
std::size_t n() const { return _n; }
friend ostream &operator<<<>(ostream &os, const Matrix<DataType> &dt);
friend std::ostream &operator<<<>(std::ostream &os,
const Matrix<DataType> &dt);
private:
std::vector<DataType> _matrix;
......@@ -104,46 +105,100 @@ struct BoundingBox {
FPolygon corners;
};
constexpr int earth_radius = 6371000; // meters (m)
constexpr double epsilon = std::numeric_limits<double>::epsilon(); // meters (m)
template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
earth);
double x = 0, y = 0, z = 0;
proj.Forward(in.latitude(), in.longitude(), 0, x, y, z);
double lat_rad = in.latitude() * M_PI / 180;
double lon_rad = in.longitude() * M_PI / 180;
double ref_lon_rad = origin.longitude() * M_PI / 180;
double ref_lat_rad = origin.latitude() * M_PI / 180;
double sin_lat = std::sin(lat_rad);
double cos_lat = std::cos(lat_rad);
double cos_d_lon = std::cos(lon_rad - ref_lon_rad);
double ref_sin_lat = std::sin(ref_lat_rad);
double ref_cos_lat = std::cos(ref_lat_rad);
double c =
std::acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
double k = (std::fabs(c) < epsilon) ? 1.0 : (c / std::sin(c));
double x = k * cos_lat * std::sin(lon_rad - ref_lon_rad) * earth_radius;
double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) *
earth_radius;
out.set<0>(x);
out.set<1>(y);
(void)z;
}
template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
earth);
double x = 0, y = 0, z = 0;
proj.Forward(in.latitude(), in.longitude(), 0, x, y, z);
using namespace std;
double lat_rad = in.latitude() * M_PI / 180;
double lon_rad = in.longitude() * M_PI / 180;
double ref_lon_rad = origin.longitude() * M_PI / 180;
double ref_lat_rad = origin.latitude() * M_PI / 180;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref_lon_rad);
double ref_sin_lat = sin(ref_lat_rad);
double ref_cos_lat = cos(ref_lat_rad);
double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));
double x = k * cos_lat * sin(lon_rad - ref_lon_rad) * earth_radius;
double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) *
earth_radius;
out.setX(x);
out.setY(y);
(void)z;
}
template <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
earth);
double lat = 0, lon = 0, alt = 0;
proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
out.setLatitude(lat);
out.setLongitude(lon);
out.setAltitude(alt);
using namespace std;
double x_rad = in.get<0>() / earth_radius;
double y_rad = in.get<1>() / earth_radius;
double c = sqrt(y_rad * y_rad + x_rad * x_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double ref_lon_rad = origin.longitude() * M_PI / 180;
double ref_lat_rad = origin.latitude() * M_PI / 180;
double ref_sin_lat = sin(ref_lat_rad);
double ref_cos_lat = cos(ref_lat_rad);
double lat_rad;
double lon_rad;
if (fabs(c) > epsilon) {
lat_rad = asin(cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c);
lon_rad =
(ref_lon_rad + atan2(x_rad * sin_c, c * ref_cos_lat * cos_c -
y_rad * ref_sin_lat * sin_c));
} else {
lat_rad = ref_lat_rad;
lon_rad = ref_lon_rad;
}
out.setLatitude(lat_rad * 180 / M_PI);
out.setLongitude(lon_rad * 180 / M_PI);
out.setAltitude(origin.altitude());
}
template <class GeoPoint, class Container1, class Container2>
......@@ -225,59 +280,17 @@ bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &jArea);
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
const FPolygon &corridor, FPolygon &jArea,
std::string &errorString);
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
std::string &errorString);
using Transects = vector<FLineString>;
using Progress = vector<int>;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString);
struct TransectInfo {
TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
size_t index;
bool reversed;
};
struct Route {
FLineString path;
std::vector<TransectInfo> info;
};
using Solution =
std::vector<Route>; // Every route corresponds to one run/vehicle
struct RouteParameter {
RouteParameter()
: numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::size_t minNumTransectsPerRun;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
std::vector<Solution> &solutionVector,
const RouteParameter &par = RouteParameter());
namespace detail {
const double offsetConstant =
0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
} // namespace geometry
// operator== and operator!= for boost point
namespace boost {
namespace geometry {
namespace model {
bool operator==(snake::FPoint &p1, snake::FPoint &p2);
bool operator!=(snake::FPoint &p1, snake::FPoint &p2);
bool operator==(snake::IPoint &p1, snake::IPoint &p2);
bool operator!=(snake::IPoint &p1, snake::IPoint &p2);
bool operator==(::geometry::FPoint &p1, ::geometry::FPoint &p2);
bool operator!=(::geometry::FPoint &p1, ::geometry::FPoint &p2);
bool operator==(::geometry::IPoint &p1, ::geometry::IPoint &p2);
bool operator!=(::geometry::IPoint &p1, ::geometry::IPoint &p2);
} // namespace model
} // namespace geometry
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment