From 36d6728107c717d32b55a7ba1d0bdb2526502815 Mon Sep 17 00:00:00 2001 From: "Gokul B. Nair" Date: Fri, 13 Sep 2024 14:55:59 +1000 Subject: [PATCH 1/3] got rid of errors concerning ffmpeg versions. need to fix imu. --- cmake_modules/Findffmpeg.cmake | 4 ++-- include/VideoExtractor.h | 2 +- src/VideoExtractor.cpp | 44 ++++++++++++++++++---------------- 3 files changed, 26 insertions(+), 24 deletions(-) diff --git a/cmake_modules/Findffmpeg.cmake b/cmake_modules/Findffmpeg.cmake index 7453ca2..67ff317 100644 --- a/cmake_modules/Findffmpeg.cmake +++ b/cmake_modules/Findffmpeg.cmake @@ -3,5 +3,5 @@ set(libdir "/usr/lib/x86_64-linux-gnu") set(FFMPEG_LIBDIR "/usr/lib/x86_64-linux-gnu") set(FFMPEG_INCLUDE_DIRS "/usr/include/x86_64-linux-gnu") set(FFMPEG_FOUND 1) -set(FFMPEG_LIBRARIES "-L${FFMPEG_LIBDIR} -lavcodec -lavformat -lpostproc -lavdevice -lavresample -lswresample -lavfilter -lswscale -lavutil") -string(STRIP "${FFMPEG_LIBRARIES}" FFMPEG_LIBRARIES) \ No newline at end of file +set(FFMPEG_LIBRARIES "-L${FFMPEG_LIBDIR} -lavcodec -lavformat -lpostproc -lavdevice -lswresample -lavfilter -lswscale -lavutil") +string(STRIP "${FFMPEG_LIBRARIES}" FFMPEG_LIBRARIES) diff --git a/include/VideoExtractor.h b/include/VideoExtractor.h index 9c5969e..f5215a6 100644 --- a/include/VideoExtractor.h +++ b/include/VideoExtractor.h @@ -28,7 +28,7 @@ class GoProVideoExtractor { AVFormatContext* pFormatContext = NULL; uint32_t videoStreamIndex; AVCodecContext* pCodecContext = NULL; - AVCodec* pCodec = NULL; + // AVCodec* pCodec = NULL; AVFrame* pFrame = NULL; AVFrame* pFrameRGB = NULL; AVPacket packet; diff --git a/src/VideoExtractor.cpp b/src/VideoExtractor.cpp index e31c8b9..54507c3 100644 --- a/src/VideoExtractor.cpp +++ b/src/VideoExtractor.cpp @@ -39,7 +39,7 @@ GoProVideoExtractor::GoProVideoExtractor(const std::string filename, bool dump_info) { video_file = filename; - av_register_all(); + // av_register_all(); // Open video file std::cout << "Opening Video File: " << video_file << std::endl; @@ -88,7 +88,7 @@ GoProVideoExtractor::GoProVideoExtractor(const std::string filename, num_frames = video_stream->nb_frames; // Get a pointer to the codec context for the video stream - pCodec = avcodec_find_decoder(pFormatContext->streams[videoStreamIndex]->codecpar->codec_id); + const AVCodec* pCodec = avcodec_find_decoder(pFormatContext->streams[videoStreamIndex]->codecpar->codec_id); // Find the decoder for the video stream if (pCodec == nullptr) { @@ -147,7 +147,7 @@ void GoProVideoExtractor::save_to_png(AVFrame* frame, int height, AVRational time_base, std::string filename) { - AVCodec* outCodec = avcodec_find_encoder(AV_CODEC_ID_PNG); + const AVCodec* outCodec = avcodec_find_encoder(AV_CODEC_ID_PNG); AVCodecContext* outCodecCtx = avcodec_alloc_context3(outCodec); outCodecCtx->width = width; @@ -249,16 +249,17 @@ int GoProVideoExtractor::extractFrames(const std::string& image_folder, uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; int frameFinished; - while (av_read_frame(pFormatContext, &packet) >= 0) { + // while (av_receive_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); // Did we get a video frame? if (packet.dts != AV_NOPTS_VALUE) { - global_clock = av_frame_get_best_effort_timestamp(pFrame); + global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); global_video_pkt_pts = packet.pts; } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { global_clock = global_video_pkt_pts; @@ -333,18 +334,19 @@ int GoProVideoExtractor::getFrameStamps(std::vector& stamps) { uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; int frameFinished; uint32_t frame_count = 0; - while (av_read_frame(pFormatContext, &packet) >= 0) { + // while (av_read_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame // avcodec_send_packet(pCodecContext, &packet); // frameFinished = avcodec_receive_frame(pCodecContext, pFrameRGB); - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); // Did we get a video frame? if (packet.dts != AV_NOPTS_VALUE) { - global_clock = av_frame_get_best_effort_timestamp(pFrame); + global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); global_video_pkt_pts = packet.pts; } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { global_clock = global_video_pkt_pts; @@ -412,11 +414,11 @@ void GoProVideoExtractor::displayImages() { int frameFinished; // cv::namedWindow("GoPro Video", cv::WINDOW_GUI_EXPANDED); - while (av_read_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); if (frameFinished) { // Convert the image from its native format to RGB @@ -496,16 +498,16 @@ void GoProVideoExtractor::writeVideo(const std::string& bag_file, int frameFinished; uint64_t seq = 0; - while (av_read_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); // Did we get a video frame? if (packet.dts != AV_NOPTS_VALUE) { - global_clock = av_frame_get_best_effort_timestamp(pFrame); + global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); global_video_pkt_pts = packet.pts; } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { global_clock = global_video_pkt_pts; @@ -615,16 +617,16 @@ void GoProVideoExtractor::writeVideo(rosbag::Bag& bag, int frameFinished; uint32_t frame_count = 0; - while (av_read_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); // Did we get a video frame? if (packet.dts != AV_NOPTS_VALUE) { - global_clock = av_frame_get_best_effort_timestamp(pFrame); + global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); global_video_pkt_pts = packet.pts; } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { global_clock = global_video_pkt_pts; @@ -749,11 +751,11 @@ void GoProVideoExtractor::writeVideo(rosbag::Bag& bag, int frameFinished; uint32_t frame_count = 0; - while (av_read_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); if (frame_count == image_stamps.size()) { ROS_WARN_STREAM( @@ -871,11 +873,11 @@ int GoProVideoExtractor::extractFrames(const std::string& image_folder, int frameFinished; uint32_t frame_count = 0; - while (av_read_frame(pFormatContext, &packet) >= 0) { + while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { // Is this a packet from the video stream? if (packet.stream_index == videoStreamIndex) { // Decode video frame - avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); + // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); if (frame_count == image_stamps.size()) { ROS_WARN_STREAM( From a482ac8095bfdc76adc8e527c1c2d964cd15138a Mon Sep 17 00:00:00 2001 From: "Gokul B. Nair" <14059534+gokulbnr@users.noreply.github.com> Date: Fri, 13 Sep 2024 19:20:58 +1000 Subject: [PATCH 2/3] Update README.md additional dependencies installation --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 582c1fb..6a8f9f4 100644 --- a/README.md +++ b/README.md @@ -44,7 +44,7 @@ sudo apt install libopencv-dev - Install FFmpeg ```bash -sudo apt install ffmpeg +sudo apt install ffmpeg libpostproc-dev libavdevice-dev libavfilter-dev ``` - Install Eigen3 From 23041f336e2fce65a772cfbfcaadc12753b8640f Mon Sep 17 00:00:00 2001 From: "Gokul B. Nair" Date: Mon, 16 Sep 2024 19:39:49 +1000 Subject: [PATCH 3/3] gopro_to_rosbag conversion fixed and verified for raw images. --- gopro_to_asl.cpp | 6 +- launch/gopro_to_rosbag.launch | 2 +- src/VideoExtractor.cpp | 767 +++++++++++++++++----------------- 3 files changed, 397 insertions(+), 378 deletions(-) diff --git a/gopro_to_asl.cpp b/gopro_to_asl.cpp index 07cf3b2..bcec4fa 100644 --- a/gopro_to_asl.cpp +++ b/gopro_to_asl.cpp @@ -11,6 +11,8 @@ #include "VideoExtractor.h" #include "color_codes.h" +#include + using namespace std; namespace fs = std::experimental::filesystem; @@ -159,7 +161,7 @@ int main(int argc, char* argv[]) { ROS_INFO_STREAM("[ACCL] Payloads: " << accl_queue.size()); ROS_INFO_STREAM("[GYRO] Payloads: " << gyro_queue.size()); - ofstream imu_stream; + std::ofstream imu_stream; imu_stream.open(imu_file); imu_stream << std::fixed << std::setprecision(19); imu_stream << "#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1]," @@ -196,4 +198,4 @@ int main(int argc, char* argv[]) { imu_stream.close(); return 0; -} \ No newline at end of file +} diff --git a/launch/gopro_to_rosbag.launch b/launch/gopro_to_rosbag.launch index 1d0d48a..92990fc 100644 --- a/launch/gopro_to_rosbag.launch +++ b/launch/gopro_to_rosbag.launch @@ -6,7 +6,7 @@ - + diff --git a/src/VideoExtractor.cpp b/src/VideoExtractor.cpp index 54507c3..fbae031 100644 --- a/src/VideoExtractor.cpp +++ b/src/VideoExtractor.cpp @@ -249,67 +249,66 @@ int GoProVideoExtractor::extractFrames(const std::string& image_folder, uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; int frameFinished; - // while (av_receive_frame(pFormatContext, &packet) >= 0) { - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - // Did we get a video frame? - - if (packet.dts != AV_NOPTS_VALUE) { - global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); - global_video_pkt_pts = packet.pts; - } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { - global_clock = global_video_pkt_pts; - } else { - global_clock = 0; + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive all available frames + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + // Calculate global clock + if (packet.dts != AV_NOPTS_VALUE) { + global_clock = pFrame->best_effort_timestamp; + global_video_pkt_pts = packet.pts; + } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { + global_clock = global_video_pkt_pts; + } else { + global_clock = 0; + } + + double frame_delay = av_q2d(video_stream->time_base); + global_clock *= frame_delay; + + // Handle frame repetition delay + if (pFrame->repeat_pict > 0) { + double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); + global_clock += extra_delay; + } + + // Convert the image from its native format to RGB + sws_scale(sws_ctx, + (uint8_t const* const*)pFrame->data, + pFrame->linesize, + 0, + pCodecContext->height, + pFrameRGB->data, + pFrameRGB->linesize); + + // Save the frame to disk + uint64_t nanosecs = (uint64_t)(global_clock * 1e9); + if (nanosecs > last_image_stamp_ns) { + break; + } + uint64_t current_stamp = video_creation_time + nanosecs; + std::string string_stamp = uint64_to_string(current_stamp); + std::string stamped_image_filename = image_data_folder + "/" + string_stamp; + image_stream << string_stamp << "," << string_stamp + ".png" << std::endl; + + save_to_png(pFrameRGB, + pCodecContext, + image_width, + image_height, + video_stream->time_base, + stamped_image_filename); + } + } } - double frame_delay = av_q2d(video_stream->time_base); - global_clock *= frame_delay; - - // Only if we are repeating the - if (pFrame->repeat_pict > 0) { - double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); - global_clock += extra_delay; - } - - if (frameFinished) { - // Convert the image from its native format to RGB - sws_scale(sws_ctx, - (uint8_t const* const*)pFrame->data, - pFrame->linesize, - 0, - pCodecContext->height, - pFrameRGB->data, - pFrameRGB->linesize); - - // Save the frame to disk - // std::cout << "Global Clock: " << global_clock << std::endl; - uint64_t nanosecs = (uint64_t)(global_clock * 1e9); - // std::cout << "Nano secs: " << nanosecs << std::endl; - if (nanosecs > last_image_stamp_ns) { - break; - } - uint64_t current_stamp = video_creation_time + nanosecs; - std::string string_stamp = uint64_to_string(current_stamp); - std::string stamped_image_filename = image_data_folder + "/" + string_stamp; - image_stream << string_stamp << "," << string_stamp + ".png" << std::endl; - save_to_png(pFrameRGB, - pCodecContext, - image_width, - image_height, - video_stream->time_base, - stamped_image_filename); - } - } - - // Free the packet that was allocated by av_read_frame - av_packet_unref(&packet); + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } + image_stream.close(); // Free the RGB image @@ -334,43 +333,47 @@ int GoProVideoExtractor::getFrameStamps(std::vector& stamps) { uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; int frameFinished; uint32_t frame_count = 0; - // while (av_read_frame(pFormatContext, &packet) >= 0) { - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_send_packet(pCodecContext, &packet); - // frameFinished = avcodec_receive_frame(pCodecContext, pFrameRGB); - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - // Did we get a video frame? - - if (packet.dts != AV_NOPTS_VALUE) { - global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); - global_video_pkt_pts = packet.pts; - } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { - global_clock = global_video_pkt_pts; - } else { - global_clock = 0; + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive the decoded frame(s) + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + + // Calculate global clock + if (packet.dts != AV_NOPTS_VALUE) { + global_clock = pFrame->best_effort_timestamp; + global_video_pkt_pts = packet.pts; + } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { + global_clock = global_video_pkt_pts; + } else { + global_clock = 0; + } + + double frame_delay = av_q2d(video_stream->time_base); + global_clock *= frame_delay; + + // Handle repeated pictures + if (pFrame->repeat_pict > 0) { + double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); + global_clock += extra_delay; + } + + // Convert global clock to microseconds + uint64_t usecs = (uint64_t)(global_clock * 1000000); + stamps.push_back(usecs); + + // Increment frame count and update progress + frame_count++; + double percent = (double)frame_count / (double)num_frames; + progress.write(percent); + } + } } - double frame_delay = av_q2d(video_stream->time_base); - global_clock *= frame_delay; - - // Only if we are repeating the - if (pFrame->repeat_pict > 0) { - double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); - global_clock += extra_delay; - } - - uint64_t usecs = (uint64_t)(global_clock * 1000000); - // std::cout << "Stamp: " << usecs << std::endl; - stamps.push_back(usecs); - - frame_count++; - double percent = (double)frame_count / (double)num_frames; - progress.write(percent); - } + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } // Close the video file @@ -414,35 +417,38 @@ void GoProVideoExtractor::displayImages() { int frameFinished; // cv::namedWindow("GoPro Video", cv::WINDOW_GUI_EXPANDED); - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - if (frameFinished) { - // Convert the image from its native format to RGB - sws_scale(sws_ctx, - (uint8_t const* const*)pFrame->data, - pFrame->linesize, - 0, - pCodecContext->height, - pFrameRGB->data, - pFrameRGB->linesize); - - cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); - cv::cvtColor(img, img, cv::COLOR_RGB2BGR); - // std::cout << "width: " << img.size().width << " height: " << img.size().height << - // std::endl; - cv::imshow("GoPro Video", img); - cv::waitKey(1); + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive the decoded frame(s) + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + // Convert the image from its native format to RGB + sws_scale(sws_ctx, + (uint8_t const* const*)pFrame->data, + pFrame->linesize, + 0, + pCodecContext->height, + pFrameRGB->data, + pFrameRGB->linesize); + + // Create OpenCV matrix and convert color format from RGB to BGR + cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); + cv::cvtColor(img, img, cv::COLOR_RGB2BGR); + + // Display the video frame using OpenCV + cv::imshow("GoPro Video", img); + cv::waitKey(1); // Display the image for 1 ms + } + } } - } - // Free the packet that was allocated by av_read_frame - av_packet_unref(&packet); + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } + cv::destroyAllWindows(); // Free the RGB image @@ -498,73 +504,78 @@ void GoProVideoExtractor::writeVideo(const std::string& bag_file, int frameFinished; uint64_t seq = 0; - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - // Did we get a video frame? - - if (packet.dts != AV_NOPTS_VALUE) { - global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); - global_video_pkt_pts = packet.pts; - } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { - global_clock = global_video_pkt_pts; - } else { - global_clock = 0; + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive the decoded frame(s) + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + + // Calculate global clock + if (packet.dts != AV_NOPTS_VALUE) { + global_clock = pFrame->best_effort_timestamp; + global_video_pkt_pts = packet.pts; + } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { + global_clock = global_video_pkt_pts; + } else { + global_clock = 0; + } + + double frame_delay = av_q2d(video_stream->time_base); + global_clock *= frame_delay; + + // Handle repeated pictures + if (pFrame->repeat_pict > 0) { + double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); + global_clock += extra_delay; + } + + // Convert the image from its native format to RGB + sws_scale(sws_ctx, + (uint8_t const* const*)pFrame->data, + pFrame->linesize, + 0, + pCodecContext->height, + pFrameRGB->data, + pFrameRGB->linesize); + + // Save the frame to disk + uint64_t nanosecs = (uint64_t)(global_clock * 1e9); + if (nanosecs > last_image_stamp_ns) { + break; + } + uint64_t current_stamp = video_creation_time + nanosecs; + uint32_t secs = current_stamp * 1e-9; + uint32_t n_secs = current_stamp % 1000000000; + ros::Time ros_time(secs, n_secs); + + // Convert to OpenCV Mat and then to ROS image + cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); + cv::cvtColor(img, img, cv::COLOR_RGB2BGR); + + // Create ROS image message with timestamp and header + std_msgs::Header header; + header.stamp = ros_time; + header.frame_id = "gopro"; + header.seq = seq++; + + sensor_msgs::ImagePtr imgmsg = + cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, img).toImageMsg(); + bag.write(image_topic, ros_time, imgmsg); + + // Update progress + double percent = (double)seq / (double)num_frames; + progress.write(percent); + } + } } - double frame_delay = av_q2d(video_stream->time_base); - global_clock *= frame_delay; - - // Only if we are repeating the - if (pFrame->repeat_pict > 0) { - double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); - global_clock += extra_delay; - } - - if (frameFinished) { - // Convert the image from its native format to RGB - sws_scale(sws_ctx, - (uint8_t const* const*)pFrame->data, - pFrame->linesize, - 0, - pCodecContext->height, - pFrameRGB->data, - pFrameRGB->linesize); - - // Save the frame to disk - uint64_t nanosecs = (uint64_t)(global_clock * 1e9); - if (nanosecs > last_image_stamp_ns) { - break; - } - uint64_t current_stamp = video_creation_time + nanosecs; - uint32_t secs = current_stamp * 1e-9; - uint32_t n_secs = current_stamp % 1000000000; - ros::Time ros_time(secs, n_secs); - - cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); - cv::cvtColor(img, img, cv::COLOR_RGB2BGR); - - std_msgs::Header header; - header.stamp = ros_time; - header.frame_id = "gopro"; - header.seq = seq++; - - sensor_msgs::ImagePtr imgmsg = - cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, img).toImageMsg(); - bag.write(image_topic, ros_time, imgmsg); - - double percent = (double)seq / (double)num_frames; - progress.write(percent); - } - } - - // Free the packet that was allocated by av_read_frame - av_packet_unref(&packet); + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } + // Free the RGB image av_free(buffer); @@ -617,86 +628,87 @@ void GoProVideoExtractor::writeVideo(rosbag::Bag& bag, int frameFinished; uint32_t frame_count = 0; - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - // Did we get a video frame? - - if (packet.dts != AV_NOPTS_VALUE) { - global_clock = pFrame->best_effort_timestamp; // av_frame_get_best_effort_timestamp(pFrame); - global_video_pkt_pts = packet.pts; - } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { - global_clock = global_video_pkt_pts; - } else { - global_clock = 0; + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive the decoded frame(s) + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + // Calculate global clock + if (packet.dts != AV_NOPTS_VALUE) { + global_clock = pFrame->best_effort_timestamp; + global_video_pkt_pts = packet.pts; + } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { + global_clock = global_video_pkt_pts; + } else { + global_clock = 0; + } + + double frame_delay = av_q2d(video_stream->time_base); + global_clock *= frame_delay; + + // Handle repeated pictures + if (pFrame->repeat_pict > 0) { + double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); + global_clock += extra_delay; + } + + // Convert the image from its native format to RGB + sws_scale(sws_ctx, + (uint8_t const* const*)pFrame->data, + pFrame->linesize, + 0, + pCodecContext->height, + pFrameRGB->data, + pFrameRGB->linesize); + + // Save the frame to disk + uint64_t nanosecs = (uint64_t)(global_clock * 1e9); + if (nanosecs > last_image_stamp_ns) { + break; + } + uint64_t current_stamp = video_creation_time + nanosecs; + uint32_t secs = current_stamp * 1e-9; + uint32_t n_secs = current_stamp % 1000000000; + ros::Time ros_time(secs, n_secs); + + cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); + cv::cvtColor(img, img, cv::COLOR_RGB2BGR); + + std::string encoding = sensor_msgs::image_encodings::BGR8; + if (grayscale) { + cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); + encoding = sensor_msgs::image_encodings::MONO8; + } + + if (display_images) { + cv::imshow("GoPro Video", img); + cv::waitKey(1); + } + + std_msgs::Header header; + header.stamp = ros_time; + header.frame_id = "gopro"; + + if (compress_image) { + sensor_msgs::CompressedImagePtr img_msg = + cv_bridge::CvImage(header, encoding, img).toCompressedImageMsg(); + bag.write(image_topic + "/compressed", ros_time, img_msg); + } else { + sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); + bag.write(image_topic, ros_time, imgmsg); + } + + // Update progress + double percent = (double)frame_count++ / (double)num_frames; + progress.write(percent); + } + } } - double frame_delay = av_q2d(video_stream->time_base); - global_clock *= frame_delay; - - // Only if we are repeating the - if (pFrame->repeat_pict > 0) { - double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); - global_clock += extra_delay; - } - - if (frameFinished) { - // Convert the image from its native format to RGB - sws_scale(sws_ctx, - (uint8_t const* const*)pFrame->data, - pFrame->linesize, - 0, - pCodecContext->height, - pFrameRGB->data, - pFrameRGB->linesize); - - // Save the frame to disk - uint64_t nanosecs = (uint64_t)(global_clock * 1e9); - if (nanosecs > last_image_stamp_ns) { - break; - } - uint64_t current_stamp = video_creation_time + nanosecs; - uint32_t secs = current_stamp * 1e-9; - uint32_t n_secs = current_stamp % 1000000000; - ros::Time ros_time(secs, n_secs); - - cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); - cv::cvtColor(img, img, cv::COLOR_RGB2BGR); - - std::string encoding = sensor_msgs::image_encodings::BGR8; - if (grayscale) { - cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); - encoding = sensor_msgs::image_encodings::MONO8; - } - - if (display_images) { - cv::imshow("GoPro Video", img); - cv::waitKey(1); - } - - std_msgs::Header header; - header.stamp = ros_time; - header.frame_id = "gopro"; - - if (compress_image) { - sensor_msgs::CompressedImagePtr img_msg = - cv_bridge::CvImage(header, encoding, img).toCompressedImageMsg(); - bag.write(image_topic + "/compressed", ros_time, img_msg); - } else { - sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); - bag.write(image_topic, ros_time, imgmsg); - } - } - - double percent = (double)frame_count++ / (double)num_frames; - progress.write(percent); - } - - // Free the packet that was allocated by av_read_frame - av_packet_unref(&packet); + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } cv::destroyAllWindows(); @@ -751,73 +763,75 @@ void GoProVideoExtractor::writeVideo(rosbag::Bag& bag, int frameFinished; uint32_t frame_count = 0; - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - if (frame_count == image_stamps.size()) { - ROS_WARN_STREAM( - "Number of images does not match number of timestamps. This should only happen in " - "last/single GoPro Video !!"); - ROS_WARN_STREAM("Skipping " << num_frames - image_stamps.size() << "/" << num_frames - << " images"); - break; - } - // Did we get a video frame? - - if (frameFinished) { - // Convert the image from its native format to RGB - sws_scale(sws_ctx, - (uint8_t const* const*)pFrame->data, - pFrame->linesize, - 0, - pCodecContext->height, - pFrameRGB->data, - pFrameRGB->linesize); - - // Save the frame to disk - uint64_t current_stamp = image_stamps[frame_count]; - uint32_t secs = current_stamp * 1e-9; - uint32_t n_secs = current_stamp % 1000000000; - ros::Time ros_time(secs, n_secs); - - cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); - cv::cvtColor(img, img, cv::COLOR_RGB2BGR); - - std::string encoding = sensor_msgs::image_encodings::BGR8; - if (grayscale) { - cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); - encoding = sensor_msgs::image_encodings::MONO8; - } - - if (display_images) { - cv::imshow("GoPro Video", img); - cv::waitKey(1); - } - - std_msgs::Header header; - header.stamp = ros_time; - header.frame_id = "gopro"; - header.seq = frame_count++; - - if (compress_image) { - sensor_msgs::CompressedImagePtr img_msg = - cv_bridge::CvImage(header, encoding, img).toCompressedImageMsg(); - bag.write(image_topic + "/compressed", ros_time, img_msg); - } else { - sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); - bag.write(image_topic, ros_time, imgmsg); - } + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive the decoded frame(s) + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + + if (frame_count == image_stamps.size()) { + ROS_WARN_STREAM( + "Number of images does not match number of timestamps. This should only happen in " + "last/single GoPro Video !!"); + ROS_WARN_STREAM("Skipping " << num_frames - image_stamps.size() << "/" << num_frames + << " images"); + break; + } + + // Convert the image from its native format to RGB + sws_scale(sws_ctx, + (uint8_t const* const*)pFrame->data, + pFrame->linesize, + 0, + pCodecContext->height, + pFrameRGB->data, + pFrameRGB->linesize); + + // Save the frame to disk + uint64_t current_stamp = image_stamps[frame_count]; + uint32_t secs = current_stamp * 1e-9; + uint32_t n_secs = current_stamp % 1000000000; + ros::Time ros_time(secs, n_secs); + + cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); + cv::cvtColor(img, img, cv::COLOR_RGB2BGR); + + std::string encoding = sensor_msgs::image_encodings::BGR8; + if (grayscale) { + cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); + encoding = sensor_msgs::image_encodings::MONO8; + } + + if (display_images) { + cv::imshow("GoPro Video", img); + cv::waitKey(1); + } + + std_msgs::Header header; + header.stamp = ros_time; + header.frame_id = "gopro"; + header.seq = frame_count++; + + if (compress_image) { + sensor_msgs::CompressedImagePtr img_msg = + cv_bridge::CvImage(header, encoding, img).toCompressedImageMsg(); + bag.write(image_topic + "/compressed", ros_time, img_msg); + } else { + sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); + bag.write(image_topic, ros_time, imgmsg); + } + + // Update progress + double percent = (double)frame_count / (double)num_frames; + progress.write(percent); + } + } } - double percent = (double)frame_count / (double)num_frames; - progress.write(percent); - } - - // Free the packet that was allocated by av_read_frame - av_packet_unref(&packet); + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } cv::destroyAllWindows(); @@ -873,62 +887,65 @@ int GoProVideoExtractor::extractFrames(const std::string& image_folder, int frameFinished; uint32_t frame_count = 0; - while (avcodec_receive_frame(pCodecContext, pFrame) >= 0) { - // Is this a packet from the video stream? - if (packet.stream_index == videoStreamIndex) { - // Decode video frame - // avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); - - if (frame_count == image_stamps.size()) { - ROS_WARN_STREAM( - "Number of images does not match number of timestamps. This should only happen in " - "last/single GoPro Video !!"); - ROS_WARN_STREAM("Skipping " << num_frames - image_stamps.size() << "/" << num_frames - << " images"); - break; + while (av_read_frame(pFormatContext, &packet) >= 0) { + // Is this a packet from the video stream? + if (packet.stream_index == videoStreamIndex) { + // Send the packet to the decoder + if (avcodec_send_packet(pCodecContext, &packet) == 0) { + // Receive the decoded frame(s) + while (avcodec_receive_frame(pCodecContext, pFrame) == 0) { + + if (frame_count == image_stamps.size()) { + ROS_WARN_STREAM( + "Number of images does not match number of timestamps. This should only happen in " + "last/single GoPro Video !!"); + ROS_WARN_STREAM("Skipping " << num_frames - image_stamps.size() << "/" << num_frames + << " images"); + break; + } + + // Convert the image from its native format to RGB + sws_scale(sws_ctx, + (uint8_t const* const*)pFrame->data, + pFrame->linesize, + 0, + pCodecContext->height, + pFrameRGB->data, + pFrameRGB->linesize); + + uint64_t current_stamp = image_stamps[frame_count]; + std::string string_stamp = uint64_to_string(current_stamp); + std::string stamped_image_filename = image_data_folder + "/" + string_stamp; + image_stream << string_stamp << "," << string_stamp + ".png" << std::endl; + + cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); + cv::cvtColor(img, img, cv::COLOR_RGB2BGR); + + if (grayscale) { + cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); + } + + if (display_images) { + cv::imshow("GoPro Video", img); + cv::waitKey(1); + } + + // Save the frame as a PNG image + cv::imwrite(stamped_image_filename + ".png", img); + frame_count++; + + // Update progress + double percent = (double)frame_count / (double)num_frames; + progress.write(percent); + } + } } - // Did we get a video frame? - - if (frameFinished) { - // Convert the image from its native format to RGB - sws_scale(sws_ctx, - (uint8_t const* const*)pFrame->data, - pFrame->linesize, - 0, - pCodecContext->height, - pFrameRGB->data, - pFrameRGB->linesize); - - uint64_t current_stamp = image_stamps[frame_count]; - std::string string_stamp = uint64_to_string(current_stamp); - std::string stamped_image_filename = image_data_folder + "/" + string_stamp; - image_stream << string_stamp << "," << string_stamp + ".png" << std::endl; - - cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); - cv::cvtColor(img, img, cv::COLOR_RGB2BGR); - - if (grayscale) { - cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); - } - - if (display_images) { - cv::imshow("GoPro Video", img); - cv::waitKey(1); - } - - cv::imwrite(stamped_image_filename + ".png", img); - frame_count++; - } - - double percent = (double)frame_count / (double)num_frames; - progress.write(percent); - } - - // Free the packet that was allocated by av_read_frame - av_packet_unref(&packet); + // Free the packet that was allocated by av_read_frame + av_packet_unref(&packet); } + image_stream.close(); // Free the RGB image