Commit c09204f9 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

Merge branch '89-visualize-cost-functions' into 'master'

Resolve "Visualize cost functions"

Closes #89

See merge request !109
parents 4177748f 21a89535
......@@ -67,6 +67,16 @@ Vector2D Policy::ComputeAcceleration(Agent* agent, WorldBase * world)
return (bestVelocity - currentVelocity) / std::max(relaxationTime_, dt);
}
float Policy::ComputeCostForVelocity(const Vector2D& velocity, Agent* agent, WorldBase* world)
{
// compute the cost for this velocity
float totalCost = 0;
for (auto& costFunction : cost_functions_)
totalCost += costFunction.second * costFunction.first->GetCost(velocity, agent, world);
return totalCost;
}
Vector2D Policy::getAccelerationFromGradient(Agent* agent, WorldBase * world)
{
const Vector2D& CurrentVelocity = agent->getVelocity();
......
......@@ -138,6 +138,13 @@ public:
/// <param name="world">The world in which the simulation takes place.</param>
Vector2D ComputeAcceleration(Agent* agent, WorldBase* world);
/// <summary>Computes and returns the cost that this Policy assigns to a (hypothetical) velocity for a given agent.
/// This is useful for e.g. visualizing a cost function. For most purposes, ComputeNewVelocity() is better because it encapsulates more work.</summary>
/// <param name="velocity">A velocity vector for which the cost should be computed.</param>
/// <param name="agent">The agent for which a new velocity should be computed.</param>
/// <param name="world">The world in which the simulation takes place.</param>
float ComputeCostForVelocity(const Vector2D& velocity, Agent* agent, WorldBase* world);
/// <summary>Computes and returns a 2D vector describing the contact forces experienced by a given agent due to collisions.
/// This force is already scaled by the scaling factor stored in this Policy.</summary>
/// <param name="agent">The agent for which a new velocity should be computed.</param>
......
......@@ -37,6 +37,7 @@
#include <tools/HelperFunctions.h>
#include <Engine/core/worldToric.h>
#include <Engine/core/policy.h>
// Vertex shader: calculates the position of a vertex in the OpenGL view, and passes a color onto the fragment shader.
static const char *vertexShaderSource =
......@@ -85,6 +86,8 @@ UMANSOpenGLWidget::UMANSOpenGLWidget(QWidget *parent) : QOpenGLWidget(parent), s
writeCSVOutput = false;
makeScreenshotsPerFrame = false;
showCostFunction = false;
setFocusPolicy(Qt::FocusPolicy::ClickFocus);
mainApplication = (UMANSQtGuiApplication*)parent;
......@@ -194,6 +197,13 @@ void UMANSOpenGLWidget::ToggleGrid()
update();
}
void UMANSOpenGLWidget::ToggleShowCostFunction()
{
// show or hide the cost function of the selected agent
showCostFunction = !showCostFunction;
update();
}
void UMANSOpenGLWidget::SetPlaybackMultiplier(int value)
{
playbackMultiplier = value;
......@@ -359,6 +369,17 @@ void UMANSOpenGLWidget::addSegmentsToBuffer(const std::vector<LineSegment2D>& se
}
}
void UMANSOpenGLWidget::addContourToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth)
{
auto vis = visualizationData[target];
for (size_t i = 0; i < points.size(); ++i)
{
vis->AddData(pointToQVector3D(points[i], depth), color);
vis->AddData(pointToQVector3D(points[(i + 1) % points.size()], depth), color);
}
}
void UMANSOpenGLWidget::addPointsToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth)
{
auto vis = visualizationData[target];
......@@ -380,7 +401,7 @@ void UMANSOpenGLWidget::drawSimulation()
drawAgent(*agent);
}
void UMANSOpenGLWidget::drawAgent(const Agent& agent)
void UMANSOpenGLWidget::drawAgent(Agent& agent)
{
const bool isActiveAgent = activeAgent != nullptr && agent.getID() == activeAgent->getID();
......@@ -412,6 +433,10 @@ void UMANSOpenGLWidget::drawAgent(const Agent& agent)
// draw the goal
addPointsToBuffer(approximateDisk_Triangles(agent.getGoal(), radius, 8), QColor(0, 255, 128), Target_Agents_Solid, Depth_Agents);
// draw the values of the agent's cost function
if (showCostFunction)
drawAgentCostCircle(agent);
}
/*// draw the trajectory that the agent has traversed so far
......@@ -421,6 +446,129 @@ void UMANSOpenGLWidget::drawAgent(const Agent& agent)
addPointsToBuffer(approximateDisk_Triangles(pt, radius / 3.0), agentQColorLight, Target_DynamicData_Solid, Depth_Obstacles);*/
}
QColor colorFromCost(float cost, float minCost, float maxCost)
{
// convert the cost to a fraction in [0,1] between min and max
const float minCost2 = std::max(minCost, -100.f); //0
const float maxCost2 = std::min(maxCost, 100.f); //50
const float cost2 = std::min(std::max(cost, minCost2), maxCost2);
float frac = (cost2 - minCost2) / (maxCost2 - minCost2);
frac = HelperFunctions::Clamp(frac, 0.f, 1.f);
if (isnan(cost) || minCost2 >= maxCost2)
frac = 1;
// convert this to a HSV representation: from blue to purple
const int minH = 360 + 250; const int maxH = 300;
int H = (int)(minH + frac * (maxH - minH)) % 360;
return QColor::fromHsv(H, 255, 255);
}
void UMANSOpenGLWidget::drawAgentCostCircle_AddPoint(const Vector2D& basePos, const std::pair<Vector2D, float>& data, const float minCost, const float maxCost)
{
Vector2D v(basePos + data.first);
QVector3D vec3D(v.x, v.y, (float)(Depth_Agents - 0.01));
visualizationData[Target_Agents_Solid]->AddData(vec3D, colorFromCost(data.second, minCost, maxCost));
}
void UMANSOpenGLWidget::drawAgentCostCircle(Agent& agent)
{
// - collect cost values for a set of sample velocities
WorldBase* world = simulator->GetWorld();
const float maxSpeed = agent.getMaximumSpeed();
const float ds = 0.025f;
const int dA = 1;
std::vector<std::vector<std::pair<Vector2D, float>>> costs;
Vector2D zeroVector(0, 0);
const float zeroVectorCost = agent.getPolicy()->ComputeCostForVelocity(zeroVector, &agent, world);
float minCost = zeroVectorCost;
Vector2D minCostVelocity(0, 0);
float maxCost = zeroVectorCost;
costs.push_back({ {zeroVector, zeroVectorCost } });
for (float s = ds; s < maxSpeed; s += ds)
{
Vector2D base(s, 0);
std::vector<std::pair<Vector2D, float>> costsForSpeed;
for (int A = 0; A < 360; A += dA)
{
float angleRadians = (float)(A / 180.0 * PI);
const Vector2D& v = rotateCounterClockwise(base, angleRadians);
float cost = agent.getPolicy()->ComputeCostForVelocity(v, &agent, world);
minCost = std::min(cost, minCost);
if (cost == minCost)
minCostVelocity = v;
maxCost = std::max(cost, maxCost);
costsForSpeed.push_back({ v, cost });
}
costs.push_back(costsForSpeed);
}
// - draw these values in a circle
const Vector2D& agentPos = agent.getPosition();
for (size_t i = 1; i < costs.size(); ++i)
{
const auto& currentList = costs[i];
const auto& prevList = costs[i - 1];
for (size_t j = 0; j < currentList.size(); ++j)
{
const auto& data1 = currentList[j];
const auto& data2 = currentList[(j + 1) % currentList.size()];
if (i == 1)
{
// 1 triangle
const auto& data3 = prevList[0];
drawAgentCostCircle_AddPoint(agentPos, data1, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data2, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data3, minCost, maxCost);
}
else
{
// 2 triangles
const auto& data3 = prevList[(j + 1) % prevList.size()];
const auto& data4 = prevList[j];
drawAgentCostCircle_AddPoint(agentPos, data1, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data2, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data3, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data1, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data3, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data4, minCost, maxCost);
}
}
}
// - highlight certain velocities
addContourToBuffer(
approximateDisk_Outline(agentPos + agent.getPreferredVelocity(), 0.1f),
QColor(128, 128, 128), Target_Agents_Contours, Depth_Agents + 0.2);
addContourToBuffer(
approximateDisk_Outline(agentPos + minCostVelocity, 0.1f),
QColor(255, 255, 255), Target_Agents_Contours, Depth_Agents + 0.2);
// - draw ORCA lines, if they exist
const auto& orcaSolution = agent.GetOrcaSolution();
const QColor& orcaColor = orcaSolution.isFeasible ? QColor(0, 0, 0) : QColor(255, 0, 0);
for (const auto& line : orcaSolution.orcaLines)
{
Vector2D dir = line.direction.getnormalized();
Vector2D drawLinePoint = agentPos + line.point;
addSegmentsToBuffer({ LineSegment2D(drawLinePoint - 2 * dir, drawLinePoint + 2 * dir) }, orcaColor, Target_Agents_Contours, Depth_Agents + 0.2);
}
}
void UMANSOpenGLWidget::drawEnvironment(const bool refresh)
{
const WorldBase* world = simulator->GetWorld();
......@@ -459,13 +607,14 @@ void UMANSOpenGLWidget::drawGrid(bool refresh)
bbox = dynamic_cast<const WorldToric*>(world)->GetBoundingBox();
// draw the bounding box in a different color
std::vector<LineSegment2D> grid_boundary = {
LineSegment2D({bbox.first.x, bbox.first.y }, {bbox.first.x, bbox.second.y}),
LineSegment2D({bbox.second.x, bbox.first.y }, {bbox.second.x, bbox.second.y}),
LineSegment2D({bbox.first.x, bbox.first.y }, {bbox.second.x, bbox.first.y }),
LineSegment2D({bbox.first.x, bbox.second.y}, {bbox.second.x, bbox.second.y})
std::vector<Vector2D> grid_boundary = {
{bbox.first.x, bbox.first.y },
{bbox.first.x, bbox.second.y},
{bbox.second.x, bbox.second.y},
{bbox.second.x, bbox.first.y }
};
addSegmentsToBuffer(grid_boundary, QColor(255, 0, 0), Target_Grid, Depth_GridBoundary);
addContourToBuffer(grid_boundary, QColor(255, 0, 0), Target_Grid, Depth_GridBoundary);
}
// - otherwise, use a default range of -100 to +100
else
......
......@@ -50,6 +50,7 @@ public slots:
void ToggleCSVOutput();
void ZoomToFit();
void ToggleGrid();
void ToggleShowCostFunction();
void ToggleScreenshots();
void MakeScreenshot();
......@@ -78,11 +79,16 @@ private:
QSizeF getScreenToWorldScale() const;
void drawSimulation();
void drawAgent(const Agent& agent);
void drawAgent(Agent& agent);
void drawAgentCostCircle(Agent& agent);
void drawAgentCostCircle_AddPoint(const Vector2D& basePos, const std::pair<Vector2D, float>& data, const float minCost, const float maxCost);
void drawEnvironment(const bool refresh = false);
void drawGrid(const bool refresh = false);
void addPointsToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth);
void addSegmentsToBuffer(const std::vector<LineSegment2D>& segments, const QColor& color, const std::string& target, const double depth);
void addContourToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth);
void updateCursor();
void updateSimulationTimerText();
......@@ -112,6 +118,7 @@ private:
bool makeScreenshotsPerFrame;
bool writeCSVOutput;
bool showCostFunction;
/// <summary>The part of the world that is currently being shown in the OpenGL widget.</summary>
QRectF viewBounds;
......
......@@ -84,7 +84,7 @@
<x>10</x>
<y>300</y>
<width>231</width>
<height>221</height>
<height>201</height>
</rect>
</property>
<property name="title">
......@@ -97,7 +97,7 @@
<property name="geometry">
<rect>
<x>10</x>
<y>90</y>
<y>80</y>
<width>61</width>
<height>31</height>
</rect>
......@@ -116,7 +116,7 @@
<property name="geometry">
<rect>
<x>80</x>
<y>90</y>
<y>80</y>
<width>71</width>
<height>31</height>
</rect>
......@@ -132,7 +132,7 @@
<property name="geometry">
<rect>
<x>10</x>
<y>140</y>
<y>120</y>
<width>201</width>
<height>20</height>
</rect>
......@@ -170,9 +170,9 @@
<property name="geometry">
<rect>
<x>10</x>
<y>50</y>
<y>40</y>
<width>201</width>
<height>21</height>
<height>31</height>
</rect>
</property>
<property name="font">
......@@ -204,7 +204,7 @@
<property name="geometry">
<rect>
<x>10</x>
<y>170</y>
<y>150</y>
<width>211</width>
<height>22</height>
</rect>
......@@ -238,7 +238,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>190</y>
<y>170</y>
<width>31</width>
<height>20</height>
</rect>
......@@ -254,7 +254,7 @@
<property name="geometry">
<rect>
<x>200</x>
<y>190</y>
<y>170</y>
<width>31</width>
<height>20</height>
</rect>
......@@ -269,8 +269,8 @@
<widget class="QLabel" name="Label_SpeedSlider_B">
<property name="geometry">
<rect>
<x>66</x>
<y>190</y>
<x>67</x>
<y>170</y>
<width>31</width>
<height>20</height>
</rect>
......@@ -286,7 +286,7 @@
<property name="geometry">
<rect>
<x>133</x>
<y>190</y>
<y>170</y>
<width>31</width>
<height>20</height>
</rect>
......@@ -305,7 +305,7 @@
<property name="geometry">
<rect>
<x>160</x>
<y>90</y>
<y>80</y>
<width>61</width>
<height>31</height>
</rect>
......@@ -367,9 +367,9 @@
<property name="geometry">
<rect>
<x>10</x>
<y>540</y>
<y>510</y>
<width>231</width>
<height>101</height>
<height>141</height>
</rect>
</property>
<property name="title">
......@@ -379,7 +379,7 @@
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<y>90</y>
<width>211</width>
<height>31</height>
</rect>
......@@ -410,6 +410,28 @@
<bool>true</bool>
</property>
</widget>
<widget class="QCheckBox" name="CheckBox_ToggleShowCostFunction">
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<width>221</width>
<height>17</height>
</rect>
</property>
<property name="toolTip">
<string>Show or hide a grid that helps understand the size of the environment.</string>
</property>
<property name="text">
<string>Show cost function of selected agent</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</widget>
<widget class="QGroupBox" name="Group_Controls">
<property name="geometry">
......@@ -625,16 +647,20 @@
<slot>ZoomToFit()</slot>
<slot>ToggleScreenshots()</slot>
<slot>ToggleGrid()</slot>
<slot>ToggleShowCostFunction()</slot>
</slots>
</customwidget>
</customwidgets>
<tabstops>
<tabstop>Button_LoadScenario</tabstop>
<tabstop>CheckBox_ToggleCSVOutput</tabstop>
<tabstop>CheckBox_ToggleScreenshots</tabstop>
<tabstop>Button_Play</tabstop>
<tabstop>Button_Pause</tabstop>
<tabstop>Button_Reset</tabstop>
<tabstop>Slider_Speed</tabstop>
<tabstop>CheckBox_ToggleGrid</tabstop>
<tabstop>CheckBox_ToggleShowCostFunction</tabstop>
<tabstop>Button_ZoomToDefault</tabstop>
</tabstops>
<resources>
......@@ -745,7 +771,7 @@
<hints>
<hint type="sourcelabel">
<x>1190</x>
<y>570</y>
<y>540</y>
</hint>
<hint type="destinationlabel">
<x>949</x>
......@@ -785,5 +811,21 @@
</hint>
</hints>
</connection>
<connection>
<sender>CheckBox_ToggleShowCostFunction</sender>
<signal>clicked()</signal>
<receiver>SimulationView</receiver>
<slot>ToggleShowCostFunction()</slot>
<hints>
<hint type="sourcelabel">
<x>989</x>
<y>576</y>
</hint>
<hint type="destinationlabel">
<x>927</x>
<y>579</y>
</hint>
</hints>
</connection>
</connections>
</ui>
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