pcm_data.cpp 13 KB
Newer Older
1
2
3
4
5
6
7
8
9
/*********************************************************************
* Copyright (c) 2017, 2018 ITK Engineering GmbH
*
* This program and the accompanying materials are made
* available under the terms of the Eclipse Public License 2.0
* which is available at https://www.eclipse.org/legal/epl-2.0/
*
* SPDX-License-Identifier: EPL-2.0
**********************************************************************/
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

#include "pcm_data.h"

PCM_Data::~PCM_Data()
{
    Clear();
}

bool PCM_Data::AddPCM_Marks(const PCM_Marks *marks)
{
    marksVec.push_back(marks);
    return true;
}

bool PCM_Data::SetPCM_Object(const PCM_Object *object)
{
    this->object = object;
    return true;
}

bool PCM_Data::SetPCM_ViewObject(const PCM_ViewObject *viewObject)
{
    this->viewObject = viewObject;
    return true;
}

bool PCM_Data::SetPCM_GlobalData(const PCM_GlobalData *globalData)
{
    this->globalData = globalData;
    return true;
}

42
43
44
45
46
47
bool PCM_Data::SetCallbacks(const CallbackInterface *callbacks)
{
    this->callbacks = callbacks;
    return true;
}

Dmitri Fix's avatar
#1051    
Dmitri Fix committed
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
bool PCM_Data::AddPCM_Course(const PCM_Course *course)
{
    intendedCourses.push_back(course);
    return true;
}

std::vector<const PCM_Marks *> *PCM_Data::GetMarksVec()
{
    return &marksVec;
}

const PCM_Marks *PCM_Data::GetMarksOfType(MarkType markType) const
{
    for (const PCM_Marks *marks : marksVec)
    {
        if (marks != nullptr)
        {
            if (marks->GetMarkType() == markType)
            {
                return marks;
            }
        }
    }
    return nullptr;
}

bool PCM_Data::AddPCM_Agent(int id, double xPos, double yPos, double yawAngle, double width,
                            double height)
{
77
78
//    LOGINFO(QString().sprintf("To dd PCM_Agent %d with width %.2f and height %.2f", id, width, height).toStdString());

Dmitri Fix's avatar
#1051    
Dmitri Fix committed
79
80
81
82
    PCM_Agent *agent = new PCM_Agent(id);
    PCM_Line *line = new PCM_Line(0);

    std::vector<Common::Vector2d> pointVector;
Dmitri Fix's avatar
Dmitri Fix committed
83
84
85
86
    pointVector.push_back(Common::Vector2d(width / 2, height / 2));// in vehicle coordinates
    pointVector.push_back(Common::Vector2d(-width / 2, height / 2));// in vehicle coordinates
    pointVector.push_back(Common::Vector2d(-width / 2, -height / 2));// in vehicle coordinates
    pointVector.push_back(Common::Vector2d(width / 2, -height / 2));// in vehicle coordinates
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
87
88
89
90

    int count = 0;
    for (Common::Vector2d point : pointVector)
    {
Dmitri Fix's avatar
Dmitri Fix committed
91
92
        point.Rotate(yawAngle);//from vehicle to world coordinates
        point.Translate(xPos, yPos);//from vehicle to world coordinates
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
        line->AddPCM_Point(new PCM_Point(count++, point.x, point.y));
    }

    agent->AddPCM_Line(line);
    agentVec.push_back(agent);

    return true;
}

void PCM_Data::ClearAgentData()
{
    for (const PCM_Agent *agent : agentVec)
    {
        delete agent;
    }
    agentVec.clear();
}

void PCM_Data::Clear()
{
    for (const PCM_Marks *marks : marksVec)
    {
        if (marks != nullptr)
        {
            delete marks;
        }
    }
    marksVec.clear();

    if (object != nullptr)
    {
        delete object;
        object = nullptr;
    }

    if (viewObject != nullptr)
    {
        delete viewObject;
        viewObject = nullptr;
    }

    for (const PCM_Course *course : intendedCourses)
    {
        if (course != nullptr)
        {
            delete course;
        }
    }
    intendedCourses.clear();

    if (globalData != nullptr)
    {
        delete globalData;
        globalData = nullptr;
    }

    ClearAgentData();
}

MarkType PCM_Data::GetMarkTypeOfNearestLineSegment(const PCM_Point *point,
153
                                                   double viewAngle,
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
154
155
156
157
158
159
160
                                                   double range) const
{
    MarkType minMarkType = MarkType::NONE;
    double minDistance = INFINITY;
    for (int i = 1; i < static_cast<int>(MarkType::NumberOfMarkTypes); i++)
    {
        MarkType currentType = static_cast<MarkType>(i);
161
162
        PCM_LineSegment lineSegment = GetNearestLineSegmentOfMarks(currentType, point,
                                                   viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
163

164
        double distance = lineSegment.CalcDistanceFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
165
166
167
168
169
170
171
172
173
174
175

        if ((minDistance > distance) && (!std::isinf(distance)))
        {
            minDistance = distance;
            minMarkType = currentType;
        }
    }

    return minMarkType;
}

176
177
AgentDetection PCM_Data::GetNearestAgent(int egoId, const PCM_Point *point,
                                         double viewAngle, double range) const
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
178
{
179
180
181
182
183
184
185
186
187
    AgentDetection detection;
    if((egoId<0) || (point == nullptr))
        return detection;

    detection.egoId = egoId;
    detection.egoPoint = point;
    detection.viewAngle = viewAngle;
    detection.range = range;

Dmitri Fix's avatar
#1051    
Dmitri Fix committed
188
189
190
191
    for (int i = 0; i < static_cast<int>(agentVec.size()); i++)
    {
        if (i != egoId)
        {
192
193
194
            // find nearest line segment from point to the agent i (agent i must not be ego)
            PCM_LineSegment fullLineSegment = GetNearestLineSegmentOfAgents(i, egoId, point, viewAngle, range, false);
            PCM_LineSegment subLineSegment = fullLineSegment.CalcSubLineSegmentInViewRange(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
195

196
197
198
199
//            LOGINFO(QString().sprintf("PCM_Data: GetNearestAgent: ego%d (%.2f, %.2f), (%.2f, %.2f) viewAngle %.2f, range %.2f", egoId,
//                          fullLineSegment.GetFirstPoint().GetX(), fullLineSegment.GetFirstPoint().GetY(),
//                          fullLineSegment.GetSecondPoint().GetX(), fullLineSegment.GetSecondPoint().GetY(), viewAngle, range).toStdString());
//            LOGINFO(QString().sprintf("PCM_Data: GetNearestAgent: ego%d, line length %f", egoId, subLineSegment.CalculateLength()).toStdString());
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
200

201
202
203
            double distance = subLineSegment.CalcDistanceFromPoint(point, viewAngle, range);

            if ((!std::isinf(distance)) && (distance < detection.distance))
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
204
            {
205
206
207
208
209
210
211
212
213
                detection.oppId = i;
                detection.fullOppLineSegment = fullLineSegment;
                detection.subOppLineSegment = subLineSegment;
                detection.oppPoint = subLineSegment.GetNearestPointFromPoint(point, viewAngle, range);
                detection.distance = distance;

//                if(fullLineSegment.CalculateLength() > subLineSegment.CalculateLength())
//                    LOGINFO(QString().sprintf("GetNearestAgent: subOppLineSegment (%f m) is shorter than fullOppLineSegment (%f m)",
//                                              subLineSegment.CalculateLength(), fullLineSegment.CalculateLength()).toStdString());
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
214
215
216
            }
        }
    }
217
    return detection;
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
218
219
}

220
ObjectType PCM_Data::GetObjectTypeOfNearestLineSegment(const PCM_Point *point, double viewAngle,
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
221
222
223
224
225
226
227
                                                       double range) const
{
    ObjectType minObjectType = ObjectType::NONE;
    double minDistance = INFINITY;
    for (int i = 1; i < static_cast<int>(ObjectType::NumberOfObjectTypes); i++)
    {
        ObjectType currentType = static_cast<ObjectType>(i);
228
229
        PCM_LineSegment lineSegment = GetNearestLineSegmentOfObject(currentType, point,
                                                    viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
230

231
        double distance = lineSegment.CalcDistanceFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
232
233
234
235
236
237
238
239
240
241
242
243

        if ((minDistance > distance) && (!std::isinf(distance)))
        {
            minDistance = distance;
            minObjectType = currentType;
        }
    }

    return minObjectType;
}

PCM_LineSegment PCM_Data::GetNearestLineSegmentOfMarks(MarkType markType, const PCM_Point *point,
244
                                                       double viewAngle, double range) const
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
{
    PCM_LineSegment minLineSegment;

    if (point == nullptr)
    {
        return minLineSegment;
    }

    double minDistance = INFINITY;

    // if no mark type, than find minimal distance for all existing mark types
    if (markType == MarkType::NONE)
    {
        for (int i = 1; i < static_cast<int>(MarkType::NumberOfMarkTypes); i++)
        {
260
261
            PCM_LineSegment lineSegment = GetNearestLineSegmentOfMarks(static_cast<MarkType>(i), point,
                                                       viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
262

263
            double distance = lineSegment.CalcDistanceFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
264
265
266
267
268
269
270
271
272
273
274

            if ((minDistance > distance) && (!std::isinf(distance)))
            {
                minDistance = distance;
                minLineSegment = lineSegment;
            }
        }
    }
    else
    {
        const PCM_Marks *marks = GetMarksOfType(markType);
275
        minLineSegment = marks->GetNearestLineSegment(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
276
277
278
279
280
    }

    return minLineSegment;
}

281
282
PCM_LineSegment PCM_Data::GetNearestLineSegmentOfAgents(int agentId, int agentIdExclude, const PCM_Point *point,
                                                        double viewAngle, double range, bool calculateSubLine) const
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
283
284
285
286
287
288
289
290
291
292
293
294
{
    PCM_LineSegment minLineSegment;

    if (point == nullptr)
    {
        return minLineSegment;
    }

    double minDistance = INFINITY;

    for (int  i = 0; i < static_cast<int>(agentVec.size()); i++)
    {
295
296
        if (((agentId >= 0) && (i == agentId))
                || ((agentIdExclude >= 0) && (i != agentIdExclude)) )
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
297
        {
298
            PCM_LineSegment lineSegment = agentVec.at(i)->GetNearestLineSegment(point, viewAngle, range, calculateSubLine);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
299

300
            double distance = lineSegment.CalcDistanceFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
301
302
303
304
305
306

            if ((minDistance > distance) && (!std::isinf(distance)))
            {
                minDistance = distance;
                minLineSegment = lineSegment;
            }
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321

            //debug
//            QString logStr = QString().sprintf("debug: lines of agent %d", i);
//            const std::map<int, PCM_Line *> *lineMap = agentVec.at(i)->GetLineMap();
//            for (std::pair<int, PCM_Line *> pcmLinePair : *lineMap)
//            {
//                PCM_Line *line = pcmLinePair.second;
//                const std::map<int, const PCM_Point *> *pointMap = line->GetPointMap();
//                for (std::pair<int, const PCM_Point *> pcmPointPair : *pointMap)
//                {
//                    const PCM_Point *point = pcmPointPair.second;
//                    logStr += QString().sprintf(" point (%.2f, %.2f)", point->GetX(), point->GetY());
//                }
//            }
//            LOGINFO(logStr.toStdString());
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
322
323
324
325
326
327
328
        }
    }

    return minLineSegment;
}

PCM_LineSegment PCM_Data::GetNearestLineSegmentOfObject(ObjectType objectType,
329
                                                        const PCM_Point *point, double viewAngle, double range) const
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
{
    PCM_LineSegment minLineSegment;

    if (point == nullptr)
    {
        return minLineSegment;
    }

    double minDistance = INFINITY;

    // if no mark type, than find minimal distance for all existing mark types
    if (objectType == ObjectType::NONE)
    {
        for (int i = 1; i < static_cast<int>(ObjectType::NumberOfObjectTypes); i++)
        {
345
346
            PCM_LineSegment lineSegment = GetNearestLineSegmentOfObject(static_cast<ObjectType>(i), point,
                                                        viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
347

348
            double distance = lineSegment.CalcDistanceFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
349
350
351
352
353
354
355
356
357
358
359
360
361

            if ((minDistance > distance) && (!std::isinf(distance)))
            {
                minDistance = distance;
                minLineSegment = lineSegment;
            }
        }
    }
    else
    {
        switch (objectType)
        {
        case ObjectType::OBJECT:
362
            minLineSegment = object->GetNearestLineSegment(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
363
364
            break;
        case ObjectType::VIEWOBJECT:
365
            minLineSegment = viewObject->GetNearestLineSegment(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
366
367
368
369
370
371
372
373
374
375
            break;
        default:
            break;
        }
    }

    return minLineSegment;
}

PCM_Point PCM_Data::GetNearestPointOfMarks(MarkType markType, const PCM_Point *point,
376
                                           double viewAngle,
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
377
378
                                           double range) const
{
379
    PCM_LineSegment minLineSegment = GetNearestLineSegmentOfMarks(markType, point, viewAngle,
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
380
                                                                  range);
381
    return minLineSegment.GetNearestPointFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
382
383
}

384
PCM_Point PCM_Data::GetNearestPointOfAgents(int agentId, int agentIdExclude, const PCM_Point *point, double viewAngle,
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
385
386
                                            double range) const
{
387
388
    PCM_LineSegment minLineSegment = GetNearestLineSegmentOfAgents(agentId, agentIdExclude, point, viewAngle, range);
    return minLineSegment.GetNearestPointFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
389
390
391
}

PCM_Point PCM_Data::GetNearestPointOfObject(ObjectType objectType, const PCM_Point *point,
392
                                            double viewAngle, double range) const
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
393
{
394
    PCM_LineSegment minLineSegment = GetNearestLineSegmentOfObject(objectType, point, viewAngle,
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
395
                                                                   range);
396
    return minLineSegment.GetNearestPointFromPoint(point, viewAngle, range);
Dmitri Fix's avatar
#1051    
Dmitri Fix committed
397
398
399
400
401
402
403
404
405
406
407
}

const PCM_Object *PCM_Data::GetObject() const
{
    return object;
}

const PCM_ViewObject *PCM_Data::GetViewObject() const
{
    return viewObject;
}