//-------------------------------------------------------------------------------------
void CDebugGUI::DrawString(float x, float y, const char* pString, const CColor& Color)
{
XMMATRIX LocalToWorld = XMMatrixIdentity();
m_rUtilityDraw->BeginTriangleList();
float X0 = -1.0f + x * m_ScreenPixelWidth;
float X1 = X0 + 7.0f * m_ScreenPixelWidth;
float Y0 = 1.0f - y * m_ScreenPixelHeight;
float Y1 = Y0 - 16.0f * m_ScreenPixelHeight;
for(unsigned int CharIndex = 0; CharIndex < strlen(pString); CharIndex++)
{
float U0 = (float)(pString[CharIndex] - ' ') * 8.0f * m_FontPixelWidth;
float U1 = U0 + 7.0f * m_FontPixelWidth;
XMFLOAT3 Pt0(X0, Y0, 0.999f);
XMFLOAT3 Pt1(X0, Y1, 0.999f);
XMFLOAT3 Pt2(X1, Y0, 0.999f);
XMFLOAT3 Pt3(X1, Y1, 0.999f);
XMFLOAT2 Uv0(U0, 0.0f);
XMFLOAT2 Uv1(U0, 1.0f);
XMFLOAT2 Uv2(U1, 0.0f);
XMFLOAT2 Uv3(U1, 1.0f);
m_rUtilityDraw->AddTriangle(Pt0, Pt2, Pt1, Uv0, Uv2, Uv1, Color, Color, Color);
m_rUtilityDraw->AddTriangle(Pt1, Pt2, Pt3, Uv1, Uv2, Uv3, Color, Color, Color);
X1 += (1.0f * m_ScreenPixelWidth);
X0 = X1;
X1 += (7.0f * m_ScreenPixelWidth);
}
m_rUtilityDraw->EndTriangleList(LocalToWorld, m_pFontMaterial, CRenderer::eDepthNone, CRenderer::eBlendModulate);
}
//----------------------------------------------------------------------------
void CTimeLine::DrawEvent( CEvent* pEvent,
unsigned int PrevFrame,
unsigned long long ClockFrequency,
unsigned int ThreadIndex,
float MicroSecondsToViewSpace,
float& X0,
float& X1,
float Y0,
float Y1,
float PixelWidth,
float PixelHeight,
const CEventDesc* pEventDesc)
{
unsigned int Index = pEvent->m_Index;
Assert(Index < scMaxEventTypesPerThread);
unsigned long long StartTime;
unsigned long long StopTime;
XMFLOAT2 Uv0(0.0f, 0.0f);
StartTime = MAX(pEvent->m_StartTime, m_FrameStart[PrevFrame]);
if (pEvent->m_State == CEvent::eStopped)
{
StopTime = MIN(pEvent->m_StopTime, m_FrameEnd[PrevFrame]);
}
else
{
StopTime = m_FrameEnd[PrevFrame];
}
unsigned long long CycleTime = StopTime - StartTime;
unsigned long Duration = (unsigned long)(CycleTime * 1000000 / ClockFrequency);
if (m_CurrentThread == ThreadIndex)
{
m_Duration[Index] += Duration;
}
X1 = X0 + MicroSecondsToViewSpace * Duration;
if ((X1 - X0) >= PixelWidth) // Only draw if bigger than a pixel
{
XMFLOAT3 Pt0(X0, Y0 - PixelHeight, 0.999f);
XMFLOAT3 Pt1(X0, Y1 + PixelHeight, 0.999f);
XMFLOAT3 Pt2(X1, Y0 - PixelHeight, 0.999f);
XMFLOAT3 Pt3(X1, Y1 + PixelHeight, 0.999f);
CColor Color = pEventDesc[Index].m_Color;
m_rUtilityDraw->AddTriangle(Pt0, Pt2, Pt1, Uv0, Uv0, Uv0, Color, Color, Color);
m_rUtilityDraw->AddTriangle(Pt1, Pt2, Pt3, Uv0, Uv0, Uv0, Color, Color, Color);
X0 = X1;
}
}
void MPointCreator::ProcessPoints(
const MHTRouteCandidate::PointData& rData1,
const MHTRouteCandidate::RouteSegment& rSegment1,
const MHTRouteCandidate::PointData& rData2,
std::vector<const SimpleLine*>& vecCurvesBetweenPoints)
{
Point Pt1(false);
if (rData1.GetPointProjection() != NULL)
Pt1 = *(rData1.GetPointProjection());
else
Pt1 = rData1.GetPointGPS();
Point Pt2(false);
if (rData2.GetPointProjection() != NULL)
Pt2 = *(rData2.GetPointProjection());
else
Pt2 = rData2.GetPointGPS();
if (AlmostEqual(Pt1, Pt2))
{
Interval<Instant> TimeInterval(rData1.GetTime(),
rData2.GetTime(),
true,
rData1.GetTime() == rData2.GetTime());
AttributePtr<UPoint> pUPoint(new UPoint(TimeInterval, Pt1, Pt1));
m_pResMPoint->Add(*pUPoint);
assert(vecCurvesBetweenPoints.size() == 0);
}
else
{
const shared_ptr<IMMNetworkSection>& pSection1 = rData1.GetSection();
const shared_ptr<IMMNetworkSection>& pSection2 = rData2.GetSection();
if (pSection1 == NULL || !pSection1->IsDefined() ||
pSection2 == NULL || !pSection2->IsDefined())
{
// at least one point is offroad
Interval<Instant> TimeInterval(
rData1.GetTime(),
rData2.GetTime(),
true,
rData1.GetTime() == rData2.GetTime());
AttributePtr<UPoint> pUPoint(new UPoint(TimeInterval, Pt1, Pt2));
m_pResMPoint->Add(*pUPoint);
}
else if (pSection1 == pSection2) // Same section
{
const SimpleLine* pSectionCurve = pSection1->GetCurve();
AttributePtr<SimpleLine> pSubline(new SimpleLine(0));
MMUtil::SubLine(pSectionCurve,
Pt1,
Pt2,
pSection1->GetCurveStartsSmaller(),
m_dNetworkScale,
*pSubline);
if (pSubline->IsDefined() &&
pSubline->StartPoint().IsDefined() &&
pSubline->EndPoint().IsDefined())
{
Interval<Instant> TimeInterval(
rData1.GetTime(),
rData2.GetTime(),
true,
rData1.GetTime() == rData2.GetTime());
//assert(AlmostEqual(Pt1, pSubline->StartPoint()));
ProcessCurve(*pSubline, TimeInterval);
}
}
else // different sections
{
// Calculate total length of curves
const SimpleLine* pSection1Curve = pSection1->GetCurve();
AttributePtr<SimpleLine> pSubline1(new SimpleLine(0));
MMUtil::SubLine(pSection1Curve,
Pt1,
!rSegment1.HasUTurn() ? pSection1->GetEndPoint() :
pSection1->GetStartPoint(),
pSection1->GetCurveStartsSmaller(),
m_dNetworkScale,
*pSubline1);
double dLenCurve1 = MMUtil::CalcLengthCurve(pSubline1.get(),
m_dNetworkScale);
const SimpleLine* pSection2Curve = pSection2->GetCurve();
AttributePtr<SimpleLine> pSubline2(new SimpleLine(0));
MMUtil::SubLine(pSection2Curve,
pSection2->GetStartPoint(),
Pt2,
pSection2->GetCurveStartsSmaller(),
m_dNetworkScale,
*pSubline2);
double dLenCurve2 = MMUtil::CalcLengthCurve(pSubline2.get(),
m_dNetworkScale);
double dLength = dLenCurve1 + dLenCurve2;
const size_t nCurves = vecCurvesBetweenPoints.size();
for (size_t i = 0; i < nCurves; ++i)
{
dLength += MMUtil::CalcLengthCurve(vecCurvesBetweenPoints[i],
m_dNetworkScale);
}
// Total time
DateTime Duration = rData2.GetTime() - rData1.GetTime();
Duration.Abs();
// Process first curve
DateTime TimeEnd(rData1.GetTime());
if (!pSubline1 ||
AlmostEqual(dLenCurve1, 0.0))
{
Interval<Instant> TimeInterval(rData1.GetTime(),
TimeEnd,
true, true);
AttributePtr<UPoint> pUPoint(new UPoint(TimeInterval,
Pt1, Pt1));
m_pResMPoint->Add(*pUPoint);
}
else
{
TimeEnd = rData1.GetTime() +
DateTime(datetime::durationtype,
(uint64_t)(((Duration.millisecondsToNull()
/ dLength) * dLenCurve1) + .5));
Interval<Instant> TimeInterval(rData1.GetTime(),
TimeEnd,
true,
rData1.GetTime() == TimeEnd);
ProcessCurve(*pSubline1, TimeInterval, dLenCurve1);
}
// Process curves in between
for (size_t i = 0; i < nCurves; ++i)
{
const SimpleLine* pCurve = vecCurvesBetweenPoints[i];
if (pCurve == NULL)
continue;
double dLenCurve = MMUtil::CalcLengthCurve(pCurve,
m_dNetworkScale);
if (AlmostEqual(dLenCurve, 0.0))
continue;
DateTime TimeStart = TimeEnd;
TimeEnd = TimeStart +
DateTime(datetime::durationtype,
(uint64_t)(((Duration.millisecondsToNull()
/ dLength) * dLenCurve) + .5));
Interval<Instant> TimeInterval(TimeStart,
TimeEnd,
true, false);
ProcessCurve(*pCurve, TimeInterval, dLenCurve);
}
// Process last curve
if (!pSubline2 ||
AlmostEqual(dLenCurve2, 0.0))
{
DateTime TimeStart = TimeEnd;
TimeEnd = rData2.GetTime();
Interval<Instant> TimeInterval(TimeStart,
TimeEnd,
true, true);
AttributePtr<UPoint> pUPoint(new UPoint(TimeInterval,
Pt2, Pt2));
m_pResMPoint->Add(*pUPoint);
}
else
{
assert(TimeEnd.millisecondsToNull() <=
rData2.GetTime().millisecondsToNull());
DateTime TimeStart = TimeEnd;
TimeEnd = rData2.GetTime();
Interval<Instant> TimeInterval(TimeStart,
TimeEnd,
true, false);
ProcessCurve(*pSubline2, TimeInterval, dLenCurve2);
}
}
}
}
void MPointCreator::ProcessCurve(const SimpleLine& rCurve,
const Interval<Instant> TimeInterval,
double dCurveLength)
{
double dLength = dCurveLength < 0.0 ?
MMUtil::CalcLengthCurve(&rCurve, m_dNetworkScale) :
dCurveLength;
if (AlmostEqual(dLength, 0.0))
{
AttributePtr<UPoint> pUPoint(new UPoint(TimeInterval,
rCurve.StartPoint(),
rCurve.EndPoint()));
m_pResMPoint->Add(*pUPoint);
return;
}
DateTime Duration = TimeInterval.end - TimeInterval.start;
Duration.Abs();
const bool bStartsSmaller = rCurve.GetStartSmaller();
Instant TimeStart(TimeInterval.start);
const int nHalfSegments = rCurve.Size();
if (nHalfSegments <= 0)
return;
assert(nHalfSegments % 2 == 0);
LRS lrs(bStartsSmaller ? 0.0 : rCurve.Length(), 0);
int lrsPosAkt = 0;
if (!const_cast<SimpleLine&>(rCurve).Get(lrs, lrsPosAkt))
{
assert(false);
return;
}
LRS lrsAkt;
rCurve.Get(lrsPosAkt, lrsAkt);
HalfSegment hs;
rCurve.Get(lrsAkt.hsPos, hs);
double dLengthHS = MMUtil::CalcDistance(hs.GetLeftPoint(),
hs.GetRightPoint(),
m_dNetworkScale);
DateTime TimeEnd = TimeStart +
DateTime(datetime::durationtype,
(uint64_t)(((Duration.millisecondsToNull()
/ dLength) * dLengthHS) + .5));
Interval<Instant> TimeIntervalAkt(TimeStart, TimeEnd,
true, TimeStart == TimeEnd);
Point Pt1(false);
Point Pt2(false);
if (const_cast<SimpleLine&>(rCurve).StartsSmaller())
{
Pt1 = hs.GetDomPoint();
Pt2 = hs.GetSecPoint();
}
else
{
Pt1 = hs.GetSecPoint();
Pt2 = hs.GetDomPoint();
}
AttributePtr<UPoint> pUPoint(new UPoint(TimeIntervalAkt, Pt1, Pt2));
m_pResMPoint->Add(*pUPoint);
TimeStart = TimeEnd;
if (bStartsSmaller)
++lrsPosAkt;
else
--lrsPosAkt;
while (lrsPosAkt >= 0 && lrsPosAkt < nHalfSegments / 2)
{
rCurve.Get(lrsPosAkt, lrsAkt);
rCurve.Get(lrsAkt.hsPos, hs);
double dLengthHS = MMUtil::CalcDistance(hs.GetLeftPoint(),
hs.GetRightPoint(),
m_dNetworkScale);
DateTime TimeEnd = TimeStart +
DateTime(datetime::durationtype,
(uint64_t)(((Duration.millisecondsToNull()
/ dLength) * dLengthHS) + .5));
Interval<Instant> TimeIntervalAkt(TimeStart, TimeEnd,
true, TimeStart == TimeEnd);
if (AlmostEqual(Pt2, hs.GetDomPoint()))
{
Pt1 = hs.GetDomPoint();
Pt2 = hs.GetSecPoint();
}
else
{
Pt1 = hs.GetSecPoint();
Pt2 = hs.GetDomPoint();
}
/*if (AlmostEqual(Pt2, rCurve.EndPoint()))
TimeStart = TimeInterval.end;*/
AttributePtr<UPoint> pUPoint(new UPoint(TimeIntervalAkt, Pt1, Pt2));
m_pResMPoint->Add(*pUPoint);
TimeStart = TimeEnd;
if (bStartsSmaller)
++lrsPosAkt;
else
--lrsPosAkt;
}
}
int main(int argc, char * argv[])
{
if(argc != 1 && argc != 3)
{
std::cout << "[ USAGE ]: " << argv[0] << " [<Image Size> = 1000] [<nPoints> = 500]" << std::endl;
return -1;
}
int Side = 1000;
int nPoints = 500;
if(argc == 3)
{
Side = std::atoi(argv[1]);
nPoints = std::atoi(argv[2]);
}
cv::Mat Canvas(Side, Side, CV_8UC3);
Canvas.setTo(255);
// Randomly generate points in a 2D plane roughly aligned in a line for testing
std::random_device SeedDevice;
std::mt19937 RNG = std::mt19937(SeedDevice());
std::uniform_int_distribution<int> UniDist(0, Side-1); // [Incl, Incl]
int Perturb = 25;
std::normal_distribution<GRANSAC::VPFloat> PerturbDist(0, Perturb);
std::vector<std::shared_ptr<GRANSAC::AbstractParameter>> CandPoints;
for(int i = 0; i < nPoints; ++i)
{
int Diag = UniDist(RNG);
cv::Point Pt(floor(Diag + PerturbDist(RNG)), floor(Diag + PerturbDist(RNG)));
cv::circle(Canvas, Pt, floor(Side / 100), cv::Scalar(0, 0, 0), -1);
std::shared_ptr<GRANSAC::AbstractParameter> CandPt = std::make_shared<Point2D>(Pt.x, Pt.y);
CandPoints.push_back(CandPt);
}
GRANSAC::RANSAC<Line2DModel, 2> Estimator;
Estimator.Initialize(20, 100); // Threshold, iterations
int start = cv::getTickCount();
Estimator.Estimate(CandPoints);
int end = cv::getTickCount();
std::cout << "RANSAC took: " << GRANSAC::VPFloat(end-start) / GRANSAC::VPFloat(cv::getTickFrequency()) * 1000.0 << " ms." << std::endl;
auto BestInliers = Estimator.GetBestInliers();
if(BestInliers.size() > 0)
{
for(auto& Inlier : BestInliers)
{
auto RPt = std::dynamic_pointer_cast<Point2D>(Inlier);
cv::Point Pt(floor(RPt->m_Point2D[0]), floor(RPt->m_Point2D[1]));
cv::circle(Canvas, Pt, floor(Side / 100), cv::Scalar(0, 255, 0), -1);
}
}
auto BestLine = Estimator.GetBestModel();
if(BestLine)
{
auto BestLinePt1 = std::dynamic_pointer_cast<Point2D>(BestLine->GetModelParams()[0]);
auto BestLinePt2 = std::dynamic_pointer_cast<Point2D>(BestLine->GetModelParams()[1]);
if(BestLinePt1 && BestLinePt2)
{
cv::Point Pt1(BestLinePt1->m_Point2D[0], BestLinePt1->m_Point2D[1]);
cv::Point Pt2(BestLinePt2->m_Point2D[0], BestLinePt2->m_Point2D[1]);
DrawFullLine(Canvas, Pt1, Pt2, cv::Scalar(0, 0, 255), 2);
}
}
while(true)
{
cv::imshow("RANSAC Example", Canvas);
char Key = cv::waitKey(1);
if(Key == 27)
return 0;
if(Key == ' ')
cv::imwrite("LineFitting.png", Canvas);
}
return 0;
}
