3D Tracking Project Multiple Plots

README.md

3D Tracking Project Multiple Plots

Put the code snippet below at the end of your Final_Project.cpp. Here's what it gets you:

keypoints_plots.cpp

bVis = true;
          if (bVis) {
            cv::Mat visImg = (dataBuffer.end() - 1)->cameraImg.clone();
            showLidarImgOverlay(visImg, currBB->lidarPoints, P_rect_00, R_rect_00, RT, &visImg);
            cv::rectangle(visImg,
                          cv::Point(currBB->roi.x, currBB->roi.y),
                          cv::Point(currBB->roi.x + currBB->roi.width, currBB->roi.y + currBB->roi.height),
                          cv::Scalar(0, 255, 0),
                          2);

            char str[200];
            sprintf(str, "TTC Lidar : %f s, TTC Camera : %f s", ttcLidar, ttcCamera);
            putText(visImg, str, cv::Point2f(80, 50), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 0, 255));

            auto currImg = (dataBuffer.end() - 1)->cameraImg.clone();
            auto prevImg = (dataBuffer.end() - 2)->cameraImg.clone();
            auto kptsPrev = (dataBuffer.end() - 2)->keypoints;
            auto kptsCurr = (dataBuffer.end() - 1)->keypoints;
            auto kptMatches = currBB->kptMatches;

            cv::Mat kpts_img;

            cv::drawMatches(prevImg, kptsPrev, currImg, kptsCurr, kptMatches, kpts_img,
                            cv::Scalar::all(-1), cv::Scalar::all(-1), {},
                            cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);


            int match_id = 0;
            auto currFrame = *(dataBuffer.end() - 1);
            auto prevFrame = *(dataBuffer.end() - 2);
            auto curr_img = prevFrame.cameraImg.clone();

            for (auto &bmatch:bbBestMatches) {
              std::string label = std::to_string(match_id);
              // Draw rectangle displaying the bounding box
              int top, left, width, height;
              top = currFrame.boundingBoxes[bmatch.second].roi.y;
              left = currFrame.boundingBoxes[bmatch.second].roi.x;
              width = currFrame.boundingBoxes[bmatch.second].roi.width;
              height = currFrame.boundingBoxes[bmatch.second].roi.height;
              cv::rectangle(curr_img, cv::Point(left, top), cv::Point(left + width, top + height), cv::Scalar(0, 255, 0), 1);
              cv::putText(curr_img, label, cv::Point(left, top), cv::FONT_ITALIC, 0.75, cv::Scalar(0, 255, 0), 1);

              top = prevFrame.boundingBoxes[bmatch.first].roi.y;
              width = prevFrame.boundingBoxes[bmatch.first].roi.width;
              left = prevFrame.boundingBoxes[bmatch.first].roi.x;
              height = prevFrame.boundingBoxes[bmatch.first].roi.height;

              cv::rectangle(curr_img, cv::Point(left, top), cv::Point(left + width, top + height), cv::Scalar(0, 0, 255), 1);
              cv::putText(curr_img, label, cv::Point(left, top), cv::FONT_ITALIC, 0.75, cv::Scalar(0, 0, 255), 1);

              ++match_id;
            }

            cv::Mat canvas = cv::Mat::zeros(visImg.size()*2, visImg.type());

            kpts_img.copyTo(canvas(cv::Rect(0, 0, kpts_img.cols,  kpts_img.rows)));
            visImg.copyTo(canvas(cv::Rect(0, visImg.rows, visImg.cols, visImg.rows)));
            curr_img.copyTo(canvas(cv::Rect(visImg.cols, visImg.rows, visImg.cols, visImg.rows)));


            std::string windowName = "Final Results : TTC";
            cv::namedWindow(windowName, cv::WINDOW_AUTOSIZE);
            cv::imshow(windowName, canvas);

            cout << "Press key to continue to next frame" << endl;
            cv::waitKey(0);
          }

          bVis = false;