//------------------------------------------------------------------------------------- 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; }