Approximating the speed of a vehicle with multiple known lengths in a scene

Viewed 47

I'm trying to get an approximate speed for a moving vehicle by using known lengths in the scene. Having a camera pointed at a scene similar to the one below; Scene depiction

I've measured the width and and height of a section of road and also measured the height of a stop sign at the other end of the road. With these lengths as my input I'm using opencv to track a vehicle along the road and leave "trajectory points" while tracking it to denote where the car has been.

    if(Trajectory.size() >6)
    {
        auto firstTraj = Trajectory.at(Trajectory.size() - 6);
        auto lastTraj = Trajectory.last();

        const QPointF firstPoint(lastTraj.GetCenterPoint().x, lastTraj.GetCenterPoint().y);
        const QPointF secondPoint(firstTraj.GetCenterPoint().x, firstTraj.GetCenterPoint().y);

        const double midPointX = (lastTraj.GetCenterPoint().x + firstTraj.GetCenterPoint().x)/2;
        const double midPointY = (lastTraj.GetCenterPoint().y + firstTraj.GetCenterPoint().y)/2;

    
        const double distanceX = QLineF(firstPoint, secondPoint).dx();
        const double distanceY = QLineF(firstPoint, secondPoint).dy();

        const CprPoint32f cprMidPoint(midPointX/704, midPointY/576);

        const CprFloat_32 horizontalDistance = scb->GetRealWorldSize(cprMidPoint, distanceX/704, Capra::ComputerVision::CprECalibrationLineDirection::CPR_CALIBRATION_HORIZONTAL,nullptr);
        const CprFloat_32 verticalDistance = scb->GetRealWorldSize(cprMidPoint, distanceY/576, Capra::ComputerVision::CprECalibrationLineDirection::CPR_CALIBRATION_VERTICAL, nullptr);

        const float secondsSecondsSinceEpoch = lastTraj.TimeStamp;
        const float firstSecondsSinceEpoch = firstTraj.TimeStamp;

        const float seconds = std::abs(secondsSecondsSinceEpoch - firstSecondsSinceEpoch);

        const double horizontalCmPerSecond = horizontalDistance / seconds;
        const double verticalCmPerSecond = verticalDistance / seconds;

        const double horizontalKmPerHour = horizontalCmPerSecond / 27.778;
        const double verticalKmPerHour = verticalCmPerSecond / 27.778;

        if(horizontalKmPerHour != 0 && verticalKmPerHour !=0 && horizontalKmPerHour <= 150 && verticalKmPerHour <= 150)
        {
            ObjectPropertySpeed speed;
            speed.Horizontal = horizontalKmPerHour;
            speed.Vertical = verticalKmPerHour;
            ObjectSpeed.push_back(speed);
        }
}

I am then taking the 2D distances between each pair of trajectory points dividing them by the resolution of the image and sending them into the GetRealWorldSize function to get their real world size which I will use to calculate the speed of the vehicle.

//Horiztonal/Vertical small and large are the known lengths in the scene

Float_32 Calibration::GetRealWorldSize(Point32f p, Float_32 sizeInImProportion,
    CprECalibrationLineDirection objectDirection,
    Float_32* pOptDistanceToTheCamera) const
{
    switch (objectDirection) {
    case CPR_CALIBRATION_HORIZONTAL:
    {
        const double currentY = p.y;
        const double topY = m_HorizontalSmall.line.center().y();
        const double botY = m_HoriztontalLarge.line.center().y();
        const double smallHorizontalPixelLength= m_HorizontalSmall.line.dx()*m_ImageSize.width;
        const double largeHorizontalPixelLength = m_HoriztontalLarge.line.dx()*m_ImageSize.width;
        const double cmPerPixelTop = std::abs(m_HorizontalSmall.lengthInCm * std::cos(Base::VmsUnitConversions::DegreeToRadian(m_HorizontalSmall.line.angle())) /  smallHorizontalPixelLength);
        const double cmPerPixelBot = std::abs(m_HoriztontalLarge.lengthInCm * std::cos(Base::VmsUnitConversions::DegreeToRadian(m_HoriztontalLarge.line.angle())) /largeHorizontalPixelLength);
        const double valRange = cmPerPixelTop - cmPerPixelBot;
        const double keyRange = botY - topY;
        const double factorForEveryY = valRange / keyRange;
        const double cmPerPixelCurrent = cmPerPixelBot + (botY - currentY ) * factorForEveryY;
        const double realLength = std::abs(sizeInImProportion * m_ImageSize.width * cmPerPixelCurrent);

        return realLength;
    }
    case CPR_CALIBRATION_VERTICAL:
    {
        const double currentY = p.y;
        const double topY = m_VerticalSmall.line.center().y();
        const double botY = m_VerticalLarge.line.center().y();
        const double smallVerticalPixel = m_VerticalSmall.line.dy()*m_ImageSize.height;
        const double largeVerticalPixel = m_VerticalLarge.line.dy()*m_ImageSize.height;
        const double cmPerPixelTop = std::abs(m_VerticalSmall.lengthInCm * std::sin(Base::VmsUnitConversions::DegreeToRadian(m_VerticalSmall.line.angle())) / smallVerticalPixel);
        const double cmPerPixelBot = std::abs(m_VerticalLarge.lengthInCm * std::sin(Base::VmsUnitConversions::DegreeToRadian(m_VerticalLarge.line.angle())) / largeVerticalPixel) ;
        const double valRange = cmPerPixelTop - cmPerPixelBot;
        const double keyRange = botY - topY;
        const double factorForEveryY = valRange / keyRange;
        const double cmPerPixelCurrent = cmPerPixelTop - (currentY - topY) * factorForEveryY;
        const double realLength = std::abs(sizeInImProportion * m_ImageSize.height * cmPerPixelCurrent);
        return realLength;
    }
    case CPR_CALIBRATION_NO_DIRECTION: break;
    default: ;
    }

    return 0;
}

The GetRealWorldSize function uses the known lengths and calculates a linear factor of pixels per cm to multiply the distances by to figure out their real world size. However when tested while I can somewhat accurately get the same width of the road at different points. The same length of road seems to produce different results. Are 4 points enough to even get such an approximation? Am I doing something wrong or is there something I could improve in my calculations that would produce better results?

1 Answers

From your code, it looks like you are assuming the following is true over the whole image:

one_metre = scaling * number_of_pixels

You have not taken the camera angle into account. If you can make sure the camera is close to road level and is perpendicular to the road then I think you can make those assumptions in your code.

Related