Tracking of multiple objects in openCV using C++

edit

Hello :) I am doing a project in openCV on estimating the speed of moving vehicle using the video captured. Here the camera is stationery. I have estimated the speed of single object using centroid and Euclidean distance. Now the problem is, I am not getting how to do the same for multiple objects. Here, I need to calculate the Euclidean distance of objects between 2 subsequent frames. I am grateful if anyone would help. Thanks in advance :)

Here's the code below:

class centroids
{
public:
    vector<Point2f> ce;
    vector<float> area;

};
centroids c[100];


class First
{
public:
    First()
    {

        //create GUI windows
    namedWindow("Frame");
    namedWindow("FG Mask MOG 2");
        //create Background Subtractor objects
        pMOG2 = new BackgroundSubtractorMOG2(); //MOG2 approach
     //create the capture object
    VideoCapture capture("video5.avi");
    if(!capture.isOpened()){
        //error in opening the video input
        cerr << "Unable to open video file: "<< endl;
        exit(EXIT_FAILURE);
    }


    int j=0;
    int a=0;
    int k=0;
    //read input data. ESC or 'q' for quitting
    while( (char)keyboard != 'q' && (char)keyboard != 27 ){
        //read the current frame
        if(!capture.read(frame1)) {
            cerr << "Unable to read next frame." << endl;
            cerr << "Exiting..." << endl;
            exit(EXIT_FAILURE);
        }

        k++;
    cout<<"frame no."<<k<<"\n"<<endl;
        medianBlur ( frame1, frame, 15 );
        //GaussianBlur( frame1, frame, Size( 5, 5 ), 0, 0 );
        //update the background model
        pMOG2->operator()(frame, fgMaskMOG2,.7);
        //get the frame number and write it on the current frame
        stringstream ss;
        rectangle(frame, cv::Point(10, 2), cv::Point(100,20),
                  cv::Scalar(255,255,255), -1);
//        ss << capture.get(CAP_PROP_POS_FRAMES);
        string frameNumberString = ss.str();
        putText(frame, frameNumberString.c_str(), cv::Point(15, 15),
                FONT_HERSHEY_SIMPLEX, 0.5 , cv::Scalar(0,0,0));
        //show the current frame and the fg masks
        imshow("Frame", frame);
        imshow("FG Mask MOG 2", fgMaskMOG2);

        blur(fgMaskMOG2,fgMaskMOG2, Size(5,5)); 
        blur(fgMaskMOG2,fgMaskMOG2, Size(5,5)); 
        int largest_area=0;
        int stop=clock();
        //cout<<"time"<<(stop-start_s)/double(CLOCKS_PER_SEC)<<endl;

    int largest_contour_index=0;
    Rect bounding_rect;

      vector<vector<Point>> contours; // Vector for storing contour
    vector<Vec4i> hierarchy;

    findContours(fgMaskMOG2, contours, hierarchy,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
    int morph_size = 6;
    Mat element = getStructuringElement( MORPH_RECT, Size( 2*morph_size + 1, 2*morph_size+1 ), Point( morph_size, morph_size ) );
    //cout<<element;
    Mat dst; // result matrix
    // Apply the specified morphology operation
 //   for (int i=1;i<5;i++)
    {   
    morphologyEx( fgMaskMOG2, dst, MORPH_OPEN, element, Point(-1,-1), 3 );   
    //morphologyEx( src, dst, MORPH_TOPHAT, element ); // here iteration=1
    }


    Scalar color( 255,255,255);  // color of the contour in the
    //Draw the contour and rectangle
    for( int i = 0; i< contours.size(); i++ )
    {
    drawContours( fgMaskMOG2, contours,i, color, CV_FILLED,8,hierarchy);
    }

    //imshow("morpho window",dst);

    vector<Moments> mu(contours.size() );

    vector<Point2f> mc( contours.size() );
    std::vector<Point2f> m ;

    vector<double> time;
     vector<Point2f> centroid(mc.size());

    //vector<vector<Point> >::iterator itc= contours.begin();
    // iterate through each contour.
    double time1[1000];

   for( int i = 0; i< contours.size(); i++ )
    {

        //  Find the area of contour
        double a=contourArea( contours[i],false); 


        if(a>500){

           // largest_area=a;cout<<i<<" area  "<<a<<endl;
            // Store the index of largest contour
          //  largest_contour_index=i;

            mu[i] = moments( contours[i], false );
            mc[i ...