├── docs └── calibration.md ├── cmake_modules └── Findffmpeg.cmake ├── package.xml ├── scripts ├── orbtimestamp.py ├── asl_to_bag.py └── bag_to_asl.py ├── launch ├── gopro_to_asl.launch └── gopro_to_rosbag.launch ├── include ├── color_codes.h ├── GPMF_utils.h ├── ImuExtractor.h ├── VideoExtractor.h ├── Utils.h ├── GPMF_bitstream.h ├── GPMF_mp4reader.h ├── GPMF_parser.h └── GPMF_common.h ├── CMakeLists.txt ├── LICENSE ├── src ├── Utils.cpp ├── GPMF_utils.c ├── GPMF_print.cpp ├── ImuExtractor.cpp ├── VideoExtractor.cpp └── GPMF_mp4reader.c ├── .clang-format ├── README.md ├── gopro_to_asl.cpp └── gopro_to_rosbag.cpp /docs/calibration.md: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /cmake_modules/Findffmpeg.cmake: -------------------------------------------------------------------------------- 1 | # FFMPEG cmake project-config input for ./configure scripts 2 | set(libdir "/usr/lib/x86_64-linux-gnu") 3 | set(FFMPEG_LIBDIR "/usr/lib/x86_64-linux-gnu") 4 | set(FFMPEG_INCLUDE_DIRS "/usr/include/x86_64-linux-gnu") 5 | set(FFMPEG_FOUND 1) 6 | set(FFMPEG_LIBRARIES "-L${FFMPEG_LIBDIR} -lavcodec -lavformat -lpostproc -lavdevice -lavresample -lswresample -lavfilter -lswscale -lavutil") 7 | string(STRIP "${FFMPEG_LIBRARIES}" FFMPEG_LIBRARIES) -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | gopro_ros 3 | 0.0.1 4 | GoPro Video information extractor. 5 | Bharat Joshi 6 | BSD 7 | Bharat Joshi 8 | 9 | catkin 10 | 11 | cv_bridge 12 | geometry_msgs 13 | rosbag 14 | roscpp 15 | sensor_msgs 16 | std_msgs 17 | 18 | rospy 19 | 20 | -------------------------------------------------------------------------------- /scripts/orbtimestamp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | import os 3 | 4 | import rosbag 5 | import rospy 6 | from sensor_msgs.msg import Image, CompressedImage, Imu 7 | from cv_bridge import CvBridge 8 | from std_msgs.msg import Header 9 | import cv2 10 | import glob 11 | from tqdm import tqdm 12 | 13 | if __name__ == "__main__": 14 | base_dir = "/home/bjoshi/GoPro9/vio_test/mav0" 15 | cam0_folder = os.path.join(base_dir, "cam0", "data") 16 | indx = 0 17 | files = glob.glob(cam0_folder+"/*") 18 | files = sorted(files) 19 | 20 | orb_file = open('/home/bjoshi/GoPro9/orbslam_vio_test.txt', 'w') 21 | 22 | for file in tqdm(files): 23 | stamp = os.path.split(file)[1].split('.')[0] 24 | orb_file.write(stamp) 25 | orb_file.write('\n') 26 | indx += 1 27 | 28 | orb_file.close() 29 | 30 | 31 | # cv2.destroyAllWindows() 32 | -------------------------------------------------------------------------------- /launch/gopro_to_asl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /include/color_codes.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 10/17/20. 3 | // 4 | 5 | #pragma once 6 | 7 | // the following are UBUNTU/LINUX, and MacOS ONLY terminal color codes. 8 | 9 | #define RESET "\033[0m" 10 | #define BLACK "\033[30m" /* Black */ 11 | #define RED "\033[31m" /* Red */ 12 | #define GREEN "\033[32m" /* Green */ 13 | #define YELLOW "\033[33m" /* Yellow */ 14 | #define BLUE "\033[34m" /* Blue */ 15 | #define MAGENTA "\033[35m" /* Magenta */ 16 | #define CYAN "\033[36m" /* Cyan */ 17 | #define WHITE "\033[37m" /* White */ 18 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 19 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 20 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 21 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 22 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 23 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 24 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 25 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 26 | -------------------------------------------------------------------------------- /launch/gopro_to_rosbag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(gopro_ros) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 7 | 8 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 9 | 10 | find_package(catkin REQUIRED COMPONENTS cv_bridge geometry_msgs rosbag 11 | roscpp sensor_msgs std_msgs ) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | CATKIN_DEPENDS cv_bridge geometry_msgs rosbag roscpp sensor_msgs std_msgs 16 | LIBRARIES ${PROJECT_NAME} 17 | ) 18 | 19 | find_package(ffmpeg REQUIRED) 20 | find_package(OpenCV REQUIRED) 21 | find_package(Eigen3 3 REQUIRED) 22 | 23 | if(FFMPEG_FOUND) 24 | message("Found FFMPEG/LibAV libraries") 25 | else() 26 | message("Can't find libavcodec, libavformat or libavutil. Add them!") 27 | endif() 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ${FMPEG_INCLUDE_DIRS} 33 | ${OpenCV_INCLUDE_DIRS} 34 | ${EIGEN3_INCLUDE_DIR} 35 | ) 36 | 37 | 38 | file(GLOB c_src "src/*.c") 39 | file(GLOB cpp_src "src/*.cpp") 40 | 41 | add_library(${PROJECT_NAME} SHARED ${c_src} ${cpp_src}) 42 | target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES} ${FFMPEG_LIBRARIES} ${OpenCV_LIBS} -lstdc++fs) 43 | 44 | add_executable(gopro_to_asl gopro_to_asl.cpp) 45 | target_link_libraries(gopro_to_asl ${PROJECT_NAME} ${catkin_LIBRARIES}) 46 | 47 | add_executable(gopro_to_rosbag gopro_to_rosbag.cpp) 48 | target_link_libraries(gopro_to_rosbag ${PROJECT_NAME} ${catkin_LIBRARIES}) 49 | 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Autonomous Field Robotics Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /include/GPMF_utils.h: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file GPMF_utils.h 4 | * 5 | * @brief Utilities GPMF and MP4 handling 6 | * 7 | * @version 1.2.0 8 | * 9 | * (C) Copyright 2020 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | * 23 | */ 24 | 25 | #pragma once 26 | 27 | #ifdef __cplusplus 28 | extern "C" 29 | { 30 | #endif 31 | 32 | #include "GPMF_parser.h" 33 | 34 | #define GPMF_SAMPLE_RATE_FAST 0 35 | #define GPMF_SAMPLE_RATE_PRECISE 1 36 | 37 | typedef struct mp4callbacks 38 | { 39 | size_t mp4handle; 40 | uint32_t (*cbGetNumberPayloads)(size_t mp4handle); // number of indexed GPMF payloads 41 | uint32_t (*cbGetPayloadSize)(size_t mp4handle, uint32_t index); // get payload size for a particular index 42 | uint32_t *(*cbGetPayload)(size_t mp4handle, size_t res, uint32_t index); // get payload data for a particular index 43 | size_t (*cbGetPayloadResource)(size_t mp4handle, size_t reshandle, uint32_t initialMemorySize); // get payload memory handler 44 | void (*cbFreePayloadResource)(size_t mp4handle, size_t reshandle); // free payload memory handler 45 | uint32_t (*cbGetPayloadTime)(size_t mp4handle, uint32_t index, double *in, double *out); //MP4 timestamps for the payload 46 | uint32_t (*cbGetEditListOffsetRationalTime)(size_t mp4handle, // get any time offset for GPMF track 47 | int32_t *offset_numerator, uint32_t *denominator); 48 | } mp4callbacks; 49 | 50 | double GetGPMFSampleRate(mp4callbacks cbobject, uint32_t fourcc, uint32_t timeBaseFourCC, uint32_t flags, double *in, double *out); 51 | 52 | #ifdef __cplusplus 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /src/Utils.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 8/26/20. 3 | // 4 | #include "Utils.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "date.h" 11 | 12 | uint64_t parseISO(const std::string& iso_date) { 13 | using namespace date; 14 | using namespace std::chrono; 15 | 16 | date::sys_time tp; 17 | std::istringstream in(iso_date); 18 | in >> date::parse("%FT%TZ", tp); 19 | if (in.fail()) { 20 | in.clear(); 21 | in.exceptions(std::ios::failbit); 22 | in.str(iso_date); 23 | in >> date::parse("%FT%T%Ez", tp); 24 | } 25 | 26 | uint64_t time = tp.time_since_epoch().count(); 27 | 28 | return time; 29 | } 30 | 31 | std::string uint64_to_string(uint64_t value) { 32 | std::ostringstream os; 33 | os << value; 34 | return os.str(); 35 | } 36 | 37 | uint64_t get_offset_1904() { 38 | using namespace date; 39 | using namespace std::chrono; 40 | constexpr auto offset = sys_days{January / 1 / 1970} - sys_days{January / 1 / 1904}; 41 | uint64_t offset_secs = duration_cast(offset).count(); 42 | return offset_secs; 43 | } 44 | 45 | ProgressBar::ProgressBar(std::ostream& os, 46 | std::size_t line_width, 47 | std::string message_, 48 | const char symbol) 49 | : os{os}, 50 | bar_width{line_width - overhead}, 51 | message{std::move(message_)}, 52 | full_bar{std::string(bar_width, symbol) + std::string(bar_width, ' ')} { 53 | if (message.size() + 1 >= bar_width || message.find('\n') != message.npos) { 54 | os << message << '\n'; 55 | message.clear(); 56 | } else { 57 | message += ' '; 58 | } 59 | write(0.0); 60 | } 61 | 62 | ProgressBar::~ProgressBar() { 63 | write(1.0); 64 | os << '\n'; 65 | } 66 | 67 | void ProgressBar::write(double fraction) { 68 | // clamp fraction to valid range [0,1] 69 | if (fraction < 0) 70 | fraction = 0; 71 | else if (fraction > 1) 72 | fraction = 1; 73 | 74 | auto width = bar_width - message.size(); 75 | auto offset = bar_width - static_cast(width * fraction); 76 | 77 | os << '\r' << message; 78 | os.write(full_bar.data() + offset, width); 79 | os << " [" << std::setw(3) << static_cast(100 * fraction) << "%] " << std::flush; 80 | } -------------------------------------------------------------------------------- /include/ImuExtractor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 10/29/20. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "GPMF_mp4reader.h" 19 | #include "GPMF_parser.h" 20 | #include "Utils.h" 21 | #include "color_codes.h" 22 | 23 | class GoProImuExtractor { 24 | private: 25 | char* video; 26 | GPMF_stream metadata_stream; 27 | GPMF_stream* ms; 28 | double metadatalength; 29 | size_t mp4; 30 | uint32_t payloads; 31 | uint32_t* payload = NULL; 32 | size_t payloadres = 0; 33 | 34 | // Video Metadata 35 | uint32_t frame_count; 36 | float frame_rate; 37 | uint64_t movie_creation_time; 38 | 39 | public: 40 | uint32_t payloads_skipped = 0; 41 | 42 | public: 43 | GoProImuExtractor(const std::string file); 44 | ~GoProImuExtractor(); 45 | 46 | bool display_video_framerate(); 47 | void cleanup(); 48 | void show_gpmf_structure(); 49 | GPMF_ERR get_scaled_data(uint32_t fourcc, std::vector>& readings); 50 | int save_imu_stream(const std::string file, uint64_t end_time); 51 | uint64_t get_stamp(uint32_t fourcc); 52 | uint32_t getNumofSamples(uint32_t fourcc); 53 | GPMF_ERR show_current_payload(uint32_t index); 54 | 55 | void getPayloadStamps(uint32_t fourcc, 56 | std::vector& start_stamps, 57 | std::vector& samples); 58 | void skipPayloads(uint32_t last_n_payloads); 59 | void getImageStamps(std::vector& image_stamps, 60 | uint64_t image_end_stamp = 0, 61 | bool offset_only = false); 62 | 63 | void writeImuData(rosbag::Bag& bag, uint64_t end_time, const std::string& imu_topic); 64 | void readImuData(std::deque& accl_data, 65 | std::deque& gyro_data, 66 | uint64_t accl_end_time = 0, 67 | uint64_t gyro_end_time = 0); 68 | uint64_t getPayloadStartStamp(uint32_t fourcc, uint32_t index); 69 | void readImuData(std::vector& image_stamps, 70 | std::deque& gyro_data, 71 | uint64_t accl_end_time = 0, 72 | uint64_t gyro_end_time = 0); 73 | void readMagnetometerData(std::deque& mag_queue, uint64_t mag_end_time = 0); 74 | 75 | // GPMF_ERR getRawData(uint32_t fourcc, vector> &readings); 76 | 77 | inline uint32_t getImageCount() { return frame_count; } 78 | inline uint64_t getVideoCreationTime() { return movie_creation_time; } 79 | }; 80 | -------------------------------------------------------------------------------- /scripts/asl_to_bag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | import os 3 | 4 | import rosbag 5 | import rospy 6 | from sensor_msgs.msg import Image, CompressedImage, Imu 7 | from cv_bridge import CvBridge 8 | from std_msgs.msg import Header 9 | from geometry_msgs.msg import Vector3 10 | import cv2 11 | import glob 12 | from tqdm import tqdm 13 | 14 | if __name__ == "__main__": 15 | base_dir = "/home/bjoshi/GoPro9/vio_test/mav0" 16 | bag_name = "/home/bjoshi/GoPro9/gopro9_vio.bag" 17 | topic = "cam0/image_raw" 18 | 19 | use_stereo = False 20 | use_imu = True 21 | 22 | cam0_folder = os.path.join(base_dir, "cam0", "data") 23 | indx = 0 24 | files = glob.glob(cam0_folder+"/*") 25 | files = sorted(files) 26 | 27 | outbag = rosbag.Bag(bag_name, 'w') 28 | 29 | for file in tqdm(files): 30 | stamp = os.path.split(file)[1].split('.')[0] 31 | secs = int(float(stamp) * 1e-9) 32 | n_secs = int(float(stamp) - secs*1e9) 33 | ros_time = rospy.Time(secs, n_secs) 34 | 35 | header = Header() 36 | header.stamp = ros_time 37 | header.frame_id = '/gopro' 38 | header.seq = indx 39 | 40 | image = cv2.imread(file) 41 | image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) 42 | bridge = CvBridge() 43 | msg = bridge.cv2_to_imgmsg(image, encoding="passthrough") 44 | msg.header = header 45 | msg.encoding = 'mono8' 46 | outbag.write('/cam0/image_raw', msg, header.stamp) 47 | indx += 1 48 | 49 | if use_imu: 50 | imu_file = open(os.path.join(base_dir, "imu0", "data.csv")) 51 | indx = 0 52 | # skip the first line asl format 53 | imu_file.readline() 54 | lines = imu_file.readlines() 55 | for line in lines: 56 | line = line.strip() 57 | line_arr = line.split(',') 58 | stamp = line_arr[0] 59 | secs = int(float(stamp) * 1e-9) 60 | n_secs = int(float(stamp) - secs*1e9) 61 | ros_time = rospy.Time(secs, n_secs) 62 | 63 | header = Header() 64 | header.stamp = ros_time 65 | header.frame_id = '/gopro' 66 | header.seq = indx 67 | 68 | angular_vel = Vector3() 69 | angular_vel.x = float(line_arr[1]) 70 | angular_vel.y = float(line_arr[2]) 71 | angular_vel.z = float(line_arr[3]) 72 | 73 | linear_acc = Vector3() 74 | linear_acc.x = float(line_arr[4]) 75 | linear_acc.y = float(line_arr[5]) 76 | linear_acc.z = float(line_arr[6]) 77 | 78 | imu = Imu() 79 | imu.header = header 80 | imu.angular_velocity = angular_vel 81 | imu.linear_acceleration = linear_acc 82 | 83 | outbag.write('/imu0', imu, header.stamp) 84 | 85 | outbag.close() 86 | 87 | 88 | # cv2.destroyAllWindows() 89 | -------------------------------------------------------------------------------- /include/VideoExtractor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 10/29/20. 3 | // 4 | 5 | #pragma once 6 | 7 | extern "C" { 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | } 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | class GoProVideoExtractor { 24 | private: 25 | std::string video_file; 26 | 27 | // Video properties 28 | AVFormatContext* pFormatContext = NULL; 29 | uint32_t videoStreamIndex; 30 | AVCodecContext* pCodecContext = NULL; 31 | AVCodec* pCodec = NULL; 32 | AVFrame* pFrame = NULL; 33 | AVFrame* pFrameRGB = NULL; 34 | AVPacket packet; 35 | 36 | AVDictionary* optionsDict = NULL; 37 | AVDictionaryEntry* tag_dict = NULL; 38 | struct SwsContext* sws_ctx = NULL; 39 | AVStream* video_stream = NULL; 40 | AVCodecParameters* codecParameters; 41 | 42 | uint64_t video_creation_time; 43 | uint32_t image_width; 44 | uint32_t image_height; 45 | uint32_t num_frames; 46 | 47 | public: 48 | GoProVideoExtractor(const std::string file, double scaling_factor = 1.0, bool dump_info = false); 49 | ~GoProVideoExtractor(); 50 | 51 | void save_to_png(AVFrame* frame, 52 | AVCodecContext* codecContext, 53 | int width, 54 | int height, 55 | AVRational time_base, 56 | std::string filename); 57 | 58 | void save_raw(AVFrame* pFrame, int width, int height, std::string filename); 59 | 60 | int extractFrames(const std::string& base_folder, uint64_t last_image_stamp_ns); 61 | int extractFrames(const std::string& image_folder, 62 | const std::vector image_stamps, 63 | bool grayscale = false, 64 | bool display_images = false); 65 | int getFrameStamps(std::vector& stamps); 66 | 67 | void displayImages(); 68 | void writeVideo(const std::string& bag_file, 69 | uint64_t last_image_stamp_ns, 70 | const std::string& image_topic); 71 | void writeVideo(rosbag::Bag& bag, 72 | uint64_t last_image_stamp_ns, 73 | const std::string& image_topic, 74 | bool grayscale = false, 75 | bool compress_image = false, 76 | bool display_images = false); 77 | 78 | void writeVideo(rosbag::Bag& bag, 79 | const std::string& image_topic, 80 | const std::vector image_stamps, 81 | bool grayscale, 82 | bool compress_image, 83 | bool display_images); 84 | 85 | inline uint32_t getFrameCount() { return num_frames; } 86 | inline uint64_t getVideoCreationTime() { return video_creation_time; } 87 | }; 88 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignOperands: true 8 | AlignTrailingComments: true 9 | AllowAllParametersOfDeclarationOnNextLine: false 10 | AllowShortBlocksOnASingleLine: false 11 | AllowShortCaseLabelsOnASingleLine: false 12 | AllowShortFunctionsOnASingleLine: All 13 | AllowShortIfStatementsOnASingleLine: true 14 | AllowShortLoopsOnASingleLine: true 15 | AlwaysBreakAfterDefinitionReturnType: None 16 | AlwaysBreakAfterReturnType: None 17 | AlwaysBreakBeforeMultilineStrings: true 18 | AlwaysBreakTemplateDeclarations: true 19 | BinPackArguments: false 20 | BinPackParameters: false 21 | BraceWrapping: 22 | AfterClass: false 23 | AfterControlStatement: false 24 | AfterEnum: false 25 | AfterFunction: false 26 | AfterNamespace: false 27 | AfterObjCDeclaration: false 28 | AfterStruct: false 29 | AfterUnion: false 30 | BeforeCatch: false 31 | BeforeElse: false 32 | IndentBraces: false 33 | BreakBeforeBinaryOperators: None 34 | BreakBeforeBraces: Attach 35 | BreakBeforeTernaryOperators: true 36 | BreakConstructorInitializersBeforeComma: false 37 | ColumnLimit: 100 38 | CommentPragmas: '^ IWYU pragma:' 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | ConstructorInitializerIndentWidth: 4 41 | ContinuationIndentWidth: 4 42 | Cpp11BracedListStyle: true 43 | DerivePointerAlignment: false 44 | DisableFormat: false 45 | ExperimentalAutoDetectBinPacking: false 46 | ForEachMacros: 47 | - foreach 48 | - Q_FOREACH 49 | - BOOST_FOREACH 50 | IncludeCategories: 51 | - Regex: '^' 52 | Priority: 2 53 | - Regex: '^<.*\.h>' 54 | Priority: 1 55 | - Regex: '^<.*' 56 | Priority: 2 57 | - Regex: '.*' 58 | Priority: 3 59 | IndentCaseLabels: true 60 | IndentWidth: 2 61 | IndentWrappedFunctionNames: false 62 | KeepEmptyLinesAtTheStartOfBlocks: false 63 | MacroBlockBegin: '' 64 | MacroBlockEnd: '' 65 | MaxEmptyLinesToKeep: 1 66 | NamespaceIndentation: None 67 | ObjCBlockIndentWidth: 2 68 | ObjCSpaceAfterProperty: false 69 | ObjCSpaceBeforeProtocolList: false 70 | PenaltyBreakBeforeFirstCallParameter: 1 71 | PenaltyBreakComment: 300 72 | PenaltyBreakFirstLessLess: 120 73 | PenaltyBreakString: 1000 74 | PenaltyExcessCharacter: 1000000 75 | PenaltyReturnTypeOnItsOwnLine: 200 76 | PointerAlignment: Left 77 | ReflowComments: true 78 | SortIncludes: true 79 | SpaceAfterCStyleCast: false 80 | SpaceBeforeAssignmentOperators: true 81 | SpaceBeforeParens: ControlStatements 82 | SpaceInEmptyParentheses: false 83 | SpacesBeforeTrailingComments: 2 84 | SpacesInAngles: false 85 | SpacesInContainerLiterals: true 86 | SpacesInCStyleCastParentheses: false 87 | SpacesInParentheses: false 88 | SpacesInSquareBrackets: false 89 | Standard: Auto 90 | TabWidth: 8 91 | UseTab: Never 92 | -------------------------------------------------------------------------------- /include/Utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 8/26/20. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | uint64_t parseISO(const std::string& iso_date); 15 | std::string uint64_to_string(uint64_t value); 16 | uint64_t get_offset_1904(); 17 | 18 | class ProgressBar { 19 | private: 20 | static const auto overhead = sizeof " [100%]"; 21 | std::ostream& os; 22 | const std::size_t bar_width; 23 | std::string message; 24 | const std::string full_bar; 25 | 26 | public: 27 | ProgressBar(std::ostream& os, 28 | std::size_t line_width, 29 | std::string message_, 30 | const char symbol = '.'); 31 | 32 | // not copyable 33 | ProgressBar(const ProgressBar&) = delete; 34 | ProgressBar& operator=(const ProgressBar&) = delete; 35 | 36 | ~ProgressBar(); 37 | 38 | void write(double fraction); 39 | }; 40 | 41 | // Inertial containers. 42 | using Timestamp = uint64_t; 43 | using ImuStamp = Timestamp; 44 | // First 3 elements correspond to acceleration data [m/s^2] 45 | // while the 3 last correspond to angular velocities [rad/s]. 46 | using ImuAccGyr = Eigen::Matrix; 47 | using ImuAccl = Eigen::Matrix; 48 | using ImuGyro = Eigen::Matrix; 49 | using MagneticField = Eigen::Matrix; 50 | 51 | struct MagMeasurement { 52 | MagMeasurement() = default; 53 | MagMeasurement(const Timestamp& timestamp, const MagneticField& magnetic_field) 54 | : timestamp_(timestamp), magfield_(magnetic_field) {} 55 | MagMeasurement(Timestamp&& timestamp, MagneticField&& magnetic_field) 56 | : timestamp_(std::move(timestamp)), magfield_(std::move(magnetic_field)) {} 57 | 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 59 | Timestamp timestamp_; 60 | MagneticField magfield_; 61 | }; 62 | 63 | struct ImuMeasurement { 64 | ImuMeasurement() = default; 65 | ImuMeasurement(const ImuStamp& timestamp, const ImuAccGyr& imu_data) 66 | : timestamp_(timestamp), acc_gyr_(imu_data) {} 67 | ImuMeasurement(ImuStamp&& timestamp, ImuAccGyr&& imu_data) 68 | : timestamp_(std::move(timestamp)), acc_gyr_(std::move(imu_data)) {} 69 | 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 71 | ImuStamp timestamp_; 72 | ImuAccGyr acc_gyr_; 73 | }; 74 | 75 | struct AcclMeasurement { 76 | AcclMeasurement() = default; 77 | AcclMeasurement(const ImuStamp& timestamp, const ImuAccl& accl_data) 78 | : timestamp_(timestamp), data_(accl_data) {} 79 | AcclMeasurement(ImuStamp&& timestamp, ImuAccl& accl_data) 80 | : timestamp_(std::move(timestamp)), data_(std::move(accl_data)) {} 81 | 82 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 83 | ImuStamp timestamp_; 84 | ImuAccl data_; 85 | }; 86 | 87 | struct GyroMeasurement { 88 | GyroMeasurement() = default; 89 | GyroMeasurement(const ImuStamp& timestamp, const ImuAccl& accl_data) 90 | : timestamp_(timestamp), data_(accl_data) {} 91 | GyroMeasurement(ImuStamp&& timestamp, ImuAccl& accl_data) 92 | : timestamp_(std::move(timestamp)), data_(std::move(accl_data)) {} 93 | 94 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 95 | ImuStamp timestamp_; 96 | ImuGyro data_; 97 | }; -------------------------------------------------------------------------------- /scripts/bag_to_asl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | import rospy 4 | from sensor_msgs.msg import Imu 5 | from sensor_msgs.msg import CompressedImage, Image 6 | import argparse 7 | import os 8 | import numpy as np 9 | import cv2 10 | from cv_bridge import CvBridge 11 | 12 | 13 | class dump_stamps: 14 | def __init__(self, root_folder): 15 | 16 | self.bridge = CvBridge() 17 | rospy.Subscriber('/gopro/image/compressed', 18 | CompressedImage, self.compressed_image_sub) 19 | rospy.Subscriber('/gopro/imu', Imu, self.imu_sub) 20 | rospy.Subscriber('/gopro/image', Image, self.image_sub) 21 | 22 | if not os.path.exists(root_folder): 23 | os.mkdir(root_folder) 24 | img_folder = os.path.join(root_folder, 'cam0') 25 | if not os.path.exists(img_folder): 26 | os.mkdir(img_folder) 27 | self.img_data_folder = os.path.join(img_folder, 'data') 28 | if not os.path.exists(self.img_data_folder): 29 | os.mkdir(self.img_data_folder) 30 | file = os.path.join(img_folder, 'data.csv') 31 | self.img_file = open(file, 'w') 32 | self.img_file.write('#timestamp [ns],filename\n') 33 | 34 | imu_folder = os.path.join(root_folder, 'imu0') 35 | if not os.path.exists(imu_folder): 36 | os.mkdir(imu_folder) 37 | file = os.path.join(imu_folder, 'data.csv') 38 | self.imu_file = open(file, 'w') 39 | self.imu_file.write('#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],' 40 | 'w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\n') 41 | 42 | def __del__(self): 43 | self.imu_file.close() 44 | self.img_file.close() 45 | 46 | def imu_sub(self, imu_msg): 47 | acc = imu_msg.linear_acceleration 48 | ang_vel = imu_msg.angular_velocity 49 | self.imu_file.write('{},{},{},{},{},{},{}\n'.format(str(imu_msg.header.stamp), ang_vel.x, ang_vel.y, ang_vel.z, 50 | acc.x, acc.y, acc.z)) 51 | 52 | def compressed_image_sub(self, img_msg): 53 | np_arr = np.fromstring(img_msg.data, np.uint8) 54 | image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) 55 | cv2.imwrite(os.path.join(self.img_data_folder, 56 | '{}.png'.format(str(img_msg.header.stamp))), image_np) 57 | self.img_file.write('{},{}\n'.format( 58 | str(img_msg.header.stamp), '{}.png'.format(str(img_msg.header.stamp)))) 59 | 60 | def image_sub(self, img_msg): 61 | cv_image = self.bridge.imgmsg_to_cv2( 62 | img_msg, desired_encoding='passthrough') 63 | cv2.imwrite(os.path.join(self.img_data_folder, 64 | '{}.png'.format(str(img_msg.header.stamp))), cv_image) 65 | self.img_file.write('{},{}\n'.format( 66 | str(img_msg.header.stamp), '{}.png'.format(str(img_msg.header.stamp)))) 67 | 68 | 69 | if __name__ == '__main__': 70 | parser = argparse.ArgumentParser(description="Save memory and CPU usage") 71 | parser.add_argument('--base_dir', help='file to record pose') 72 | args = parser.parse_args() 73 | root_folder = args.base_dir 74 | 75 | rospy.init_node("asl_format") 76 | 77 | dump_stamps(root_folder) 78 | 79 | while not rospy.is_shutdown(): 80 | rospy.spin() 81 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # gopro_ros 2 | 3 | This repository contains code for parsing GoPro telemetry metadata to obtain GoPro images with synchronized IMU measurements. The GoPro visual-inertial data can then be saved in [rosbag](http://wiki.ros.org/rosbag) or [Euroc](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) format. Thus, effectively paving the way for visual-inertial odometry/SLAM for GoPro cameras. 4 | 5 | This repository use [gpmf-parser](https://github.com/gopro/gpmf-parser) from [GoPro](https://gopro.com) to extract metadata and timing information from GoPro cameras. 6 | 7 | ## Related Paper 8 | If you find the code useful in your research, please cite our paper 9 | ``` 10 | @inproceedings{joshi_gopro_icra_2022, 11 | author = {Bharat Joshi and Marios Xanthidis and Sharmin Rahman and Ioannis Rekleitis}, 12 | title = {High Definition, Inexpensive, Underwater Mapping}, 13 | booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, 14 | year = {2022}, 15 | pages = {1113-1121}, 16 | doi = {10.1109/ICRA46639.2022.9811695}, 17 | abbr = {ICRA}, 18 | bibtex_show = {true}, 19 | code = {https://github.com/AutonomousFieldRoboticsLab/gopro_ros}, 20 | } 21 | ``` 22 | 23 | # Installation 24 | 25 | Tested on Ubuntu 18.04 (ros-melodic) & 20.04 (ros-noetic). The install instructions are for Ubuntu 18.04. 26 | 27 | ## Prerequisites 28 | 29 | - ros-melodic-desktop-full 30 | - [OpenCV](https://github.com/opencv/opencv) >= 3.2 31 | - [FFmpeg](http://ffmpeg.org/) 32 | - [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) 33 | 34 | ## Install Dependencies 35 | 36 | - Install ROS using [this guide](http://wiki.ros.org/ROS/Installation) 37 | 38 | - System installation of OpenCV should work 39 | 40 | ```bash 41 | sudo apt install libopencv-dev 42 | ``` 43 | 44 | - Install FFmpeg 45 | 46 | ```bash 47 | sudo apt install ffmpeg 48 | ``` 49 | 50 | - Install Eigen3 51 | 52 | ```bash 53 | sudo apt install libeigen3-dev 54 | ``` 55 | 56 | ## Install gopro_ros 57 | 58 | Before proceeding, ensure all dependencies are installed. To install gopro_ros 59 | 60 | ```bash 61 | mkdir -p ~/gopro_ws/src 62 | cd gopro_ws/src 63 | git clone https://github.com/joshi-bharat/gopro_ros.git 64 | cd ~/gopro_ws/ 65 | catkin_make 66 | source ~/gopro_ws/devel/setup.bash 67 | # add this to ~/.bashrc to make this permanent 68 | ``` 69 | 70 | # Usage 71 | 72 | GoPro splits video into smaller chunks. By splitting up the video it reduces the chance of you losing all your footage if the file gets corrupted somehow. It’s called chaptering, and the idea is that if one chapter gets corrupted the others should still be okay because they’re separate files. 73 | 74 | ## Save to rosbag 75 | 76 | To save GoPro video with IMU measurements to rosbag: 77 | 78 | ```bash 79 | roslaunch gopro_ros gopro_to_rosbag.launch gopro_video:= rosbag:= 80 | ``` 81 | 82 | If you have multiple files from a single session, put all videos in same folder you can use the following command to concatenate into a single rosbag: 83 | 84 | ```bash 85 | roslaunch gopro_ros gopro_to_rosbag.launch gopro_folder:= multiple_files:=true rosbag:= 86 | ``` 87 | 88 | ## Save in Euroc format 89 | 90 | To save GoPro video with IMU measurements in Euroc format: 91 | 92 | ```bash 93 | roslaunch gopro_ros gopro_to_rosbag.launch gopro_video:= asl_dir:= 94 | ``` 95 | 96 | If you have multiple files from a single session, put all videos in same folder you can use the following command extract all videos in a single folder: 97 | 98 | ```bash 99 | roslaunch gopro_ros gopro_to_rosbag.launch gopro_folder:= multiple_files:=true asl_dir:= 100 | ``` 101 | # TODO: 102 | Extraction video takes a lot of time. Implement multi-threaded. 103 | -------------------------------------------------------------------------------- /include/GPMF_bitstream.h: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file GPMF_bitstream.h 4 | * 5 | * @brief GPMF Parser library include 6 | * 7 | * Some GPMF streams may contain compressed data, this is useful for high frequency 8 | * sensor data that is highly correlated like IMU data. The compression is Huffman 9 | * coding of the delta between samples, with addition codewords for runs of zeros, 10 | * and optional quantization. The compression scheme is similar to the Huffman coding 11 | * in JPEG. As it intended for lossless compression (with quantize set to 1) it can 12 | * only comrpess/decompress integer based streams. 13 | * 14 | * @version 1.2.0 15 | * 16 | * (C) Copyright 2017 GoPro Inc (http://gopro.com/). 17 | * 18 | * Licensed under either: 19 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 20 | * - MIT license, http://opensource.org/licenses/MIT 21 | * at your option. 22 | * 23 | * Unless required by applicable law or agreed to in writing, software 24 | * distributed under the License is distributed on an "AS IS" BASIS, 25 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 26 | * See the License for the specific language governing permissions and 27 | * limitations under the License. 28 | * 29 | */ 30 | 31 | #ifndef _GPMF_BITSTREAM_H 32 | #define _GPMF_BITSTREAM_H 33 | 34 | #ifdef __cplusplus 35 | extern "C" { 36 | #endif 37 | 38 | #include 39 | 40 | typedef struct rlv { // Codebook entries for arbitrary runs 41 | uint16_t size; // Size of code word in bits 42 | uint16_t bits; // Code word bits right justified 43 | uint16_t count; // Run length for zeros 44 | int16_t value; // Value for difference 45 | } RLV; 46 | 47 | typedef const struct { 48 | int length; // Number of entries in the code book 49 | RLV entries[39]; 50 | } RLVTABLE; 51 | 52 | #define BITSTREAM_WORD_TYPE uint16_t // use 16-bit buffer for compression 53 | #define BITSTREAM_WORD_SIZE 16 // use 16-bit buffer for compression 54 | #define BITSTREAM_ERROR_OVERFLOW 1 55 | 56 | #define BITMASK(n) _bitmask[n] 57 | #define _BITMASK(n) ((((BITSTREAM_WORD_TYPE )1 << (n))) - 1) 58 | 59 | static const BITSTREAM_WORD_TYPE _bitmask[] = 60 | { 61 | _BITMASK(0), _BITMASK(1), _BITMASK(2), _BITMASK(3), 62 | _BITMASK(4), _BITMASK(5), _BITMASK(6), _BITMASK(7), 63 | _BITMASK(8), _BITMASK(9), _BITMASK(10), _BITMASK(11), 64 | _BITMASK(12), _BITMASK(13), _BITMASK(14), _BITMASK(15), 65 | 0xFFFF 66 | }; 67 | 68 | 69 | 70 | typedef struct bitstream 71 | { 72 | int32_t error; // Error parsing the bitstream 73 | int32_t bitsFree; // Number of bits available in the current word 74 | uint8_t *lpCurrentWord; // Pointer to next word in block 75 | int32_t wordsUsed; // Number of words used in the block 76 | int32_t dwBlockLength; // Number of entries in the block 77 | uint16_t wBuffer; // Current word bit buffer 78 | uint16_t bits_per_src_word; // Bitused in the source word. e.g. 's' = 16-bits 79 | } BITSTREAM; 80 | 81 | 82 | static RLVTABLE enchuftable = { 83 | 39, 84 | { 85 | { 1, 0b0, 1, 0 }, // m0 86 | { 2, 0b10, 1, 1 }, // m1 87 | { 4, 0b1100, 1, 2 }, // m2 88 | { 5, 0b11011, 1, 3 }, // m3 89 | { 5, 0b11101, 1, 4 }, // m4 90 | { 6, 0b110100, 1, 5 }, // m5 91 | { 6, 0b110101, 1, 6 }, // m6 92 | { 6, 0b111110, 1, 7 }, // m7 93 | { 7, 0b1110000, 1, 8 }, // m8 94 | { 7, 0b1110011, 1, 9 }, // m9 95 | { 7, 0b1111000, 1, 10 }, // m10 96 | { 7, 0b1111001, 1, 11 }, // m11 97 | { 7, 0b1111011, 1, 12 }, // m12 98 | { 8, 0b11100100, 1, 13 }, // m13 99 | { 8, 0b11100101, 1, 14 }, // m14 100 | { 8, 0b11110100, 1, 15 }, // m15 101 | { 9, 0b111000101, 1, 16 }, // m16 102 | { 9, 0b111000110, 1, 17 }, // m17 103 | { 9, 0b111101010, 1, 18 }, // m18 104 | { 10, 0b1110001000, 1, 19 }, // m19 105 | { 10, 0b1110001110, 1, 20 }, // m20 106 | { 10, 0b1111010110, 1, 21 }, // m21 107 | { 10, 0b1111111100, 1, 22 }, // m22 108 | { 11, 0b11100010010, 1, 23 }, // m23 109 | { 11, 0b11100011111, 1, 24 }, // m24 110 | { 11, 0b11110101110, 1, 25 }, // m25 111 | { 12, 0b111000100111, 1, 26 }, // m26 112 | { 12, 0b111000111101, 1, 27 }, // m27 113 | { 12, 0b111101011111, 1, 28 }, // m28 114 | { 13, 0b1110001001101, 1, 29 }, // m29 115 | { 13, 0b1110001111001, 1, 30 }, // m30 116 | { 13, 0b1111010111101, 1, 31 }, // m31 117 | { 14, 0b11100010011000, 1, 32 }, // m32 118 | { 14, 0b11100011110000, 1, 33 }, // m33 119 | { 14, 0b11110101111000, 1, 34 }, // m34 120 | { 14, 0b11110101111001, 1, 35 }, // m35 121 | { 15, 0b111000100110010, 1, 36 }, // m36 122 | { 15, 0b111000100110011, 1, 37 }, // m37 123 | { 15, 0b111000111100011, 1, 38 }, // m38 124 | } 125 | }; 126 | 127 | 128 | 129 | static RLVTABLE enczerorunstable = { 130 | 4, 131 | { 132 | { 7, 0b1111110, 16, 0 }, // z16 133 | { 8, 0b11111110, 32, 0 }, // z32 134 | { 9, 0b111111111, 64, 0 }, // z64 135 | { 10,0b1111111101, 128, 0 }, // z128 136 | } 137 | }; 138 | 139 | #define HUFF_ESC_CODE_ENTRY 0 140 | #define HUFF_END_CODE_ENTRY 1 141 | static RLVTABLE enccontrolcodestable = { 142 | 2, 143 | { 144 | { 16, 0b1110001111000100, 0, 0 }, // escape code for direct data Continue 145 | { 16, 0b1110001111000101, 0, 0 }, // end code. Ends each compressed stream 146 | } 147 | }; 148 | 149 | 150 | 151 | #ifdef __cplusplus 152 | } 153 | #endif 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /include/GPMF_mp4reader.h: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file mp4reader.h 4 | * 5 | * @brief Way Too Crude MP4|MOV reader 6 | * 7 | * @version 1.8.0 8 | * 9 | * (C) Copyright 2017-2020 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | * 23 | */ 24 | 25 | #ifndef _GPMF_MP4READER_H 26 | #define _GPMF_MP4READER_H 27 | 28 | #include "GPMF_parser.h" 29 | 30 | #ifdef __cplusplus 31 | extern "C" 32 | { 33 | #endif 34 | 35 | typedef struct media_header 36 | { 37 | uint8_t version_flags[4]; 38 | uint32_t creation_time; 39 | uint32_t modification_time; 40 | uint32_t time_scale; 41 | uint32_t duration; 42 | uint16_t language; 43 | uint16_t quality; 44 | } media_header; 45 | 46 | typedef struct SampleToChunk 47 | { 48 | uint32_t chunk_num; 49 | uint32_t samples; 50 | uint32_t id; 51 | } SampleToChunk; 52 | 53 | #define MAX_TRACKS 16 54 | typedef struct mp4object 55 | { 56 | uint32_t *metasizes; 57 | uint32_t metasize_count; 58 | uint64_t *metaoffsets; 59 | uint32_t metastco_count; 60 | SampleToChunk *metastsc; 61 | uint32_t metastsc_count; 62 | uint32_t indexcount; 63 | double videolength; 64 | double metadatalength; 65 | int32_t metadataoffset_clockcount; 66 | uint32_t clockdemon, clockcount; 67 | uint32_t trak_clockdemon, trak_clockcount; 68 | uint32_t meta_clockdemon, meta_clockcount; 69 | uint32_t video_framerate_numerator; 70 | uint32_t video_framerate_denominator; 71 | uint32_t video_frames; 72 | double basemetadataduration; 73 | int32_t trak_edit_list_offsets[MAX_TRACKS]; 74 | uint32_t trak_num; 75 | FILE *mediafp; 76 | uint64_t filesize; 77 | uint64_t filepos; 78 | uint32_t movie_creation_time; 79 | uint32_t timeBaseFourCC; 80 | 81 | } mp4object; 82 | 83 | enum mp4flag 84 | { 85 | MP4_FLAG_READ_WRITE_MODE = 1 << 0, 86 | }; 87 | 88 | typedef struct resObject 89 | { 90 | uint32_t *buffer; 91 | uint32_t bufferSize; 92 | } resObject; 93 | 94 | typedef enum MP4READER_ERROR 95 | { 96 | MP4_ERROR_OK = 0, 97 | MP4_ERROR_MEMORY 98 | } MP4READER_ERROR; 99 | 100 | #define MOV_GPMF_TRAK_TYPE MAKEID('m', 'e', 't', 'a') // track is the type for metadata 101 | #define MOV_GPMF_TRAK_SUBTYPE MAKEID('g', 'p', 'm', 'd') // subtype is GPMF 102 | #define MOV_VIDE_TRAK_TYPE MAKEID('v', 'i', 'd', 'e') // MP4 track for video 103 | #define MOV_SOUN_TRAK_TYPE MAKEID('s', 'o', 'u', 'n') // MP4 track for audio 104 | #define MOV_AVC1_SUBTYPE MAKEID('a', 'v', 'c', '1') // subtype H264 105 | #define MOV_HVC1_SUBTYPE MAKEID('h', 'v', 'c', '1') // subtype H265 106 | #define MOV_MP4A_SUBTYPE MAKEID('m', 'p', '4', 'a') // subtype for audio 107 | #define MOV_CFHD_SUBTYPE MAKEID('C', 'F', 'H', 'D') // subtype is CineForm HD 108 | #define AVI_VIDS_TRAK_TYPE MAKEID('v', 'i', 'd', 's') // track is the type for video 109 | #define AVI_CFHD_SUBTYPE MAKEID('c', 'f', 'h', 'd') // subtype is CineForm HD 110 | 111 | #define NESTSIZE(x) \ 112 | { \ 113 | int _i = nest; \ 114 | while (_i > 0 && nestsize[_i] > 0) \ 115 | { \ 116 | (nestsize[_i] >= x) ? (nestsize[_i] -= x) : (nestsize[_i] = 0); \ 117 | if (nestsize[_i] <= 8) \ 118 | { \ 119 | nestsize[_i] = 0; \ 120 | nest--; \ 121 | } \ 122 | _i--; \ 123 | } \ 124 | } 125 | 126 | #define VALID_FOURCC(a) (((((a >> 24) & 0xff) >= 'a' && ((a >> 24) & 0xff) <= 'z') || (((a >> 24) & 0xff) >= 'A' && ((a >> 24) & 0xff) <= 'Z') || (((a >> 24) & 0xff) >= '0' && ((a >> 24) & 0xff) <= '9') || (((a >> 24) & 0xff) == ' ')) && \ 127 | ((((a >> 16) & 0xff) >= 'a' && ((a >> 16) & 0xff) <= 'z') || (((a >> 16) & 0xff) >= 'A' && ((a >> 16) & 0xff) <= 'Z') || (((a >> 16) & 0xff) >= '0' && ((a >> 16) & 0xff) <= '9') || (((a >> 16) & 0xff) == ' ')) && \ 128 | ((((a >> 8) & 0xff) >= 'a' && ((a >> 8) & 0xff) <= 'z') || (((a >> 8) & 0xff) >= 'A' && ((a >> 8) & 0xff) <= 'Z') || (((a >> 8) & 0xff) >= '0' && ((a >> 8) & 0xff) <= '9') || (((a >> 8) & 0xff) == ' ')) && \ 129 | ((((a >> 0) & 0xff) >= 'a' && ((a >> 0) & 0xff) <= 'z') || (((a >> 0) & 0xff) >= 'A' && ((a >> 0) & 0xff) <= 'Z') || (((a >> 0) & 0xff) >= '0' && ((a >> 0) & 0xff) <= '9') || (((a >> 0) & 0xff) == ' '))) 130 | 131 | size_t OpenMP4Source(char *filename, uint32_t traktype, uint32_t subtype, int32_t flags); 132 | size_t OpenMP4SourceUDTA(char *filename, int32_t flags); 133 | void CloseSource(size_t mp4Handle); 134 | float GetDuration(size_t mp4Handle); 135 | uint32_t GetVideoFrameRateAndCount(size_t mp4Handle, uint32_t *numer, uint32_t *demon); 136 | uint32_t GetNumberPayloads(size_t mp4Handle); 137 | uint32_t WritePayload(size_t mp4Handle, uint32_t *payload, uint32_t payloadsize, uint32_t index); 138 | size_t GetPayloadResource(size_t mp4Handle, size_t resHandle, uint32_t payloadBufferSize); 139 | void FreePayloadResource(size_t mp4Handle, size_t resHandle); 140 | uint32_t *GetPayload(size_t mp4Handle, size_t resHandle, uint32_t index); 141 | uint32_t GetPayloadSize(size_t mp4Handle, uint32_t index); 142 | uint32_t GetPayloadTime(size_t mp4Handle, uint32_t index, double *in, double *out); //MP4 timestamps for the payload 143 | uint32_t GetPayloadRationalTime(size_t mp4Handle, uint32_t index, int32_t *in_numerator, int32_t *out_numerator, uint32_t *denominator); 144 | uint32_t GetEditListOffset(size_t mp4Handle, double *offset); 145 | uint32_t GetEditListOffsetRationalTime(size_t mp4Handle, int32_t *offset_numerator, uint32_t *denominator); 146 | uint32_t getCreationtime(size_t handle); 147 | 148 | #ifdef __cplusplus 149 | } 150 | #endif 151 | 152 | #endif 153 | -------------------------------------------------------------------------------- /include/GPMF_parser.h: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file GPMF_parser.h 4 | * 5 | * @brief GPMF Parser library include 6 | * 7 | * @version 2.0.0 8 | * 9 | * (C) Copyright 2017-2020 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under either: 12 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 13 | * - MIT license, http://opensource.org/licenses/MIT 14 | * at your option. 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | * 22 | */ 23 | 24 | #pragma once 25 | 26 | #include 27 | #include 28 | #include "GPMF_common.h" 29 | 30 | #ifdef __cplusplus 31 | extern "C" 32 | { 33 | #endif 34 | 35 | #define GPMF_NEST_LIMIT 16 36 | 37 | typedef struct GPMF_stream 38 | { 39 | uint32_t *buffer; 40 | uint32_t buffer_size_longs; 41 | uint32_t pos; 42 | uint32_t last_level_pos[GPMF_NEST_LIMIT]; 43 | uint32_t nest_size[GPMF_NEST_LIMIT]; 44 | uint32_t last_seek[GPMF_NEST_LIMIT]; 45 | uint32_t nest_level; 46 | uint32_t device_count; 47 | uint32_t device_id; 48 | char device_name[32]; 49 | size_t cbhandle; // compression handler 50 | } GPMF_stream; 51 | 52 | typedef enum GPMF_LEVELS 53 | { 54 | GPMF_CURRENT_LEVEL = 0, // search or validate within the current GPMF next level 55 | GPMF_RECURSE_LEVELS = 1, // search or validate recursing all levels 56 | GPMF_TOLERANT = 2 // Ignore minor errors like unknown datatypes if the structure is otherwise valid. 57 | } GPMF_LEVELS; 58 | 59 | // Prepare GPMF data 60 | GPMF_ERR GPMF_Init(GPMF_stream *gs, uint32_t *buffer, uint32_t datasize); //Initialize a GPMF_stream for parsing a particular buffer. 61 | GPMF_ERR GPMF_ResetState(GPMF_stream *gs); //Read from beginning of the buffer again 62 | GPMF_ERR GPMF_CopyState(GPMF_stream *src, GPMF_stream *dst); //Copy state, 63 | GPMF_ERR GPMF_Validate(GPMF_stream *gs, GPMF_LEVELS recurse); //Is the nest structure valid GPMF? 64 | 65 | // Navigate through GPMF data 66 | GPMF_ERR GPMF_Next(GPMF_stream *gs, GPMF_LEVELS recurse); //Step to the next GPMF KLV entrance, optionally recurse up or down nesting levels. 67 | GPMF_ERR GPMF_FindPrev(GPMF_stream *gs, uint32_t fourCC, GPMF_LEVELS recurse); //find a previous FourCC -- at the current level only if recurse is false 68 | GPMF_ERR GPMF_FindNext(GPMF_stream *gs, uint32_t fourCC, GPMF_LEVELS recurse); //find a particular FourCC upcoming -- at the current level only if recurse is false 69 | GPMF_ERR GPMF_SeekToSamples(GPMF_stream *gs); //find the last FourCC in the current level, this is raw data for any STRM 70 | 71 | // Get information about the current GPMF KLV 72 | uint32_t GPMF_Key(GPMF_stream *gs); //return the current Key (FourCC) 73 | GPMF_SampleType GPMF_Type(GPMF_stream *gs); //return the current Type (GPMF_Type) 74 | uint32_t GPMF_StructSize(GPMF_stream *gs); //return the current sample structure size 75 | uint32_t GPMF_Repeat(GPMF_stream *gs); //return the current repeat or the number of samples of this structure 76 | uint32_t GPMF_PayloadSampleCount(GPMF_stream *gs); //return the current number of samples of this structure, supporting multisample entries. 77 | uint32_t GPMF_ElementsInStruct(GPMF_stream *gs); //return the current number elements within the structure (e.g. 3-axis gyro) 78 | uint32_t GPMF_RawDataSize(GPMF_stream *gs); //return the data size for the current GPMF KLV 79 | void *GPMF_RawData(GPMF_stream *gs); //return a pointer the KLV data (which is Bigendian if the type is known.) 80 | 81 | GPMF_ERR GPMF_Modify(GPMF_stream *gs, //find and inplace overwrite a GPMF KLV with new KLV, if the lengths match. 82 | uint32_t origfourCC, uint32_t newfourCC, GPMF_SampleType newType, uint32_t newStructSize, uint32_t newRepeat, void *newData); 83 | 84 | // Get information about where the GPMF KLV is nested 85 | uint32_t GPMF_NestLevel(GPMF_stream *gs); //return the current nest level 86 | uint32_t GPMF_DeviceID(GPMF_stream *gs); //return the current device ID (DVID), to seperate match sensor data from difference devices. 87 | GPMF_ERR GPMF_DeviceName(GPMF_stream *gs, char *devicename_buf, uint32_t devicename_buf_size); //return the current device name (DVNM), to seperate match sensor data from difference devices. 88 | 89 | // Utilities for data types 90 | uint32_t GPMF_SizeofType(GPMF_SampleType type); // GPMF equivalent to sizeof(type) 91 | uint32_t GPMF_ExpandComplexTYPE(char *src, uint32_t srcsize, char *dst, uint32_t *dstsize); // GPMF using TYPE for cmple structure. { float val[16],uin32_t flags; } has type "f[8]L", this tools expands to the simpler format "ffffffffL" 92 | uint32_t GPMF_SizeOfComplexTYPE(char *typearray, uint32_t typestringlength); // GPMF equivalent to sizeof(typedef) for complex types. 93 | GPMF_ERR GPMF_Reserved(uint32_t key); // Test for a reverse GPMF Key, returns GPMF_OK is not reversed. 94 | 95 | //Tools for extracting sensor data 96 | uint32_t GPMF_FormattedDataSize(GPMF_stream *gs); //return the decompressed data size for the current GPMF KLV 97 | uint32_t GPMF_ScaledDataSize(GPMF_stream *gs, GPMF_SampleType type); //return the decompressed data size for the current GPMF KLV 98 | GPMF_ERR GPMF_FormattedData(GPMF_stream *gs, void *buffer, uint32_t buffersize, uint32_t sample_offset, uint32_t read_samples); // extract 'n' samples into local endian memory format. 99 | GPMF_ERR GPMF_ScaledData(GPMF_stream *gs, void *buffer, uint32_t buffersize, uint32_t sample_offset, uint32_t read_samples, GPMF_SampleType type); // extract 'n' samples into local endian memory format // return a point the KLV data. 100 | 101 | //Tools for Compressed datatypes 102 | 103 | typedef struct GPMF_codebook 104 | { 105 | int16_t value; //value to store 106 | uint8_t offset; //0 to 128+ bytes to skip before store (leading zeros) 107 | uint8_t bits_used; //1 to 16,32 (if escape code > 16 then read from bit-steam), 108 | int8_t bytes_stored; //bytes stored in value: 0, 1 or 2 109 | int8_t command; //0 - OKAY, -1 valid code, 1 - end 110 | } GPMF_codebook; 111 | 112 | GPMF_ERR GPMF_AllocCodebook(size_t *cbhandle); 113 | GPMF_ERR GPMF_FreeCodebook(size_t cbhandle); 114 | GPMF_ERR GPMF_DecompressedSize(GPMF_stream *gs, uint32_t *neededsize); 115 | GPMF_ERR GPMF_Decompress(GPMF_stream *gs, uint32_t *localbuf, uint32_t localbuf_size); 116 | GPMF_ERR GPMF_Free(GPMF_stream *gs); 117 | 118 | #ifdef __cplusplus 119 | } 120 | #endif 121 | -------------------------------------------------------------------------------- /gopro_to_asl.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 10/29/20. 3 | // 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "ImuExtractor.h" 11 | #include "VideoExtractor.h" 12 | #include "color_codes.h" 13 | 14 | using namespace std; 15 | namespace fs = std::experimental::filesystem; 16 | 17 | int main(int argc, char* argv[]) { 18 | ros::init(argc, argv, "gopro_to_asl"); 19 | ros::NodeHandle nh_private("~"); 20 | 21 | string gopro_video; 22 | string asl_dir; 23 | string gopro_folder; 24 | 25 | bool is_gopro_video = nh_private.getParam("gopro_video", gopro_video); 26 | bool is_gopro_folder = nh_private.getParam("gopro_folder", gopro_folder); 27 | 28 | if (!is_gopro_video && !is_gopro_folder) { 29 | ROS_FATAL("Please specify the gopro video or folder"); 30 | ros::shutdown(); 31 | } 32 | 33 | ROS_FATAL_STREAM_COND(!nh_private.getParam("asl_dir", asl_dir), "No asl directory specified"); 34 | 35 | double scaling = 1.0; 36 | if (nh_private.hasParam("scale")) nh_private.getParam("scale", scaling); 37 | 38 | bool grayscale = false; 39 | if (nh_private.hasParam("grayscale")) nh_private.getParam("grayscale", grayscale); 40 | 41 | bool display_images = false; 42 | if (nh_private.hasParam("display_images")) nh_private.getParam("display_images", display_images); 43 | 44 | bool multiple_files = false; 45 | if (nh_private.hasParam("multiple_files")) nh_private.getParam("multiple_files", multiple_files); 46 | 47 | vector video_files; 48 | 49 | if (is_gopro_folder && multiple_files) { 50 | std::copy(fs::directory_iterator(gopro_folder), 51 | fs::directory_iterator(), 52 | std::back_inserter(video_files)); 53 | std::sort(video_files.begin(), video_files.end()); 54 | 55 | } else { 56 | video_files.push_back(fs::path(gopro_video)); 57 | } 58 | 59 | auto end = std::remove_if(video_files.begin(), video_files.end(), [](const fs::path& p) { 60 | return p.extension() != ".MP4" || fs::is_directory(p); 61 | }); 62 | video_files.erase(end, video_files.end()); 63 | 64 | string image_folder = asl_dir + "/mav0/cam0"; 65 | // print image folder 66 | cout << "Image folder: " << image_folder << endl; 67 | 68 | if (!experimental::filesystem::is_directory(image_folder)) { 69 | experimental::filesystem::create_directories(image_folder); 70 | } 71 | 72 | std::string image_data_folder = image_folder + "/data"; 73 | if (!std::experimental::filesystem::is_directory(image_data_folder)) { 74 | std::experimental::filesystem::create_directories(image_data_folder); 75 | } 76 | 77 | std::string image_file = image_folder + "/data.csv"; 78 | std::ofstream image_stream; 79 | image_stream.open(image_file); 80 | image_stream << std::fixed << std::setprecision(19); 81 | image_stream << "#timestamp [ns],filename" << std::endl; 82 | image_stream.close(); 83 | 84 | string imu_folder = asl_dir + "/mav0/imu0"; 85 | if (!experimental::filesystem::is_directory(imu_folder)) { 86 | experimental::filesystem::create_directories(imu_folder); 87 | } 88 | 89 | string imu_file = imu_folder + "/data.csv"; 90 | 91 | vector start_stamps; 92 | vector samples; 93 | vector image_stamps; 94 | 95 | std::deque accl_queue; 96 | std::deque gyro_queue; 97 | 98 | for (uint32_t i = 0; i < video_files.size(); i++) { 99 | image_stamps.clear(); 100 | 101 | ROS_WARN_STREAM("Opening Video File: " << video_files[i].filename().string()); 102 | 103 | fs::path file = video_files[i]; 104 | GoProImuExtractor imu_extractor(file.string()); 105 | GoProVideoExtractor video_extractor(file.string(), scaling); 106 | 107 | imu_extractor.getPayloadStamps(STR2FOURCC("ACCL"), start_stamps, samples); 108 | ROS_INFO_STREAM("[ACCL] Payloads: " << start_stamps.size() 109 | << " Start stamp: " << start_stamps[0] 110 | << " End stamp: " << start_stamps[samples.size() - 1] 111 | << " Total Samples: " << samples.at(samples.size() - 1)); 112 | imu_extractor.getPayloadStamps(STR2FOURCC("GYRO"), start_stamps, samples); 113 | ROS_INFO_STREAM("[GYRO] Payloads: " << start_stamps.size() 114 | << " Start stamp: " << start_stamps[0] 115 | << " End stamp: " << start_stamps[samples.size() - 1] 116 | << " Total Samples: " << samples.at(samples.size() - 1)); 117 | imu_extractor.getPayloadStamps(STR2FOURCC("CORI"), start_stamps, samples); 118 | ROS_INFO_STREAM("[CORI] Payloads: " << start_stamps.size() 119 | << " Start stamp: " << start_stamps[0] 120 | << " End stamp: " << start_stamps[samples.size() - 1] 121 | << " Total Samples: " << samples.at(samples.size() - 1)); 122 | 123 | uint64_t accl_end_stamp = 0, gyro_end_stamp = 0; 124 | uint64_t video_end_stamp = 0; 125 | if (i < video_files.size() - 1) { 126 | GoProImuExtractor imu_extractor_next(video_files[i + 1].string()); 127 | accl_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("ACCL"), 0); 128 | gyro_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("GYRO"), 0); 129 | video_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("CORI"), 0); 130 | } 131 | 132 | imu_extractor.readImuData(accl_queue, gyro_queue, accl_end_stamp, gyro_end_stamp); 133 | 134 | uint32_t gpmf_frame_count = imu_extractor.getImageCount(); 135 | uint32_t ffmpeg_frame_count = video_extractor.getFrameCount(); 136 | if (gpmf_frame_count != ffmpeg_frame_count) { 137 | ROS_FATAL_STREAM("Video and metadata frame count do not match"); 138 | ros::shutdown(); 139 | } 140 | 141 | uint64_t gpmf_video_time = imu_extractor.getVideoCreationTime(); 142 | uint64_t ffmpeg_video_time = video_extractor.getVideoCreationTime(); 143 | 144 | if (ffmpeg_video_time != gpmf_video_time) { 145 | ROS_FATAL_STREAM("Video creation time does not match"); 146 | ros::shutdown(); 147 | } 148 | 149 | imu_extractor.getImageStamps(image_stamps, video_end_stamp); 150 | if (i != video_files.size() - 1 && image_stamps.size() != ffmpeg_frame_count) { 151 | ROS_FATAL_STREAM("ffmpeg and gpmf frame count does not match."); 152 | ROS_FATAL_STREAM(image_stamps.size() << " vs " << ffmpeg_frame_count); 153 | ros::shutdown(); 154 | } 155 | 156 | video_extractor.extractFrames(image_folder, image_stamps, grayscale, display_images); 157 | } 158 | 159 | ROS_INFO_STREAM("[ACCL] Payloads: " << accl_queue.size()); 160 | ROS_INFO_STREAM("[GYRO] Payloads: " << gyro_queue.size()); 161 | 162 | ofstream imu_stream; 163 | imu_stream.open(imu_file); 164 | imu_stream << std::fixed << std::setprecision(19); 165 | 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]," 166 | "a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]" 167 | << endl; 168 | 169 | while (!accl_queue.empty() && !gyro_queue.empty()) { 170 | AcclMeasurement accl = accl_queue.front(); 171 | GyroMeasurement gyro = gyro_queue.front(); 172 | // ROS_INFO_STREAM("***************Here**********"); 173 | Timestamp stamp; 174 | int64_t diff = accl.timestamp_ - gyro.timestamp_; 175 | if (abs(diff) > 100000) { 176 | // I will need to handle this case more carefully 177 | ROS_WARN_STREAM(diff << " ns difference between gyro and accl"); 178 | stamp = (Timestamp)(((double)accl.timestamp_ + (double)gyro.timestamp_) / 2.0); 179 | } else { 180 | stamp = accl.timestamp_; 181 | } 182 | imu_stream << uint64_to_string(stamp); 183 | 184 | imu_stream << "," << gyro.data_.x(); 185 | imu_stream << "," << gyro.data_.y(); 186 | imu_stream << "," << gyro.data_.z(); 187 | 188 | imu_stream << "," << accl.data_.x(); 189 | imu_stream << "," << accl.data_.y(); 190 | imu_stream << "," << accl.data_.z() << endl; 191 | 192 | accl_queue.pop_front(); 193 | gyro_queue.pop_front(); 194 | } 195 | 196 | imu_stream.close(); 197 | 198 | return 0; 199 | } -------------------------------------------------------------------------------- /include/GPMF_common.h: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file GPMF_common.h 4 | * 5 | * @brief GPMF Parser library include 6 | * 7 | * @version 2.1.0 8 | * 9 | * (C) Copyright 2017-2020 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under either: 12 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 13 | * - MIT license, http://opensource.org/licenses/MIT 14 | * at your option. 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | * 22 | */ 23 | 24 | #ifndef _GPMF_COMMON_H 25 | #define _GPMF_COMMON_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | typedef enum GPMF_ERROR 32 | { 33 | GPMF_OK = 0, 34 | GPMF_ERROR_MEMORY, // NULL Pointer or insufficient memory 35 | GPMF_ERROR_BAD_STRUCTURE, // Non-complaint GPMF structure detected 36 | GPMF_ERROR_BUFFER_END, // reached the end of the provided buffer 37 | GPMF_ERROR_FIND, // Find failed to return the requested data, but structure is valid. 38 | GPMF_ERROR_LAST, // reached the end of a search at the current nest level 39 | GPMF_ERROR_TYPE_NOT_SUPPORTED, // a needed TYPE tuple is missing or has unsupported elements. 40 | GPMF_ERROR_SCALE_NOT_SUPPORTED, // scaling for an non-scaling type, e.g. scaling a FourCC field to a float. 41 | GPMF_ERROR_SCALE_COUNT, // A SCAL tuple has a mismatching element count. 42 | GPMF_ERROR_UNKNOWN_TYPE, // Potentially valid data with a new or unknown type. 43 | GPMF_ERROR_RESERVED // internal usage 44 | } GPMF_ERROR; 45 | 46 | #define GPMF_ERR uint32_t 47 | 48 | typedef enum 49 | { 50 | GPMF_TYPE_STRING_ASCII = 'c', //single byte 'c' style character string 51 | GPMF_TYPE_SIGNED_BYTE = 'b',//single byte signed number 52 | GPMF_TYPE_UNSIGNED_BYTE = 'B', //single byte unsigned number 53 | GPMF_TYPE_SIGNED_SHORT = 's',//16-bit integer 54 | GPMF_TYPE_UNSIGNED_SHORT = 'S',//16-bit integer 55 | GPMF_TYPE_FLOAT = 'f', //32-bit single precision float (IEEE 754) 56 | GPMF_TYPE_FOURCC = 'F', //32-bit four character tag 57 | GPMF_TYPE_SIGNED_LONG = 'l',//32-bit integer 58 | GPMF_TYPE_UNSIGNED_LONG = 'L', //32-bit integer 59 | GPMF_TYPE_Q15_16_FIXED_POINT = 'q', // Q number Q15.16 - 16-bit signed integer (A) with 16-bit fixed point (B) for A.B value (range -32768.0 to 32767.99998). 60 | GPMF_TYPE_Q31_32_FIXED_POINT = 'Q', // Q number Q31.32 - 32-bit signed integer (A) with 32-bit fixed point (B) for A.B value. 61 | GPMF_TYPE_SIGNED_64BIT_INT = 'j', //64 bit signed long 62 | GPMF_TYPE_UNSIGNED_64BIT_INT = 'J', //64 bit unsigned long 63 | GPMF_TYPE_DOUBLE = 'd', //64 bit double precision float (IEEE 754) 64 | GPMF_TYPE_STRING_UTF8 = 'u', //UTF-8 formatted text string. As the character storage size varies, the size is in bytes, not UTF characters. 65 | GPMF_TYPE_UTC_DATE_TIME = 'U', //128-bit ASCII Date + UTC Time format yymmddhhmmss.sss - 16 bytes ASCII (years 20xx covered) 66 | GPMF_TYPE_GUID = 'G', //128-bit ID (like UUID) 67 | 68 | GPMF_TYPE_COMPLEX = '?', //for sample with complex data structures, base size in bytes. Data is either opaque, or the stream has a TYPE structure field for the sample. 69 | GPMF_TYPE_COMPRESSED = '#', //Huffman compression STRM payloads. 4-CC is compressed as 4-CC '#' 70 | 71 | GPMF_TYPE_NEST = 0, // used to nest more GPMF formatted metadata 72 | 73 | /* ------------- Internal usage only ------------- */ 74 | GPMF_TYPE_EMPTY = 0xfe, // used to distinguish between grouped metadata (like FACE) with no data (no faces detected) and an empty payload (FACE device reported no samples.) 75 | GPMF_TYPE_ERROR = 0xff // used to report an error 76 | } GPMF_SampleType; 77 | 78 | 79 | 80 | #define MAKEID(a,b,c,d) (((d&0xff)<<24)|((c&0xff)<<16)|((b&0xff)<<8)|(a&0xff)) 81 | #define STR2FOURCC(s) ((s[0]<<0)|(s[1]<<8)|(s[2]<<16)|(s[3]<<24)) 82 | 83 | #define BYTESWAP64(a) (((a&0xff)<<56)|((a&0xff00)<<40)|((a&0xff0000)<<24)|((a&0xff000000)<<8) | ((a>>56)&0xff)|((a>>40)&0xff00)|((a>>24)&0xff0000)|((a>>8)&0xff000000) ) 84 | #define BYTESWAP32(a) (((a&0xff)<<24)|((a&0xff00)<<8)|((a>>8)&0xff00)|((a>>24)&0xff)) 85 | #define BYTESWAP16(a) ((((a)>>8)&0xff)|(((a)<<8)&0xff00)) 86 | #define BYTESWAP2x16(a) (((a>>8)&0xff)|((a<<8)&0xff00)|((a>>8)&0xff0000)|((a<<8)&0xff000000)) 87 | #define NOSWAP8(a) (a) 88 | 89 | #define GPMF_SAMPLES(a) (((a>>24) & 0xff)|(((a>>16)&0xff)<<8)) 90 | #define GPMF_SAMPLE_SIZE(a) (((a)>>8)&0xff) 91 | #define GPMF_SAMPLE_TYPE(a) (a&0xff) 92 | #define GPMF_MAKE_TYPE_SIZE_COUNT(t,s,c) ((t)&0xff)|(((s)&0xff)<<8)|(((c)&0xff)<<24)|(((c)&0xff00)<<8) 93 | #define GPMF_DATA_SIZE(a) ((GPMF_SAMPLE_SIZE(a)*GPMF_SAMPLES(a)+3)&~0x3) 94 | #define GPMF_DATA_PACKEDSIZE(a) ((GPMF_SAMPLE_SIZE(a)*GPMF_SAMPLES(a))) 95 | #define GPMF_VALID_FOURCC(a) (((((a>>24)&0xff)>='a'&&((a>>24)&0xff)<='z') || (((a>>24)&0xff)>='A'&&((a>>24)&0xff)<='Z') || (((a>>24)&0xff)>='0'&&((a>>24)&0xff)<='9') || (((a>>24)&0xff)==' ') ) && \ 96 | ( (((a>>16)&0xff)>='a'&&((a>>16)&0xff)<='z') || (((a>>16)&0xff)>='A'&&((a>>16)&0xff)<='Z') || (((a>>16)&0xff)>='0'&&((a>>16)&0xff)<='9') || (((a>>16)&0xff)==' ') ) && \ 97 | ( (((a>>8)&0xff)>='a'&&((a>>8)&0xff)<='z') || (((a>>8)&0xff)>='A'&&((a>>8)&0xff)<='Z') || (((a>>8)&0xff)>='0'&&((a>>8)&0xff)<='9') || (((a>>8)&0xff)==' ') ) && \ 98 | ( (((a>>0)&0xff)>='a'&&((a>>0)&0xff)<='z') || (((a>>0)&0xff)>='A'&&((a>>0)&0xff)<='Z') || (((a>>0)&0xff)>='0'&&((a>>0)&0xff)<='9') || (((a>>0)&0xff)==' ') )) 99 | 100 | #define PRINTF_4CC(k) ((k) >> 0) & 0xff, ((k) >> 8) & 0xff, ((k) >> 16) & 0xff, ((k) >> 24) & 0xff 101 | 102 | 103 | typedef enum GPMFKey // TAG in all caps are GoPro preserved (are defined by GoPro, but can be used by others.) 104 | { 105 | // Internal Metadata structure and formatting tags 106 | GPMF_KEY_DEVICE = MAKEID('D','E','V','C'),//DEVC - nested device data to speed the parsing of multiple devices in post 107 | GPMF_KEY_DEVICE_ID = MAKEID('D','V','I','D'),//DVID - unique id per stream for a metadata source (in camera or external input) (single 4 byte int) 108 | GPMF_KEY_DEVICE_NAME = MAKEID('D','V','N','M'),//DVNM - human readable device type/name (char string) 109 | GPMF_KEY_STREAM = MAKEID('S','T','R','M'),//STRM - nested channel/stream of telemetry data 110 | GPMF_KEY_STREAM_NAME = MAKEID('S','T','N','M'),//STNM - human readable telemetry/metadata stream type/name (char string) 111 | GPMF_KEY_SI_UNITS = MAKEID('S','I','U','N'),//SIUN - Display string for metadata units where inputs are in SI units "uT","rad/s","km/s","m/s","mm/s" etc. 112 | GPMF_KEY_UNITS = MAKEID('U','N','I','T'),//UNIT - Freedform display string for metadata units (char sting like "RPM", "MPH", "km/h", etc) 113 | GPMF_KEY_MATRIX = MAKEID('M','T','R','X'),//MTRX - 2D matrix for any sensor calibration. 114 | GPMF_KEY_ORIENTATION_IN = MAKEID('O','R','I','N'),//ORIN - input 'n' channel data orientation, lowercase is negative, e.g. "Zxy" or "ABGR". 115 | GPMF_KEY_ORIENTATION_OUT = MAKEID('O','R','I','O'),//ORIO - output 'n' channel data orientation, e.g. "XYZ" or "RGBA". 116 | GPMF_KEY_SCALE = MAKEID('S','C','A','L'),//SCAL - divisor for input data to scale to the correct units. 117 | GPMF_KEY_TYPE = MAKEID('T','Y','P','E'),//TYPE - Type define for complex data structures 118 | GPMF_KEY_TOTAL_SAMPLES = MAKEID('T','S','M','P'),//TOTL - Total Sample Count including the current payload 119 | GPMF_KEY_TICK = MAKEID('T','I','C','K'),//TICK - Beginning of data timing (arrival) in milliseconds. 120 | GPMF_KEY_TOCK = MAKEID('T','O','C','K'),//TOCK - End of data timing (arrival) in milliseconds. 121 | GPMF_KEY_TIME_OFFSET = MAKEID('T','I','M','O'),//TIMO - Time offset of the metadata stream that follows (single 4 byte float) 122 | GPMF_KEY_TIMING_OFFSET = MAKEID('T','I','M','O'),//TIMO - duplicated, as older code might use the other version of TIMO 123 | GPMF_KEY_TIME_STAMP = MAKEID('S','T','M','P'),//STMP - Time stamp for the first sample. 124 | GPMF_KEY_TIME_STAMPS = MAKEID('S','T','P','S'),//STPS - Stream of all the timestamps delivered (Generally don't use this. This would be if your sensor has no peroidic times, yet precision is required, or for debugging.) 125 | GPMF_KEY_PREFORMATTED = MAKEID('P','F','R','M'),//PFRM - GPMF data 126 | GPMF_KEY_TEMPERATURE_C = MAKEID('T','M','P','C'),//TMPC - Temperature in Celsius 127 | GPMF_KEY_EMPTY_PAYLOADS = MAKEID('E','M','P','T'),//EMPT - Payloads that are empty since the device start (e.g. BLE disconnect.) 128 | GPMF_KEY_QUANTIZE = MAKEID('Q','U','A','N'),//QUAN - quantize used to enable stream compression - 1 - enable, 2+ enable and quantize by this value 129 | GPMF_KEY_VERSION = MAKEID('V','E','R','S'),//VERS - version of the metadata stream (debugging) 130 | GPMF_KEY_FREESPACE = MAKEID('F','R','E','E'),//FREE - n bytes reserved for more metadata added to an existing stream 131 | GPMF_KEY_REMARK = MAKEID('R','M','R','K'),//RMRK - adding comments to the bitstream (debugging) 132 | 133 | GPMF_KEY_END = 0//(null) 134 | } GPMFKey; 135 | 136 | 137 | 138 | #ifdef __cplusplus 139 | } 140 | #endif 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /gopro_to_rosbag.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bjoshi on 10/29/20. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include "ImuExtractor.h" 15 | #include "VideoExtractor.h" 16 | #include "color_codes.h" 17 | 18 | using namespace std; 19 | namespace fs = std::experimental::filesystem; 20 | 21 | int main(int argc, char* argv[]) { 22 | ros::init(argc, argv, "gopro_to_rosbag"); 23 | ros::NodeHandle nh_private("~"); 24 | 25 | string gopro_video; 26 | string gopro_folder; 27 | string rosbag; 28 | 29 | bool is_gopro_video = nh_private.getParam("gopro_video", gopro_video); 30 | bool is_gopro_folder = nh_private.getParam("gopro_folder", gopro_folder); 31 | 32 | if (!is_gopro_video && !is_gopro_folder) { 33 | ROS_FATAL("Please specify the gopro video or folder"); 34 | ros::shutdown(); 35 | } 36 | 37 | ROS_FATAL_STREAM_COND(!nh_private.getParam("rosbag", rosbag), "No rosbag file specified"); 38 | 39 | double scaling = 1.0; 40 | if (nh_private.hasParam("scale")) nh_private.getParam("scale", scaling); 41 | 42 | bool compress_images = false; 43 | if (nh_private.hasParam("compressed_image_format")) 44 | nh_private.getParam("compressed_image_format", compress_images); 45 | 46 | bool grayscale = false; 47 | if (nh_private.hasParam("grayscale")) nh_private.getParam("grayscale", grayscale); 48 | 49 | bool display_images = false; 50 | if (nh_private.hasParam("display_images")) nh_private.getParam("display_images", display_images); 51 | 52 | bool multiple_files = false; 53 | if (nh_private.hasParam("multiple_files")) nh_private.getParam("multiple_files", multiple_files); 54 | 55 | rosbag::Bag bag; 56 | bag.open(rosbag, rosbag::bagmode::Write); 57 | 58 | vector video_files; 59 | 60 | if (is_gopro_folder && multiple_files) { 61 | std::copy(fs::directory_iterator(gopro_folder), 62 | fs::directory_iterator(), 63 | std::back_inserter(video_files)); 64 | std::sort(video_files.begin(), video_files.end()); 65 | 66 | } else { 67 | video_files.push_back(fs::path(gopro_video)); 68 | } 69 | 70 | auto end = std::remove_if(video_files.begin(), video_files.end(), [](const fs::path& p) { 71 | return p.extension() != ".MP4" || fs::is_directory(p); 72 | }); 73 | video_files.erase(end, video_files.end()); 74 | 75 | vector start_stamps; 76 | vector samples; 77 | std::deque accl_queue; 78 | std::deque gyro_queue; 79 | std::deque magnetometer_queue; 80 | 81 | vector image_stamps; 82 | 83 | bool has_magnetic_field_readings = false; 84 | for (uint32_t i = 0; i < video_files.size(); i++) { 85 | image_stamps.clear(); 86 | 87 | ROS_WARN_STREAM("Opening Video File: " << video_files[i].filename().string()); 88 | 89 | fs::path file = video_files[i]; 90 | GoProImuExtractor imu_extractor(file.string()); 91 | GoProVideoExtractor video_extractor(file.string(), scaling, true); 92 | 93 | if (i == 0 && imu_extractor.getNumofSamples(STR2FOURCC("MAGN"))) { 94 | has_magnetic_field_readings = true; 95 | } 96 | 97 | imu_extractor.getPayloadStamps(STR2FOURCC("ACCL"), start_stamps, samples); 98 | ROS_INFO_STREAM("[ACCL] Payloads: " << start_stamps.size() 99 | << " Start stamp: " << start_stamps[0] 100 | << " End stamp: " << start_stamps[samples.size() - 1] 101 | << " Total Samples: " << samples.at(samples.size() - 1)); 102 | imu_extractor.getPayloadStamps(STR2FOURCC("GYRO"), start_stamps, samples); 103 | ROS_INFO_STREAM("[GYRO] Payloads: " << start_stamps.size() 104 | << " Start stamp: " << start_stamps[0] 105 | << " End stamp: " << start_stamps[samples.size() - 1] 106 | << " Total Samples: " << samples.at(samples.size() - 1)); 107 | imu_extractor.getPayloadStamps(STR2FOURCC("CORI"), start_stamps, samples); 108 | ROS_INFO_STREAM("[Image] Payloads: " << start_stamps.size() 109 | << " Start stamp: " << start_stamps[0] 110 | << " End stamp: " << start_stamps[samples.size() - 1] 111 | << " Total Samples: " << samples.at(samples.size() - 1)); 112 | if (has_magnetic_field_readings) { 113 | imu_extractor.getPayloadStamps(STR2FOURCC("MAGN"), start_stamps, samples); 114 | ROS_INFO_STREAM("[MAGN] Payloads: " << start_stamps.size() 115 | << " Start stamp: " << start_stamps[0] 116 | << " End stamp: " << start_stamps[samples.size() - 1] 117 | << " Total Samples: " << samples.at(samples.size() - 1)); 118 | } 119 | 120 | uint64_t accl_end_stamp = 0, gyro_end_stamp = 0; 121 | uint64_t video_end_stamp = 0; 122 | uint64_t magnetometer_end_stamp = 0; 123 | 124 | if (i < video_files.size() - 1) { 125 | GoProImuExtractor imu_extractor_next(video_files[i + 1].string()); 126 | accl_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("ACCL"), 0); 127 | gyro_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("GYRO"), 0); 128 | video_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("CORI"), 0); 129 | if (has_magnetic_field_readings) { 130 | magnetometer_end_stamp = imu_extractor_next.getPayloadStartStamp(STR2FOURCC("MAGN"), 0); 131 | } 132 | } 133 | 134 | imu_extractor.readImuData(accl_queue, gyro_queue, accl_end_stamp, gyro_end_stamp); 135 | imu_extractor.readMagnetometerData(magnetometer_queue, magnetometer_end_stamp); 136 | 137 | uint32_t gpmf_frame_count = imu_extractor.getImageCount(); 138 | uint32_t ffmpeg_frame_count = video_extractor.getFrameCount(); 139 | if (gpmf_frame_count != ffmpeg_frame_count) { 140 | ROS_FATAL_STREAM("Video and metadata frame count do not match"); 141 | ros::shutdown(); 142 | } 143 | 144 | uint64_t gpmf_video_time = imu_extractor.getVideoCreationTime(); 145 | uint64_t ffmpeg_video_time = video_extractor.getVideoCreationTime(); 146 | 147 | if (ffmpeg_video_time != gpmf_video_time) { 148 | ROS_FATAL_STREAM("Video creation time does not match"); 149 | ros::shutdown(); 150 | } 151 | 152 | imu_extractor.getImageStamps(image_stamps, video_end_stamp); 153 | if (i != video_files.size() - 1 && image_stamps.size() != ffmpeg_frame_count) { 154 | ROS_FATAL_STREAM("ffmpeg and gpmf frame count does not match."); 155 | ROS_FATAL_STREAM(image_stamps.size() << " vs " << ffmpeg_frame_count); 156 | ros::shutdown(); 157 | } 158 | 159 | video_extractor.writeVideo( 160 | bag, "/gopro/image_raw", image_stamps, grayscale, compress_images, display_images); 161 | } 162 | 163 | ROS_INFO_STREAM("[ACCL] Payloads: " << accl_queue.size()); 164 | ROS_INFO_STREAM("[GYRO] Payloads: " << gyro_queue.size()); 165 | 166 | assert(accl_queue.size() == gyro_queue.size()); 167 | 168 | double previous = accl_queue.front().timestamp_ * 1e-9; 169 | 170 | while (!accl_queue.empty() && !gyro_queue.empty()) { 171 | AcclMeasurement accl = accl_queue.front(); 172 | GyroMeasurement gyro = gyro_queue.front(); 173 | int64_t diff = accl.timestamp_ - gyro.timestamp_; 174 | 175 | Timestamp stamp; 176 | if (abs(diff) > 100000) { 177 | // I will need to handle this case more carefully 178 | ROS_WARN_STREAM(diff << " ns difference between gyro and accl"); 179 | stamp = (Timestamp)(((double)accl.timestamp_ + (double)gyro.timestamp_) / 2.0); 180 | } else { 181 | stamp = accl.timestamp_; 182 | } 183 | 184 | double current = stamp * 1e-9; 185 | // assert(abs(current - previous) < 0.001); 186 | previous = current; 187 | uint32_t secs = stamp * 1e-9; 188 | uint32_t n_secs = stamp % 1000000000; 189 | ros::Time ros_time(secs, n_secs); 190 | sensor_msgs::Imu imu_msg; 191 | std_msgs::Header header; 192 | header.stamp = ros_time; 193 | header.frame_id = "body"; 194 | imu_msg.header = header; 195 | imu_msg.linear_acceleration.x = accl.data_.x(); 196 | imu_msg.linear_acceleration.y = accl.data_.y(); 197 | imu_msg.linear_acceleration.z = accl.data_.z(); 198 | imu_msg.angular_velocity.x = gyro.data_.x(); 199 | imu_msg.angular_velocity.y = gyro.data_.y(); 200 | imu_msg.angular_velocity.z = gyro.data_.z(); 201 | 202 | bag.write("/gopro/imu", ros_time, imu_msg); 203 | 204 | accl_queue.pop_front(); 205 | gyro_queue.pop_front(); 206 | } 207 | 208 | while (!magnetometer_queue.empty()) { 209 | MagMeasurement mag = magnetometer_queue.front(); 210 | Timestamp stamp = mag.timestamp_; 211 | 212 | uint32_t secs = stamp * 1e-9; 213 | uint32_t n_secs = stamp % 1000000000; 214 | ros::Time ros_time(secs, n_secs); 215 | sensor_msgs::MagneticField magnetic_field_msg; 216 | std_msgs::Header header; 217 | header.stamp = ros_time; 218 | header.frame_id = "body"; 219 | magnetic_field_msg.header = header; 220 | magnetic_field_msg.magnetic_field.x = mag.magfield_.x(); 221 | magnetic_field_msg.magnetic_field.y = mag.magfield_.y(); 222 | magnetic_field_msg.magnetic_field.z = mag.magfield_.z(); 223 | 224 | bag.write("/gopro/magnetic_field", ros_time, magnetic_field_msg); 225 | 226 | magnetometer_queue.pop_front(); 227 | } 228 | } 229 | -------------------------------------------------------------------------------- /src/GPMF_utils.c: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file GPMF_utils.c 4 | * 5 | * @brief Utilities GPMF and MP4 handling 6 | * 7 | * @version 1.2.0 8 | * 9 | * (C) Copyright 2020 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under either: 12 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 13 | * - MIT license, http://opensource.org/licenses/MIT 14 | * at your option. 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | * 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "GPMF_utils.h" 30 | 31 | 32 | double GetGPMFSampleRate(mp4callbacks cb, uint32_t fourcc, uint32_t timeBaseFourCC, uint32_t flags, double *firstsampletime, double *lastsampletime) 33 | { 34 | if (cb.mp4handle == 0) 35 | return 0.0; 36 | 37 | uint32_t indexcount = cb.cbGetNumberPayloads(cb.mp4handle); 38 | 39 | GPMF_stream metadata_stream, *ms = &metadata_stream; 40 | uint32_t teststart = 0; 41 | uint32_t testend = indexcount; 42 | double rate = 0.0; 43 | 44 | uint32_t *payload; 45 | uint32_t payloadsize; 46 | size_t payloadres = 0; 47 | 48 | GPMF_ERR ret; 49 | 50 | if (indexcount < 1) 51 | return 0.0; 52 | 53 | payloadsize = cb.cbGetPayloadSize(cb.mp4handle, teststart); 54 | payloadres = cb.cbGetPayloadResource(cb.mp4handle, 0, payloadsize); 55 | payload = cb.cbGetPayload(cb.mp4handle, payloadres, teststart); 56 | 57 | ret = GPMF_Init(ms, payload, payloadsize); 58 | 59 | if (ret != GPMF_OK) 60 | goto cleanup; 61 | 62 | { 63 | uint64_t basetimestamp = 0; 64 | uint64_t starttimestamp = 0; 65 | uint64_t endtimestamp = 0; 66 | uint32_t startsamples = 0; 67 | uint32_t endsamples = 0; 68 | double intercept = 0.0; 69 | 70 | 71 | 72 | while (teststart < indexcount && ret == GPMF_OK && GPMF_OK != GPMF_FindNext(ms, fourcc, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)) 73 | { 74 | teststart++; 75 | payloadsize = cb.cbGetPayloadSize(cb.mp4handle, teststart); 76 | payload = cb.cbGetPayload(cb.mp4handle, payloadres, teststart); // second last payload 77 | ret = GPMF_Init(ms, payload, payloadsize); 78 | } 79 | 80 | if (ret == GPMF_OK && payload) 81 | { 82 | double startin, startout, endin, endout; 83 | int usedTimeStamps = 0; 84 | 85 | uint32_t samples = GPMF_PayloadSampleCount(ms); 86 | GPMF_stream find_stream; 87 | GPMF_CopyState(ms, &find_stream); //ms is at the searched fourcc 88 | if (GPMF_OK == GPMF_FindPrev(&find_stream, GPMF_KEY_TOTAL_SAMPLES, GPMF_CURRENT_LEVEL)) 89 | startsamples = BYTESWAP32(*(uint32_t *)GPMF_RawData(&find_stream)) - samples; 90 | 91 | GPMF_CopyState(ms, &find_stream); 92 | if (GPMF_OK == GPMF_FindPrev(&find_stream, GPMF_KEY_TIME_STAMP, GPMF_CURRENT_LEVEL)) 93 | starttimestamp = BYTESWAP64(*(uint64_t *)GPMF_RawData(&find_stream)); 94 | 95 | if (starttimestamp) // how does this compare to other streams in this early payload? 96 | { 97 | GPMF_stream any_stream; 98 | if (GPMF_OK == GPMF_Init(&any_stream, payload, payloadsize)) 99 | { 100 | basetimestamp = starttimestamp; 101 | 102 | if (timeBaseFourCC) 103 | { 104 | if (GPMF_OK == GPMF_FindNext(&any_stream, timeBaseFourCC, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)) 105 | { 106 | if (GPMF_OK == GPMF_FindPrev(&any_stream, GPMF_KEY_TIME_STAMP, GPMF_CURRENT_LEVEL)) 107 | { 108 | basetimestamp = BYTESWAP64(*(uint64_t*)GPMF_RawData(&any_stream)); 109 | } 110 | } 111 | } 112 | else 113 | { 114 | while (GPMF_OK == GPMF_FindNext(&any_stream, GPMF_KEY_TIME_STAMP, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)) 115 | { 116 | uint64_t timestamp = BYTESWAP64(*(uint64_t*)GPMF_RawData(&any_stream)); 117 | if (timestamp < basetimestamp) 118 | basetimestamp = timestamp; 119 | } 120 | } 121 | } 122 | } 123 | //Note: basetimestamp is used the remove offset from the timestamp, 124 | // however 0.0 may not be the same zero for your video or audio presentation time (although it should be close.) 125 | // On GoPro camera, metadata streams like SHUT and ISOE are metadata fields associated with video, and these can be used 126 | // to accurately sync meta with video. 127 | 128 | testend = indexcount; 129 | do 130 | { 131 | testend--;// last payload with the fourcc needed 132 | payloadsize = cb.cbGetPayloadSize(cb.mp4handle, testend); 133 | payload = cb.cbGetPayload(cb.mp4handle, payloadres, testend); // second last payload 134 | ret = GPMF_Init(ms, payload, payloadsize); 135 | } while (testend > 0 && ret == GPMF_OK && GPMF_OK != GPMF_FindNext(ms, fourcc, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)); 136 | 137 | cb.cbGetPayloadTime(cb.mp4handle, teststart, &startin, &startout); 138 | cb.cbGetPayloadTime(cb.mp4handle, testend, &endin, &endout); 139 | 140 | GPMF_CopyState(ms, &find_stream); 141 | if (GPMF_OK == GPMF_FindPrev(&find_stream, GPMF_KEY_TOTAL_SAMPLES, GPMF_CURRENT_LEVEL)) 142 | endsamples = BYTESWAP32(*(uint32_t *)GPMF_RawData(&find_stream)); 143 | else // If there is no TSMP we have to count the samples. 144 | { 145 | uint32_t i; 146 | for (i = teststart; i <= testend; i++) 147 | { 148 | payloadsize = cb.cbGetPayloadSize(cb.mp4handle, i); 149 | payload = cb.cbGetPayload(cb.mp4handle, payloadres, i); 150 | if (GPMF_OK == GPMF_Init(ms, payload, payloadsize)) 151 | if (GPMF_OK == GPMF_FindNext(ms, fourcc, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)) 152 | endsamples += GPMF_PayloadSampleCount(ms); 153 | } 154 | } 155 | 156 | if (starttimestamp != 0) 157 | { 158 | uint32_t last_samples = GPMF_PayloadSampleCount(ms); 159 | uint32_t totaltimestamped_samples = endsamples - last_samples - startsamples; 160 | double time_stamp_scale = 1000000000.0; // scan for nanoseconds, microseconds to seconds, all base 10. 161 | 162 | GPMF_CopyState(ms, &find_stream); 163 | if (GPMF_OK == GPMF_FindPrev(&find_stream, GPMF_KEY_TIME_STAMP, GPMF_CURRENT_LEVEL)) 164 | endtimestamp = BYTESWAP64(*(uint64_t *)GPMF_RawData(&find_stream)); 165 | 166 | if (endtimestamp) 167 | { 168 | double approxrate = 0.0; 169 | if (endsamples > startsamples) 170 | approxrate = (double)(endsamples - startsamples) / (endout - startin); 171 | 172 | if (approxrate == 0.0) 173 | approxrate = (double)(samples) / (endout - startin); 174 | 175 | 176 | while (time_stamp_scale >= 1) 177 | { 178 | rate = (double)(totaltimestamped_samples) / ((double)(endtimestamp - starttimestamp) / time_stamp_scale); 179 | if (rate*0.9 < approxrate && approxrate < rate*1.1) 180 | break; 181 | 182 | time_stamp_scale *= 0.1; 183 | } 184 | if (time_stamp_scale < 1.0) rate = 0.0; 185 | intercept = (((double)basetimestamp - (double)starttimestamp) / time_stamp_scale) * rate; 186 | usedTimeStamps = 1; 187 | } 188 | } 189 | 190 | if (rate == 0.0) //Timestamps didn't help, or weren't available 191 | { 192 | if (!(flags & GPMF_SAMPLE_RATE_PRECISE)) 193 | { 194 | if (endsamples > startsamples) 195 | rate = (double)(endsamples - startsamples) / (endout - startin); 196 | 197 | if (rate == 0.0) 198 | rate = (double)(samples) / (endout - startin); 199 | 200 | intercept = (double)-startin * rate; 201 | } 202 | else // for increased precision, for older GPMF streams sometimes missing the total sample count 203 | { 204 | uint32_t payloadpos = 0, payloadcount = 0; 205 | double slope, top = 0.0, bot = 0.0, meanX = 0, meanY = 0; 206 | uint32_t *repeatarray = (uint32_t *)malloc(indexcount * 4 + 4); 207 | memset(repeatarray, 0, indexcount * 4 + 4); 208 | 209 | samples = 0; 210 | 211 | for (payloadpos = teststart; payloadpos <= testend; payloadpos++) 212 | { 213 | 214 | payloadsize = cb.cbGetPayloadSize(cb.mp4handle, payloadpos); 215 | payload = cb.cbGetPayload(cb.mp4handle, payloadres, payloadpos); // second last payload 216 | ret = GPMF_Init(ms, payload, payloadsize); 217 | 218 | if (ret != GPMF_OK) 219 | goto cleanup; 220 | 221 | if (GPMF_OK == GPMF_FindNext(ms, fourcc, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)) 222 | { 223 | GPMF_stream find_stream2; 224 | GPMF_CopyState(ms, &find_stream2); 225 | 226 | payloadcount++; 227 | 228 | if (GPMF_OK == GPMF_FindNext(&find_stream2, fourcc, GPMF_CURRENT_LEVEL)) // Count the instances, not the repeats 229 | { 230 | if (repeatarray) 231 | { 232 | double in, out; 233 | 234 | do 235 | { 236 | samples++; 237 | } while (GPMF_OK == GPMF_FindNext(ms, fourcc, GPMF_CURRENT_LEVEL)); 238 | 239 | repeatarray[payloadpos] = samples; 240 | meanY += (double)samples; 241 | 242 | if (GPMF_OK == cb.cbGetPayloadTime(cb.mp4handle, payloadpos, &in, &out)) 243 | meanX += out; 244 | } 245 | } 246 | else 247 | { 248 | uint32_t repeat = GPMF_PayloadSampleCount(ms); 249 | samples += repeat; 250 | 251 | if (repeatarray) 252 | { 253 | double in, out; 254 | 255 | repeatarray[payloadpos] = samples; 256 | meanY += (double)samples; 257 | 258 | if (GPMF_OK == cb.cbGetPayloadTime(cb.mp4handle, payloadpos, &in, &out)) 259 | meanX += out; 260 | } 261 | } 262 | } 263 | else 264 | { 265 | repeatarray[payloadpos] = 0; 266 | } 267 | } 268 | 269 | // Compute the line of best fit for a jitter removed sample rate. 270 | // This does assume an unchanging clock, even though the IMU data can thermally impacted causing small clock changes. 271 | // TODO: Next enhancement would be a low order polynominal fit the compensate for any thermal clock drift. 272 | if (repeatarray) 273 | { 274 | meanY /= (double)payloadcount; 275 | meanX /= (double)payloadcount; 276 | 277 | for (payloadpos = teststart; payloadpos <= testend; payloadpos++) 278 | { 279 | double in, out; 280 | if (repeatarray[payloadpos] && GPMF_OK == cb.cbGetPayloadTime(cb.mp4handle, payloadpos, &in, &out)) 281 | { 282 | top += ((double)out - meanX)*((double)repeatarray[payloadpos] - meanY); 283 | bot += ((double)out - meanX)*((double)out - meanX); 284 | } 285 | } 286 | 287 | slope = top / bot; 288 | rate = slope; 289 | 290 | // This sample code might be useful for compare data latency between channels. 291 | intercept = meanY - slope * meanX; 292 | #if 0 293 | printf("%c%c%c%c start offset = %f (%.3fms) rate = %f\n", PRINTF_4CC(fourcc), intercept, 1000.0 * intercept / slope, rate); 294 | printf("%c%c%c%c first sample at time %.3fms\n", PRINTF_4CC(fourcc), -1000.0 * intercept / slope); 295 | #endif 296 | } 297 | else 298 | { 299 | rate = (double)(samples) / (endout - startin); 300 | } 301 | 302 | free(repeatarray); 303 | } 304 | } 305 | 306 | if (firstsampletime && lastsampletime) 307 | { 308 | uint32_t endpayload = indexcount; 309 | do 310 | { 311 | endpayload--;// last payload with the fourcc needed 312 | payloadsize = cb.cbGetPayloadSize(cb.mp4handle, endpayload); 313 | payload = cb.cbGetPayload(cb.mp4handle, payloadres, endpayload); 314 | ret = GPMF_Init(ms, payload, payloadsize); 315 | } while (endpayload > 0 && ret == GPMF_OK && GPMF_OK != GPMF_FindNext(ms, fourcc, GPMF_RECURSE_LEVELS | GPMF_TOLERANT)); 316 | 317 | if (endpayload > 0 && ret == GPMF_OK) 318 | { 319 | uint32_t totalsamples = endsamples - startsamples; 320 | float timo = 0.0; 321 | 322 | GPMF_CopyState(ms, &find_stream); 323 | if (GPMF_OK == GPMF_FindPrev(&find_stream, GPMF_KEY_TIME_OFFSET, GPMF_CURRENT_LEVEL)) 324 | GPMF_FormattedData(&find_stream, &timo, 4, 0, 1); 325 | 326 | double first, last; 327 | first = -intercept / rate - timo; 328 | last = first + (double)totalsamples / rate; 329 | 330 | //Apply any Edit List corrections. 331 | if (usedTimeStamps) // clips with STMP have the Edit List already applied via GetPayloadTime() 332 | { 333 | if (cb.cbGetEditListOffsetRationalTime) 334 | { 335 | int32_t num = 0; 336 | uint32_t dem = 1; 337 | cb.cbGetEditListOffsetRationalTime(cb.mp4handle, &num, &dem); 338 | first += (double)num / (double)dem; 339 | last += (double)num / (double)dem; 340 | } 341 | } 342 | 343 | //printf("%c%c%c%c first sample at time %.3fms, last at %.3fms\n", PRINTF_4CC(fourcc), 1000.0*first, 1000.0*last); 344 | 345 | if (firstsampletime) *firstsampletime = first; 346 | 347 | if (lastsampletime) *lastsampletime = last; 348 | } 349 | } 350 | } 351 | } 352 | 353 | cleanup: 354 | if (payloadres) cb.cbFreePayloadResource(cb.mp4handle, payloadres); 355 | return rate; 356 | } 357 | 358 | -------------------------------------------------------------------------------- /src/GPMF_print.cpp: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file GPMF_print.c 4 | * 5 | * @brief Demo to output GPMF for debugging 6 | * 7 | * @version 1.1.0 8 | * 9 | * (C) Copyright 2017 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under either: 12 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 13 | * - MIT license, http://opensource.org/licenses/MIT 14 | * at your option. 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | * 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "GPMF_parser.h" 30 | 31 | 32 | #define DBG_MSG printf 33 | 34 | 35 | #define VERBOSE_OUTPUT 0 36 | #define VERBOSE_LIMIT 6 37 | 38 | #if VERBOSE_OUTPUT 39 | #define LIMITOUTPUT arraysize = structsize; 40 | #else 41 | #define LIMITOUTPUT if (arraysize > 1 && arraysize*repeat > VERBOSE_LIMIT) repeat = (VERBOSE_LIMIT/arraysize)+1, dots = 1; else if (repeat > VERBOSE_LIMIT) repeat = VERBOSE_LIMIT, dots = 1; 42 | #endif 43 | 44 | void printfData(uint32_t type, uint32_t structsize, uint32_t repeat, void *data) 45 | { 46 | int dots = 0; 47 | 48 | switch (type) 49 | { 50 | case GPMF_TYPE_STRING_ASCII: 51 | { 52 | char t[256]; 53 | int size = structsize*repeat; 54 | uint32_t arraysize = structsize; 55 | LIMITOUTPUT; 56 | 57 | if (size > 255) 58 | { 59 | size = 255; 60 | } 61 | memcpy(t, (char *)data, size); 62 | t[size] = 0; 63 | 64 | if (arraysize == 1 || repeat == 1) 65 | { 66 | DBG_MSG("\"%s\"", t); 67 | dots = 0; 68 | } 69 | else 70 | { 71 | uint32_t i,j,pos=0; 72 | for (i = 0; i < repeat; i++) 73 | { 74 | DBG_MSG("\""); 75 | for (j = 0; j < arraysize; j++) 76 | { 77 | if (t[pos] != '\0' && t[pos] != ' ') 78 | DBG_MSG("%c", t[pos]); 79 | pos++; 80 | } 81 | DBG_MSG("\", "); 82 | } 83 | } 84 | } 85 | break; 86 | case GPMF_TYPE_SIGNED_BYTE: 87 | { 88 | int8_t *b = (int8_t *)data; 89 | uint32_t arraysize = structsize; 90 | LIMITOUTPUT; 91 | 92 | while (repeat--) 93 | { 94 | arraysize = structsize; 95 | 96 | while (arraysize--) 97 | { 98 | DBG_MSG("%d,", (int8_t)*b); 99 | b++; 100 | } 101 | if (repeat) DBG_MSG(" "); 102 | } 103 | } 104 | break; 105 | case GPMF_TYPE_UNSIGNED_BYTE: 106 | { 107 | uint8_t *b = (uint8_t *)data; 108 | uint32_t arraysize = structsize; 109 | LIMITOUTPUT; 110 | 111 | while (repeat--) 112 | { 113 | arraysize = structsize; 114 | 115 | while (arraysize--) 116 | { 117 | DBG_MSG("%d,", *b); 118 | b++; 119 | } 120 | if (repeat) DBG_MSG(" "); 121 | } 122 | } 123 | break; 124 | case GPMF_TYPE_DOUBLE: 125 | { 126 | uint64_t Swap, *L = (uint64_t *)data; 127 | double *d; 128 | uint32_t arraysize = structsize / sizeof(uint64_t); 129 | LIMITOUTPUT; 130 | 131 | while (repeat--) 132 | { 133 | arraysize = structsize / sizeof(uint64_t); 134 | 135 | while (arraysize--) 136 | { 137 | Swap = BYTESWAP64(*L); 138 | d = (double *)&Swap; 139 | DBG_MSG("%.3f,", *d); 140 | L++; 141 | } 142 | if (repeat) DBG_MSG(" "); 143 | } 144 | } 145 | break; 146 | case GPMF_TYPE_FLOAT: 147 | { 148 | uint32_t Swap, *L = (uint32_t *)data; 149 | float *f; 150 | uint32_t arraysize = structsize / sizeof(uint32_t); 151 | LIMITOUTPUT; 152 | 153 | while (repeat--) 154 | { 155 | arraysize = structsize / sizeof(uint32_t); 156 | 157 | while (arraysize--) 158 | { 159 | Swap = BYTESWAP32(*L); 160 | f = (float *)&Swap; 161 | DBG_MSG("%.3f,", *f); 162 | L++; 163 | } 164 | if (repeat) DBG_MSG(" "); 165 | } 166 | } 167 | break; 168 | case GPMF_TYPE_FOURCC: 169 | { 170 | uint32_t *L = (uint32_t *)data; 171 | uint32_t arraysize = structsize / sizeof(uint32_t); 172 | LIMITOUTPUT; 173 | while (repeat--) 174 | { 175 | arraysize = structsize / sizeof(uint32_t); 176 | 177 | while (arraysize--) 178 | { 179 | DBG_MSG("%c%c%c%c,", PRINTF_4CC(*L)); 180 | L++; 181 | } 182 | if (repeat) DBG_MSG(" "); 183 | } 184 | } 185 | break; 186 | case GPMF_TYPE_GUID: // display GUID in this formatting ABCDEF01-02030405-06070809-10111213 187 | { 188 | uint8_t *B = (uint8_t *)data; 189 | uint32_t arraysize = structsize; 190 | LIMITOUTPUT; 191 | while (repeat--) 192 | { 193 | arraysize = structsize; 194 | 195 | while (arraysize--) 196 | { 197 | DBG_MSG("%02X", *B); 198 | B++; 199 | } 200 | if (repeat) DBG_MSG(" "); 201 | } 202 | } 203 | break; 204 | case GPMF_TYPE_SIGNED_SHORT: 205 | { 206 | int16_t *s = (int16_t *)data; 207 | uint32_t arraysize = structsize / sizeof(int16_t); 208 | LIMITOUTPUT; 209 | 210 | while (repeat--) 211 | { 212 | arraysize = structsize / sizeof(int16_t); 213 | 214 | while (arraysize--) 215 | { 216 | DBG_MSG("%d,", (int16_t)BYTESWAP16(*s)); 217 | s++; 218 | } 219 | if (repeat) DBG_MSG(" "); 220 | } 221 | } 222 | break; 223 | case GPMF_TYPE_UNSIGNED_SHORT: 224 | { 225 | uint16_t *S = (uint16_t *)data; 226 | uint32_t arraysize = structsize / sizeof(uint16_t); 227 | LIMITOUTPUT; 228 | 229 | while (repeat--) 230 | { 231 | arraysize = structsize / sizeof(uint16_t); 232 | 233 | while (arraysize--) 234 | { 235 | DBG_MSG("%d,", BYTESWAP16(*S)); 236 | S++; 237 | } 238 | if (repeat) DBG_MSG(" "); 239 | } 240 | } 241 | break; 242 | case GPMF_TYPE_SIGNED_LONG: 243 | { 244 | int32_t *l = (int32_t *)data; 245 | uint32_t arraysize = structsize / sizeof(uint32_t); 246 | LIMITOUTPUT; 247 | 248 | while (repeat--) 249 | { 250 | arraysize = structsize / sizeof(uint32_t); 251 | 252 | while (arraysize--) 253 | { 254 | DBG_MSG("%d,", (int32_t)BYTESWAP32(*l)); 255 | l++; 256 | } 257 | if (repeat) DBG_MSG(" "); 258 | } 259 | } 260 | break; 261 | case GPMF_TYPE_UNSIGNED_LONG: 262 | { 263 | uint32_t *L = (uint32_t *)data; 264 | uint32_t arraysize = structsize / sizeof(uint32_t); 265 | LIMITOUTPUT; 266 | while (repeat--) 267 | { 268 | arraysize = structsize / sizeof(uint32_t); 269 | 270 | while (arraysize--) 271 | { 272 | DBG_MSG("%d,", BYTESWAP32(*L)); 273 | L++; 274 | } 275 | if(repeat) DBG_MSG(" "); 276 | } 277 | } 278 | break; 279 | case GPMF_TYPE_Q15_16_FIXED_POINT: 280 | { 281 | int32_t *q = (int32_t *)data; 282 | uint32_t arraysize = structsize / sizeof(int32_t); 283 | LIMITOUTPUT; 284 | 285 | while (repeat--) 286 | { 287 | arraysize = structsize / sizeof(int32_t); 288 | 289 | while (arraysize--) 290 | { 291 | double dq = BYTESWAP32(*q); 292 | dq /= (double)65536.0; 293 | DBG_MSG("%.3f,", dq); 294 | q++; 295 | } 296 | if (repeat) DBG_MSG(" "); 297 | } 298 | } 299 | break; 300 | case GPMF_TYPE_Q31_32_FIXED_POINT: 301 | { 302 | int64_t *Q = (int64_t *)data; 303 | uint32_t arraysize = structsize / sizeof(int64_t); 304 | LIMITOUTPUT; 305 | 306 | while (repeat--) 307 | { 308 | arraysize = structsize / sizeof(int64_t); 309 | 310 | while (arraysize--) 311 | { 312 | uint64_t Q64 = (uint64_t)BYTESWAP64(*Q); 313 | double dq = (double)(Q64 >> (uint64_t)32); 314 | dq += (double)(Q64 & (uint64_t)0xffffffff) / (double)0x100000000; 315 | DBG_MSG("%.3f,", dq); 316 | Q++; 317 | } 318 | if (repeat) DBG_MSG(" "); 319 | } 320 | } 321 | break; 322 | case GPMF_TYPE_UTC_DATE_TIME: 323 | { 324 | char *U = (char *)data; 325 | uint32_t arraysize = structsize / 16; 326 | LIMITOUTPUT; 327 | while (repeat--) 328 | { 329 | arraysize = structsize / 16; 330 | char t[17]; 331 | t[16] = 0; 332 | 333 | while (arraysize--) 334 | { 335 | #ifdef _WINDOWS 336 | strncpy_s(t, 17, U, 16); 337 | #else 338 | strncpy(t, U, 16); 339 | #endif 340 | DBG_MSG("\"%s\",", t); 341 | U += 16; 342 | } 343 | if (repeat) DBG_MSG(" "); 344 | } 345 | } 346 | break; 347 | case GPMF_TYPE_SIGNED_64BIT_INT: 348 | { 349 | int64_t *J = (int64_t *)data; 350 | uint32_t arraysize = structsize / sizeof(int64_t); 351 | LIMITOUTPUT; 352 | while (repeat--) 353 | { 354 | arraysize = structsize / sizeof(int64_t); 355 | 356 | while (arraysize--) 357 | { 358 | DBG_MSG("%lld,", (long long int)BYTESWAP64(*J)); 359 | J++; 360 | } 361 | if (repeat) DBG_MSG(" "); 362 | } 363 | } 364 | break; 365 | case GPMF_TYPE_UNSIGNED_64BIT_INT: 366 | { 367 | uint64_t *J = (uint64_t *)data; 368 | uint32_t arraysize = structsize / sizeof(uint64_t); 369 | LIMITOUTPUT; 370 | while (repeat--) 371 | { 372 | arraysize = structsize / sizeof(uint64_t); 373 | 374 | while (arraysize--) 375 | { 376 | DBG_MSG("%llu,", (long long unsigned int)BYTESWAP64(*J)); 377 | J++; 378 | } 379 | if (repeat) DBG_MSG(" "); 380 | } 381 | } 382 | break; 383 | default: 384 | break; 385 | } 386 | 387 | if (dots) // more data was not output 388 | DBG_MSG("..."); 389 | } 390 | 391 | 392 | void PrintGPMF(GPMF_stream *ms) 393 | { 394 | if (ms) 395 | { 396 | uint32_t key = GPMF_Key(ms); 397 | uint32_t type = GPMF_Type(ms); 398 | uint32_t structsize = GPMF_StructSize(ms); 399 | uint32_t repeat = GPMF_Repeat(ms); 400 | uint32_t size = GPMF_RawDataSize(ms); 401 | uint32_t indent, level = GPMF_NestLevel(ms); 402 | void *data = GPMF_RawData(ms); 403 | 404 | if (key != GPMF_KEY_DEVICE) level++; 405 | 406 | indent = level; 407 | while (indent > 0 && indent < 10) 408 | { 409 | DBG_MSG(" "); 410 | indent--; 411 | } 412 | if (type == 0) 413 | DBG_MSG("%c%c%c%c nest size %d ", (key >> 0) & 0xff, (key >> 8) & 0xff, (key >> 16) & 0xff, (key >> 24) & 0xff, size); 414 | else if (structsize == 1 || (repeat == 1 && type != '?')) 415 | DBG_MSG("%c%c%c%c type '%c' size %d ", (key >> 0) & 0xff, (key >> 8) & 0xff, (key >> 16) & 0xff, (key >> 24) & 0xff, type == 0 ? '0' : type, size); 416 | else 417 | DBG_MSG("%c%c%c%c type '%c' samplesize %d repeat %d ", (key >> 0) & 0xff, (key >> 8) & 0xff, (key >> 16) & 0xff, (key >> 24) & 0xff, type == 0 ? '0' : type, structsize, repeat); 418 | 419 | if (type && repeat > 0) 420 | { 421 | DBG_MSG("data: "); 422 | 423 | if (type == GPMF_TYPE_COMPLEX) 424 | { 425 | GPMF_stream find_stream; 426 | GPMF_CopyState(ms, &find_stream); 427 | if (GPMF_OK == GPMF_FindPrev(&find_stream, GPMF_KEY_TYPE, GPMF_CURRENT_LEVEL)) 428 | { 429 | char *srctype = (char *)GPMF_RawData(&find_stream); 430 | uint32_t typelen = GPMF_RawDataSize(&find_stream); 431 | int struct_size_of_type; 432 | 433 | struct_size_of_type = GPMF_SizeOfComplexTYPE(srctype, typelen); 434 | if (struct_size_of_type != (int32_t)structsize) 435 | { 436 | DBG_MSG("error: found structure of %d bytes reported as %d bytes", struct_size_of_type, structsize); 437 | } 438 | else 439 | { 440 | char typearray[64]; 441 | uint32_t elements = sizeof(typearray); 442 | uint8_t *bdata = (uint8_t *)data; 443 | uint32_t i; 444 | 445 | if (GPMF_OK == GPMF_ExpandComplexTYPE(srctype, typelen, typearray, &elements)) 446 | { 447 | int dots = 0; 448 | uint32_t arraysize = elements; 449 | 450 | LIMITOUTPUT; 451 | 452 | uint32_t j; 453 | for (j = 0; j < repeat; j++) 454 | { 455 | for (i = 0; i < elements; i++) 456 | { 457 | int elementsize = (int)GPMF_SizeofType((GPMF_SampleType)typearray[i]); 458 | printfData(typearray[i], elementsize, 1, bdata); 459 | bdata += elementsize; 460 | } 461 | if (j < repeat-1) DBG_MSG(" "); 462 | 463 | } 464 | if (dots) 465 | DBG_MSG("..."); 466 | } 467 | } 468 | } 469 | else 470 | { 471 | DBG_MSG("unknown formatting"); 472 | } 473 | } 474 | else 475 | { 476 | printfData(type, structsize, repeat, data); 477 | } 478 | } 479 | 480 | DBG_MSG("\n"); 481 | } 482 | } -------------------------------------------------------------------------------- /src/ImuExtractor.cpp: -------------------------------------------------------------------------------- 1 | /*! @file GPMF_demo.c 2 | * 3 | * @brief Demo to extract GPMF from an MP4 4 | * 5 | * @version 2.0.0 6 | * 7 | * (C) Copyright 2017-2020 GoPro Inc (http://gopro.com/). 8 | * 9 | * Licensed under either: 10 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 11 | * - MIT license, http://opensource.org/licenses/MIT 12 | * at your option. 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | * 20 | */ 21 | 22 | #include "ImuExtractor.h" 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | extern void PrintGPMF(GPMF_stream* ms); 34 | 35 | using namespace std; 36 | 37 | GoProImuExtractor::GoProImuExtractor(const std::string file) { 38 | video = const_cast(file.c_str()); 39 | 40 | ms = &metadata_stream; 41 | mp4 = OpenMP4Source(video, MOV_GPMF_TRAK_TYPE, MOV_GPMF_TRAK_SUBTYPE, 0); 42 | if (mp4 == 0) { 43 | std::cout << RED << "Could not open video file" << RESET << std::endl; 44 | } 45 | metadatalength = GetDuration(mp4); 46 | 47 | if (metadatalength > 0.0) { 48 | payloads = GetNumberPayloads(mp4); 49 | // payloads = payloads - 1; // Discarding the last payload. Found that payload is not reliable 50 | // as others 51 | } 52 | uint32_t fr_num, fr_dem; 53 | frame_count = GetVideoFrameRateAndCount(mp4, &fr_num, &fr_dem); 54 | frame_rate = (float)fr_num / (float)fr_dem; 55 | 56 | movie_creation_time = (uint64_t)((getCreationtime(mp4) - get_offset_1904()) * 1000000000); 57 | } 58 | 59 | bool GoProImuExtractor::display_video_framerate() { 60 | if (frame_count) { 61 | printf("VIDEO FRAMERATE:\n %.3f with %d frames\n", frame_rate, frame_count); 62 | return true; 63 | } else { 64 | return false; 65 | } 66 | } 67 | 68 | GoProImuExtractor::~GoProImuExtractor() { 69 | if (payloadres) FreePayloadResource(mp4, payloadres); 70 | if (ms) GPMF_Free(ms); 71 | 72 | payload = NULL; 73 | CloseSource(mp4); 74 | } 75 | 76 | void GoProImuExtractor::cleanup() { 77 | if (payloadres) FreePayloadResource(mp4, payloadres); 78 | if (ms) GPMF_Free(ms); 79 | 80 | payload = NULL; 81 | CloseSource(mp4); 82 | } 83 | 84 | void GoProImuExtractor::show_gpmf_structure() { 85 | uint32_t payload_size; 86 | GPMF_ERR ret = GPMF_OK; 87 | 88 | // Just print the structure of first payload 89 | // Remaining structure should also be similar 90 | uint32_t index = 0; 91 | double in = 0.0, out = 0.0; // times 92 | 93 | payload_size = GetPayloadSize(mp4, index); 94 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 95 | payload = GetPayload(mp4, payloadres, index); 96 | 97 | if (payload == NULL) cleanup(); 98 | 99 | ret = GetPayloadTime(mp4, index, &in, &out); 100 | if (ret != GPMF_OK) cleanup(); 101 | 102 | ret = GPMF_Init(ms, payload, payload_size); 103 | if (ret != GPMF_OK) cleanup(); 104 | 105 | printf("PAYLOAD TIME:\n %.3f to %.3f seconds\n", in, out); 106 | printf("GPMF STRUCTURE:\n"); 107 | // Output (printf) all the contained GPMF data within this payload 108 | ret = GPMF_Validate(ms, GPMF_RECURSE_LEVELS); // optional 109 | if (GPMF_OK != ret) { 110 | if (GPMF_ERROR_UNKNOWN_TYPE == ret) { 111 | printf("Unknown GPMF Type within, ignoring\n"); 112 | ret = GPMF_OK; 113 | } else 114 | printf("Invalid GPMF Structure\n"); 115 | } 116 | 117 | GPMF_ResetState(ms); 118 | 119 | GPMF_ERR nextret; 120 | do { 121 | printf(" "); 122 | PrintGPMF(ms); // printf current GPMF KLV 123 | 124 | nextret = GPMF_Next(ms, GPMF_RECURSE_LEVELS); 125 | 126 | while ( 127 | nextret == 128 | GPMF_ERROR_UNKNOWN_TYPE) // or just using GPMF_Next(ms, GPMF_RECURSE_LEVELS|GPMF_TOLERANT) 129 | // to ignore and skip unknown types 130 | nextret = GPMF_Next(ms, GPMF_RECURSE_LEVELS); 131 | 132 | } while (GPMF_OK == nextret); 133 | GPMF_ResetState(ms); 134 | } 135 | 136 | /** Only supports native formats like ACCL, GYRO, GPS 137 | * 138 | * @param fourcc 139 | * @param readings 140 | * @param timestamp 141 | * @return 142 | */ 143 | 144 | GPMF_ERR GoProImuExtractor::get_scaled_data(uint32_t fourcc, vector>& readings) { 145 | while (GPMF_OK == 146 | GPMF_FindNext(ms, 147 | STR2FOURCC("STRM"), 148 | static_cast(GPMF_RECURSE_LEVELS | 149 | GPMF_TOLERANT))) // GoPro Hero5/6/7 Accelerometer) 150 | { 151 | if (GPMF_OK != 152 | GPMF_FindNext(ms, fourcc, static_cast(GPMF_RECURSE_LEVELS | GPMF_TOLERANT))) 153 | continue; 154 | 155 | uint32_t samples = GPMF_Repeat(ms); 156 | uint32_t elements = GPMF_ElementsInStruct(ms); 157 | uint32_t buffersize = samples * elements * sizeof(double); 158 | double *ptr, *tmpbuffer = (double*)malloc(buffersize); 159 | 160 | readings.resize(samples); 161 | 162 | if (tmpbuffer && samples) { 163 | uint32_t i, j; 164 | 165 | // GPMF_FormattedData(ms, tmpbuffer, buffersize, 0, samples); // Output data in LittleEnd, but 166 | // no scale 167 | if (GPMF_OK == GPMF_ScaledData(ms, 168 | tmpbuffer, 169 | buffersize, 170 | 0, 171 | samples, 172 | GPMF_TYPE_DOUBLE)) // Output scaled data as floats 173 | { 174 | ptr = tmpbuffer; 175 | for (i = 0; i < samples; i++) { 176 | vector sample(elements); 177 | for (j = 0; j < elements; j++) { 178 | sample.at(j) = *ptr++; 179 | } 180 | readings.at(i) = sample; 181 | } 182 | } 183 | free(tmpbuffer); 184 | } 185 | } 186 | 187 | GPMF_ResetState(ms); 188 | return GPMF_OK; 189 | } 190 | 191 | /** returns STMP (start time of current payload) 192 | * 193 | * @param fourcc 194 | * @return timestmamp 195 | */ 196 | 197 | uint64_t GoProImuExtractor::get_stamp(uint32_t fourcc) { 198 | GPMF_stream find_stream; 199 | 200 | uint64_t timestamp; 201 | while (GPMF_OK == 202 | GPMF_FindNext(ms, 203 | STR2FOURCC("STRM"), 204 | static_cast(GPMF_RECURSE_LEVELS | 205 | GPMF_TOLERANT))) // GoPro Hero5/6/7 Accelerometer) 206 | { 207 | if (GPMF_OK != 208 | GPMF_FindNext(ms, fourcc, static_cast(GPMF_RECURSE_LEVELS | GPMF_TOLERANT))) 209 | continue; 210 | 211 | GPMF_CopyState(ms, &find_stream); 212 | if (GPMF_OK == GPMF_FindPrev(&find_stream, 213 | GPMF_KEY_TIME_STAMP, 214 | static_cast(GPMF_CURRENT_LEVEL | GPMF_TOLERANT))) 215 | timestamp = BYTESWAP64(*(uint64_t*)GPMF_RawData(&find_stream)); 216 | } 217 | GPMF_ResetState(ms); 218 | return timestamp; 219 | } 220 | 221 | GPMF_ERR GoProImuExtractor::show_current_payload(uint32_t index) { 222 | uint32_t payload_size; 223 | GPMF_ERR ret; 224 | 225 | payload_size = GetPayloadSize(mp4, index); 226 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 227 | payload = GetPayload(mp4, payloadres, index); 228 | 229 | if (payload == NULL) cleanup(); 230 | ret = GPMF_Init(ms, payload, payload_size); 231 | if (ret != GPMF_OK) cleanup(); 232 | 233 | GPMF_ERR nextret; 234 | do { 235 | printf(" "); 236 | PrintGPMF(ms); // printf current GPMF KLV 237 | 238 | nextret = GPMF_Next(ms, GPMF_RECURSE_LEVELS); 239 | 240 | while ( 241 | nextret == 242 | GPMF_ERROR_UNKNOWN_TYPE) // or just using GPMF_Next(ms, GPMF_RECURSE_LEVELS|GPMF_TOLERANT) 243 | // to ignore and skip unknown types 244 | nextret = GPMF_Next(ms, GPMF_RECURSE_LEVELS); 245 | 246 | } while (GPMF_OK == nextret); 247 | GPMF_ResetState(ms); 248 | } 249 | 250 | void GoProImuExtractor::getImageStamps(vector& image_stamps, 251 | uint64_t image_end_stamp, 252 | bool offset_only) { 253 | vector> cam_orient_data; 254 | 255 | uint64_t current_stamp, prev_stamp; 256 | 257 | uint64_t total_samples = 0; 258 | uint64_t seq = 0; 259 | 260 | for (uint32_t index = 0; index < payloads; index++) { 261 | GPMF_ERR ret; 262 | uint32_t payload_size; 263 | 264 | payload_size = GetPayloadSize(mp4, index); 265 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 266 | payload = GetPayload(mp4, payloadres, index); 267 | 268 | if (payload == NULL) cleanup(); 269 | ret = GPMF_Init(ms, payload, payload_size); 270 | if (ret != GPMF_OK) cleanup(); 271 | 272 | current_stamp = get_stamp(STR2FOURCC("CORI")); 273 | 274 | current_stamp = current_stamp * 1000; // us to ns 275 | 276 | if (index > 0) { 277 | uint64_t time_span = current_stamp - prev_stamp; 278 | 279 | if (time_span < 0) { 280 | cout << RED << "previous timestamp should be smaller than current stamp" << RESET << endl; 281 | exit(1); 282 | } 283 | 284 | double step_size = (double)time_span / (double)cam_orient_data.size(); 285 | 286 | for (uint32_t i = 0; i < cam_orient_data.size(); i++) { 287 | uint64_t stamp = prev_stamp + (uint64_t)((double)i * step_size); 288 | if (!offset_only) stamp = movie_creation_time + stamp; 289 | image_stamps.push_back(stamp); 290 | } 291 | total_samples += cam_orient_data.size(); 292 | } 293 | 294 | cam_orient_data.clear(); 295 | get_scaled_data(STR2FOURCC("CORI"), cam_orient_data); 296 | 297 | prev_stamp = current_stamp; 298 | 299 | GPMF_Free(ms); 300 | } 301 | 302 | if (image_end_stamp > 0) { 303 | // ROS_WARN_STREAM("Writing last payload"); 304 | uint64_t time_span = image_end_stamp * 1000 - current_stamp; 305 | double step_size = (double)time_span / (double)cam_orient_data.size(); 306 | 307 | for (uint32_t i = 0; i < cam_orient_data.size(); i++) { 308 | uint64_t stamp = prev_stamp + (uint64_t)((double)i * step_size); 309 | if (!offset_only) stamp = movie_creation_time + stamp; 310 | image_stamps.push_back(stamp); 311 | } 312 | } 313 | } 314 | 315 | int GoProImuExtractor::save_imu_stream(const std::string imu_file, uint64_t end_time) { 316 | ofstream imu_stream; 317 | imu_stream.open(imu_file); 318 | imu_stream << std::fixed << std::setprecision(19); 319 | 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]," 320 | "a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]" 321 | << endl; 322 | 323 | uint64_t first_frame_us, first_frame_ns; 324 | vector> accl_data; 325 | vector> gyro_data; 326 | 327 | uint64_t current_stamp, prev_stamp; 328 | 329 | vector steps; 330 | uint64_t total_samples = 0; 331 | 332 | for (uint32_t index = 0; index < payloads; index++) { 333 | GPMF_ERR ret; 334 | uint32_t payload_size; 335 | 336 | payload_size = GetPayloadSize(mp4, index); 337 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 338 | payload = GetPayload(mp4, payloadres, index); 339 | 340 | if (payload == NULL) cleanup(); 341 | ret = GPMF_Init(ms, payload, payload_size); 342 | if (ret != GPMF_OK) cleanup(); 343 | 344 | if (index == 0) { 345 | first_frame_us = get_stamp(STR2FOURCC("CORI")); 346 | first_frame_ns = first_frame_us * 1000; 347 | // cout << "Image Initial Stamp: " << first_frame_ns << endl; 348 | } 349 | 350 | current_stamp = get_stamp(STR2FOURCC("ACCL")); 351 | uint64_t current_gyro_stamp = get_stamp(STR2FOURCC("GYRO")); 352 | 353 | if (current_gyro_stamp != current_stamp) { 354 | int32_t diff = current_gyro_stamp - current_stamp; 355 | if (abs(diff) > 20) { 356 | cout << RED 357 | << "[ERROR] ACCL and GYRO timestamp heavily un-synchronized ....Shutting Down!!!" 358 | << RESET << endl; 359 | std::cout << RED << "Index: " << index << " accl stamp: " << current_stamp 360 | << " gyro stamp: " << current_gyro_stamp << RESET << endl; 361 | exit(1); 362 | } else { 363 | cout << YELLOW << "[WARN] ACCL and GYRO timestamp slightly not synchronized ...." << RESET 364 | << endl; 365 | std::cout << YELLOW << "Index: " << index << " accl stamp: " << current_stamp 366 | << " gyro stamp: " << current_gyro_stamp << RESET << endl; 367 | } 368 | 369 | // show_current_payload(index); 370 | } 371 | 372 | current_stamp = current_stamp * 1000; // us to ns 373 | if (index > 0) { 374 | uint64_t time_span = current_stamp - prev_stamp; 375 | if (time_span < 0) { 376 | cout << RED << "previous timestamp should be smaller than current stamp" << RESET << endl; 377 | exit(1); 378 | } 379 | 380 | uint64_t step_size = time_span / accl_data.size(); 381 | steps.emplace_back(step_size); 382 | 383 | if (accl_data.size() != gyro_data.size()) { 384 | cout << RED << "ACCL and GYRO data must be of same size" << RESET << endl; 385 | exit(1); 386 | } 387 | 388 | for (int i = 0; i < gyro_data.size(); ++i) { 389 | uint64_t s = prev_stamp + i * step_size; 390 | uint64_t ros_stamp = movie_creation_time + s - first_frame_ns; 391 | imu_stream << uint64_to_string(ros_stamp); 392 | 393 | vector gyro_sample = gyro_data.at(i); 394 | 395 | // Data comes in ZXY order 396 | imu_stream << "," << gyro_sample.at(1); 397 | imu_stream << "," << gyro_sample.at(2); 398 | imu_stream << "," << gyro_sample.at(0); 399 | 400 | vector accl_sample = accl_data.at(i); 401 | imu_stream << "," << accl_sample.at(1); 402 | imu_stream << "," << accl_sample.at(2); 403 | imu_stream << "," << accl_sample.at(0) << endl; 404 | 405 | // Letting one extra imu stamp 406 | if ((ros_stamp - movie_creation_time) > end_time) break; 407 | } 408 | } 409 | 410 | gyro_data.clear(); 411 | accl_data.clear(); 412 | get_scaled_data(STR2FOURCC("ACCL"), accl_data); 413 | get_scaled_data(STR2FOURCC("GYRO"), gyro_data); 414 | 415 | total_samples += gyro_data.size(); 416 | // double start_stamp_secs = ((double) current_stamp)*1e-9; 417 | // std::cout << "Payload Start Stamp: " << start_stamp_secs << "\tSamples: " << 418 | // accl_data.size() << endl; 419 | prev_stamp = current_stamp; 420 | 421 | GPMF_Free(ms); 422 | } 423 | 424 | std::cout << GREEN << "Wrote " << total_samples << " imu samples to file" << RESET << endl; 425 | imu_stream.close(); 426 | // CloseSource(mp4); 427 | } 428 | 429 | uint32_t GoProImuExtractor::getNumofSamples(uint32_t fourcc) { 430 | GPMF_stream find_stream; 431 | 432 | uint32_t total_samples; 433 | while (GPMF_OK == 434 | GPMF_FindNext(ms, 435 | STR2FOURCC("STRM"), 436 | static_cast(GPMF_RECURSE_LEVELS | 437 | GPMF_TOLERANT))) // GoPro Hero5/6/7 Accelerometer) 438 | { 439 | if (GPMF_OK != 440 | GPMF_FindNext(ms, fourcc, static_cast(GPMF_RECURSE_LEVELS | GPMF_TOLERANT))) 441 | continue; 442 | 443 | GPMF_CopyState(ms, &find_stream); 444 | if (GPMF_OK == GPMF_FindPrev(&find_stream, 445 | GPMF_KEY_TOTAL_SAMPLES, 446 | static_cast(GPMF_CURRENT_LEVEL | GPMF_TOLERANT))) 447 | total_samples = BYTESWAP32(*(uint32_t*)GPMF_RawData(&find_stream)); 448 | } 449 | GPMF_ResetState(ms); 450 | return total_samples; 451 | } 452 | 453 | void GoProImuExtractor::getPayloadStamps(uint32_t fourcc, 454 | std::vector& start_stamps, 455 | std::vector& samples) { 456 | start_stamps.clear(); 457 | samples.clear(); 458 | 459 | for (uint32_t index = 0; index < payloads; index++) { 460 | GPMF_ERR ret; 461 | uint32_t payload_size; 462 | 463 | payload_size = GetPayloadSize(mp4, index); 464 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 465 | payload = GetPayload(mp4, payloadres, index); 466 | 467 | if (payload == NULL) cleanup(); 468 | ret = GPMF_Init(ms, payload, payload_size); 469 | if (ret != GPMF_OK) cleanup(); 470 | 471 | uint64_t stamp = get_stamp(fourcc); 472 | uint32_t total_samples = getNumofSamples(fourcc); 473 | 474 | // cout << RED << "Stamp: " << stamp << "\tSamples: " << total_samples << RESET << endl; 475 | start_stamps.push_back(stamp); 476 | samples.push_back(total_samples); 477 | } 478 | } 479 | 480 | void GoProImuExtractor::skipPayloads(uint32_t num_payloads) { payloads_skipped = num_payloads; } 481 | 482 | void GoProImuExtractor::writeImuData(rosbag::Bag& bag, 483 | uint64_t end_time, 484 | const std::string& imu_topic) { 485 | uint64_t first_frame_us, first_frame_ns; 486 | vector> accl_data; 487 | vector> gyro_data; 488 | 489 | uint64_t current_stamp, prev_stamp; 490 | 491 | vector steps; 492 | uint64_t total_samples = 0; 493 | uint64_t seq = 0; 494 | 495 | for (uint32_t index = 0; index < payloads; index++) { 496 | GPMF_ERR ret; 497 | uint32_t payload_size; 498 | 499 | payload_size = GetPayloadSize(mp4, index); 500 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 501 | payload = GetPayload(mp4, payloadres, index); 502 | 503 | if (payload == NULL) cleanup(); 504 | ret = GPMF_Init(ms, payload, payload_size); 505 | if (ret != GPMF_OK) cleanup(); 506 | 507 | if (index == 0) { 508 | first_frame_us = get_stamp(STR2FOURCC("CORI")); 509 | first_frame_ns = first_frame_us * 1000; 510 | // cout << "Image Initial Stamp: " << first_frame_ns << endl; 511 | } 512 | 513 | current_stamp = get_stamp(STR2FOURCC("ACCL")); 514 | uint64_t current_gyro_stamp = get_stamp(STR2FOURCC("GYRO")); 515 | 516 | if (current_gyro_stamp != current_stamp) { 517 | int32_t diff = current_gyro_stamp - current_stamp; 518 | if (abs(diff) > 100) { 519 | cout << RED 520 | << "[ERROR] ACCL and GYRO timestamp heavily un-synchronized ....Shutting Down!!!" 521 | << RESET << endl; 522 | std::cout << RED << "Index: " << index << " accl stamp: " << current_stamp 523 | << " gyro stamp: " << current_gyro_stamp << RESET << endl; 524 | exit(1); 525 | } else { 526 | cout << YELLOW << "[WARN] ACCL and GYRO timestamp slightly not synchronized ...." << RESET 527 | << endl; 528 | std::cout << YELLOW << "Index: " << index << " accl stamp: " << current_stamp 529 | << " gyro stamp: " << current_gyro_stamp << RESET << endl; 530 | } 531 | 532 | // show_current_payload(index); 533 | } 534 | 535 | current_stamp = current_stamp * 1000; // us to ns 536 | if (index > 0) { 537 | uint64_t time_span = current_stamp - prev_stamp; 538 | if (time_span < 0) { 539 | cout << RED << "previous timestamp should be smaller than current stamp" << RESET << endl; 540 | exit(1); 541 | } 542 | 543 | uint64_t step_size = time_span / accl_data.size(); 544 | steps.emplace_back(step_size); 545 | 546 | if (accl_data.size() != gyro_data.size()) { 547 | cout << RED << "ACCL and GYRO data must be of same size" << RESET << endl; 548 | exit(1); 549 | } 550 | 551 | for (int i = 0; i < gyro_data.size(); ++i) { 552 | uint64_t s = prev_stamp + i * step_size; 553 | uint64_t current_stamp = movie_creation_time + s - first_frame_ns; 554 | 555 | uint32_t secs = current_stamp * 1e-9; 556 | uint32_t n_secs = current_stamp % 1000000000; 557 | ros::Time ros_time(secs, n_secs); 558 | 559 | std_msgs::Header header; 560 | header.stamp = ros_time; 561 | header.frame_id = "gopro"; 562 | header.seq = seq++; 563 | 564 | vector gyro_sample = gyro_data.at(i); 565 | vector accl_sample = accl_data.at(i); 566 | 567 | sensor_msgs::Imu imu_msg; 568 | imu_msg.header = header; 569 | // Data comes in ZXY order 570 | imu_msg.angular_velocity.x = gyro_sample.at(1); 571 | imu_msg.angular_velocity.y = gyro_sample.at(2); 572 | imu_msg.angular_velocity.z = gyro_sample.at(0); 573 | imu_msg.linear_acceleration.x = accl_sample.at(1); 574 | imu_msg.linear_acceleration.y = accl_sample.at(2); 575 | imu_msg.linear_acceleration.z = accl_sample.at(0); 576 | 577 | bag.write(imu_topic, ros_time, imu_msg); // Write to bag 578 | 579 | // Letting one extra imu stamp 580 | if ((current_stamp - movie_creation_time) > end_time) break; 581 | } 582 | } 583 | 584 | gyro_data.clear(); 585 | accl_data.clear(); 586 | get_scaled_data(STR2FOURCC("ACCL"), accl_data); 587 | get_scaled_data(STR2FOURCC("GYRO"), gyro_data); 588 | 589 | total_samples += gyro_data.size(); 590 | // double start_stamp_secs = ((double) current_stamp)*1e-9; 591 | // std::cout << "Payload Start Stamp: " << start_stamp_secs << "\tSamples: " << 592 | // accl_data.size() << endl; 593 | prev_stamp = current_stamp; 594 | 595 | GPMF_Free(ms); 596 | } 597 | 598 | std::cout << GREEN << "Wrote " << total_samples << " imu samples to file" << RESET << endl; 599 | } 600 | 601 | uint64_t GoProImuExtractor::getPayloadStartStamp(uint32_t fourcc, uint32_t index) { 602 | GPMF_ERR ret; 603 | uint32_t payload_size; 604 | 605 | payload_size = GetPayloadSize(mp4, index); 606 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 607 | payload = GetPayload(mp4, payloadres, index); 608 | 609 | if (payload == NULL) cleanup(); 610 | ret = GPMF_Init(ms, payload, payload_size); 611 | if (ret != GPMF_OK) cleanup(); 612 | 613 | uint64_t stamp = get_stamp(fourcc); 614 | return stamp; 615 | } 616 | 617 | void GoProImuExtractor::readImuData(std::deque& accl_queue, 618 | std::deque& gyro_queue, 619 | uint64_t accl_end_time, 620 | uint64_t gyro_end_time) { 621 | vector> accl_data; 622 | vector> gyro_data; 623 | 624 | uint64_t current_accl_stamp, prev_accl_stamp; 625 | uint64_t current_gyro_stamp, prev_gyro_stamp; 626 | 627 | uint64_t total_samples = 0; 628 | uint64_t seq = 0; 629 | 630 | for (uint32_t index = 0; index < payloads; index++) { 631 | GPMF_ERR ret; 632 | uint32_t payload_size; 633 | 634 | payload_size = GetPayloadSize(mp4, index); 635 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 636 | payload = GetPayload(mp4, payloadres, index); 637 | 638 | if (payload == NULL) cleanup(); 639 | ret = GPMF_Init(ms, payload, payload_size); 640 | if (ret != GPMF_OK) cleanup(); 641 | 642 | current_accl_stamp = get_stamp(STR2FOURCC("ACCL")); 643 | current_gyro_stamp = get_stamp(STR2FOURCC("GYRO")); 644 | 645 | if (current_gyro_stamp != current_accl_stamp) { 646 | int32_t diff = current_gyro_stamp - current_accl_stamp; 647 | if (abs(diff) > 100) { 648 | ROS_WARN_STREAM("ACCL and GYRO timestamp slightly un-synchronized ...."); 649 | ROS_WARN_STREAM("Index: " << index << " accl stamp: " << current_accl_stamp 650 | << " gyro stamp: " << current_gyro_stamp << " diff: " << diff); 651 | } 652 | } 653 | 654 | current_accl_stamp = current_accl_stamp * 1000; // us to ns 655 | current_gyro_stamp = current_gyro_stamp * 1000; // us to ns 656 | 657 | if (index > 0) { 658 | uint64_t accl_time_span = current_accl_stamp - prev_accl_stamp; 659 | uint64_t gyro_time_span = current_gyro_stamp - prev_gyro_stamp; 660 | 661 | if (accl_time_span < 0 || gyro_time_span < 0) { 662 | cout << RED << "previous timestamp should be smaller than current stamp" << RESET << endl; 663 | exit(1); 664 | } 665 | 666 | double accl_step_size = (double)accl_time_span / (double)accl_data.size(); 667 | double gyro_step_size = (double)gyro_time_span / (double)gyro_data.size(); 668 | 669 | if (accl_data.size() != gyro_data.size()) { 670 | cout << RED << "ACCL and GYRO data are not of same size" << RESET << endl; 671 | } 672 | 673 | for (uint32_t i = 0; i < accl_data.size(); i++) { 674 | uint64_t accl_time = prev_accl_stamp + (uint64_t)((double)i * accl_step_size); 675 | uint64_t accl_stamp = movie_creation_time + accl_time; 676 | vector accl_sample = accl_data.at(i); 677 | 678 | // Data comes in ZXY order 679 | ImuAccl accl; 680 | accl << accl_sample.at(1), accl_sample.at(2), accl_sample.at(0); 681 | accl_queue.push_back(AcclMeasurement(accl_stamp, accl)); 682 | } 683 | 684 | for (uint32_t i = 0; i < gyro_data.size(); ++i) { 685 | uint64_t gyro_time = prev_gyro_stamp + (uint64_t)((double)i * gyro_step_size); 686 | uint64_t gyro_stamp = movie_creation_time + gyro_time; 687 | vector gyro_sample = gyro_data.at(i); 688 | 689 | // Data comes in ZXY order 690 | ImuGyro gyro; 691 | gyro << gyro_sample.at(1), gyro_sample.at(2), gyro_sample.at(0); 692 | gyro_queue.push_back(GyroMeasurement(gyro_stamp, gyro)); 693 | 694 | total_samples += 1; 695 | } 696 | } 697 | 698 | gyro_data.clear(); 699 | accl_data.clear(); 700 | get_scaled_data(STR2FOURCC("ACCL"), accl_data); 701 | get_scaled_data(STR2FOURCC("GYRO"), gyro_data); 702 | 703 | prev_accl_stamp = current_accl_stamp; 704 | prev_gyro_stamp = current_gyro_stamp; 705 | 706 | GPMF_Free(ms); 707 | } 708 | 709 | // If this is not the last video file, extract the last payload 710 | // For the last video file, the last payload is not used 711 | // Since we do not have the end time of the last payload 712 | 713 | if (accl_end_time > 0 && gyro_end_time > 0) { 714 | // ROS_WARN_STREAM("Writing last payload"); 715 | uint64_t accl_time_span = accl_end_time * 1000 - current_accl_stamp; 716 | double accl_step_size = (double)accl_time_span / (double)accl_data.size(); 717 | 718 | uint64_t gyro_time_span = gyro_end_time * 1000 - current_gyro_stamp; 719 | double gyro_step_size = (double)gyro_time_span / (double)gyro_data.size(); 720 | 721 | if (accl_data.size() != gyro_data.size()) { 722 | ROS_WARN_STREAM("ACCL and GYRO data must be of same size"); 723 | } 724 | 725 | for (uint32_t i = 0; i < accl_data.size(); i++) { 726 | uint64_t accl_time = prev_accl_stamp + (uint64_t)((double)i * accl_step_size); 727 | uint64_t accl_stamp = movie_creation_time + accl_time; 728 | vector accl_sample = accl_data.at(i); 729 | 730 | // Data comes in ZXY order 731 | ImuAccl accl; 732 | accl << accl_sample.at(1), accl_sample.at(2), accl_sample.at(0); 733 | accl_queue.push_back(AcclMeasurement(accl_stamp, accl)); 734 | } 735 | 736 | for (uint32_t i = 0; i < gyro_data.size(); ++i) { 737 | uint64_t gyro_time = prev_gyro_stamp + (uint64_t)((double)i * gyro_step_size); 738 | uint64_t gyro_stamp = movie_creation_time + gyro_time; 739 | vector gyro_sample = gyro_data.at(i); 740 | 741 | // Data comes in ZXY order 742 | ImuGyro gyro; 743 | gyro << gyro_sample.at(1), gyro_sample.at(2), gyro_sample.at(0); 744 | gyro_queue.push_back(GyroMeasurement(gyro_stamp, gyro)); 745 | 746 | total_samples += 1; 747 | } 748 | } 749 | } 750 | 751 | void GoProImuExtractor::readMagnetometerData(std::deque& mag_queue, 752 | uint64_t mag_end_time) { 753 | vector> mag_data; 754 | uint64_t current_mag_stamp, prev_mag_stamp; 755 | 756 | uint64_t total_samples = 0; 757 | uint64_t seq = 0; 758 | 759 | // This extracts measurements from payload 0 to payloads - 1 760 | for (uint32_t index = 0; index < payloads; index++) { 761 | GPMF_ERR ret; 762 | uint32_t payload_size; 763 | 764 | payload_size = GetPayloadSize(mp4, index); 765 | payloadres = GetPayloadResource(mp4, payloadres, payload_size); 766 | payload = GetPayload(mp4, payloadres, index); 767 | 768 | if (payload == NULL) cleanup(); 769 | ret = GPMF_Init(ms, payload, payload_size); 770 | if (ret != GPMF_OK) cleanup(); 771 | 772 | current_mag_stamp = get_stamp(STR2FOURCC("MAGN")); 773 | current_mag_stamp = current_mag_stamp * 1000; // us to ns 774 | 775 | if (index > 0) { 776 | uint64_t mag_time_span = current_mag_stamp - prev_mag_stamp; 777 | 778 | if (mag_time_span < 0) { 779 | cout << RED << "previous magnetometer timestamp should be smaller than current stamp" 780 | << RESET << endl; 781 | exit(1); 782 | } 783 | 784 | double mag_step_size = (double)mag_time_span / (double)mag_data.size(); 785 | 786 | for (uint32_t i = 0; i < mag_data.size(); i++) { 787 | Timestamp mag_time = prev_mag_stamp + (uint64_t)((double)i * mag_step_size); 788 | Timestamp mag_stamp = movie_creation_time + mag_time; 789 | vector mag_sample = mag_data.at(i); 790 | 791 | // The data comes in ZXY order 792 | MagneticField mag; 793 | mag << mag_sample.at(1), mag_sample.at(2), mag_sample.at(0); 794 | mag_queue.push_back(MagMeasurement(mag_stamp, mag)); 795 | } 796 | } 797 | 798 | mag_data.clear(); 799 | get_scaled_data(STR2FOURCC("MAGN"), mag_data); 800 | 801 | prev_mag_stamp = current_mag_stamp; 802 | GPMF_Free(ms); 803 | } 804 | 805 | // If this is not the last video file, extract the last payload 806 | if (mag_end_time > 0) { 807 | // ROS_WARN_STREAM("Writing last payload"); 808 | uint64_t mag_time_span = mag_end_time * 1000 - current_mag_stamp; 809 | double mag_step_size = (double)mag_time_span / (double)mag_data.size(); 810 | 811 | for (uint32_t i = 0; i < mag_data.size(); i++) { 812 | Timestamp mag_time = prev_mag_stamp + (uint64_t)((double)i * mag_step_size); 813 | Timestamp mag_stamp = movie_creation_time + mag_time; 814 | vector mag_sample = mag_data.at(i); 815 | 816 | // Data comes in XYZ order, aligned with camera 817 | MagneticField magnetic_field; 818 | magnetic_field << mag_sample.at(1), mag_sample.at(2), mag_sample.at(0); 819 | mag_queue.push_back(MagMeasurement(mag_stamp, magnetic_field)); 820 | } 821 | } 822 | } -------------------------------------------------------------------------------- /src/VideoExtractor.cpp: -------------------------------------------------------------------------------- 1 | // tutorial01.c 2 | // 3 | // This tutorial was written by Stephen Dranger (dranger@gmail.com). 4 | // 5 | // Code based on a tutorial by Martin Bohme (boehme@inb.uni-luebeckREMOVETHIS.de) 6 | // Tested on Gentoo, CVS version 5/01/07 compiled with GCC 4.1.1 7 | 8 | // A small sample program that shows how to use libavformat and libavcodec to 9 | // read video from a file. 10 | // 11 | // Use the Makefile to build all examples. 12 | // 13 | // Run using 14 | // 15 | // tutorial01 myvideofile.mpg 16 | // 17 | // to write the first five frames from "myvideofile.mpg" to disk in PPM 18 | // format. 19 | 20 | #include "VideoExtractor.h" 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "Utils.h" 35 | #include "color_codes.h" 36 | 37 | GoProVideoExtractor::GoProVideoExtractor(const std::string filename, 38 | double scaling_factor, 39 | bool dump_info) { 40 | video_file = filename; 41 | 42 | av_register_all(); 43 | 44 | // Open video file 45 | std::cout << "Opening Video File: " << video_file << std::endl; 46 | pFormatContext = avformat_alloc_context(); 47 | if (!pFormatContext) { 48 | printf("ERROR could not allocate memory for Format Context"); 49 | } 50 | 51 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 52 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 53 | } 54 | // Retrieve stream information 55 | if (avformat_find_stream_info(pFormatContext, NULL) < 0) 56 | std::cout << RED << "Couldn't find stream information" << RESET << std::endl; 57 | 58 | // Dump information about file onto standard error 59 | if (dump_info) av_dump_format(pFormatContext, 0, video_file.c_str(), 0); 60 | 61 | // Find the first video stream 62 | videoStreamIndex = -1; 63 | std::string creation_time; 64 | 65 | for (uint32_t i = 0; i < pFormatContext->nb_streams; i++) { 66 | codecParameters = pFormatContext->streams[i]->codecpar; 67 | if (codecParameters->codec_type == AVMEDIA_TYPE_VIDEO) { 68 | tag_dict = av_dict_get(pFormatContext->metadata, "", tag_dict, AV_DICT_IGNORE_SUFFIX); 69 | while (tag_dict) { 70 | if (strcmp(tag_dict->key, "creation_time") == 0) { 71 | std::stringstream ss; 72 | ss << tag_dict->value; 73 | ss >> creation_time; 74 | } 75 | tag_dict = av_dict_get(pFormatContext->metadata, "", tag_dict, AV_DICT_IGNORE_SUFFIX); 76 | } 77 | 78 | videoStreamIndex = i; 79 | break; 80 | } 81 | } 82 | 83 | video_creation_time = parseISO(creation_time); 84 | if (videoStreamIndex == -1) 85 | std::cout << RED << "Didn't find a video stream" << RESET << std::endl; 86 | 87 | video_stream = pFormatContext->streams[videoStreamIndex]; 88 | num_frames = video_stream->nb_frames; 89 | 90 | // Get a pointer to the codec context for the video stream 91 | pCodec = avcodec_find_decoder(pFormatContext->streams[videoStreamIndex]->codecpar->codec_id); 92 | 93 | // Find the decoder for the video stream 94 | if (pCodec == nullptr) { 95 | std::cout << RED << "Unsupported codec!" << RESET << std::endl; 96 | } 97 | 98 | pCodecContext = avcodec_alloc_context3(pCodec); 99 | if (!pCodecContext) { 100 | std::cout << RED << "Failed to allocated memory for AVCodecContext" << RESET << std::endl; 101 | } 102 | 103 | pCodecContext->thread_count=0; 104 | pCodecContext->thread_type=FF_THREAD_FRAME; 105 | 106 | if (avcodec_parameters_to_context(pCodecContext, codecParameters) < 0) { 107 | std::cout << RED << "failed to copy codec params to codec context" RESET << std::endl; 108 | } 109 | 110 | // Open codec 111 | if (avcodec_open2(pCodecContext, pCodec, &optionsDict) < 0) 112 | std::cout << RED << "Could not open codec" << RESET << std::endl; 113 | 114 | // Allocate video frame 115 | pFrame = av_frame_alloc(); 116 | 117 | // Allocate an AVFrame structure 118 | pFrameRGB = av_frame_alloc(); 119 | if (pFrameRGB == nullptr) std::cout << RED << "Cannot allocate RGB Frame" << RESET << std::endl; 120 | 121 | image_height = pCodecContext->height; 122 | image_width = pCodecContext->width; 123 | 124 | if (scaling_factor != 1.0) { 125 | image_height = (int)((double)image_height * scaling_factor); 126 | image_width = (int)((double)image_width * scaling_factor); 127 | } 128 | 129 | // Close the video formatcontext 130 | avformat_close_input(&pFormatContext); 131 | } 132 | 133 | GoProVideoExtractor::~GoProVideoExtractor() { 134 | // Free the RGB image 135 | av_free(pFrameRGB); 136 | 137 | // Free the YUV frame 138 | av_free(pFrame); 139 | 140 | // Close the codec 141 | avcodec_close(pCodecContext); 142 | 143 | // Close the format context 144 | avformat_close_input(&pFormatContext); 145 | } 146 | 147 | void GoProVideoExtractor::save_to_png(AVFrame* frame, 148 | AVCodecContext* codecContext, 149 | int width, 150 | int height, 151 | AVRational time_base, 152 | std::string filename) { 153 | AVCodec* outCodec = avcodec_find_encoder(AV_CODEC_ID_PNG); 154 | AVCodecContext* outCodecCtx = avcodec_alloc_context3(outCodec); 155 | 156 | outCodecCtx->width = width; 157 | outCodecCtx->height = height; 158 | outCodecCtx->pix_fmt = AV_PIX_FMT_RGB24; 159 | outCodecCtx->codec_type = AVMEDIA_TYPE_VIDEO; 160 | outCodecCtx->codec_id = AV_CODEC_ID_PNG; 161 | outCodecCtx->time_base.num = codecContext->time_base.num; 162 | outCodecCtx->time_base.den = codecContext->time_base.den; 163 | 164 | frame->height = height; 165 | frame->width = width; 166 | frame->format = AV_PIX_FMT_RGB24; 167 | 168 | if (!outCodec || avcodec_open2(outCodecCtx, outCodec, NULL) < 0) { 169 | return; 170 | } 171 | 172 | AVPacket outPacket; 173 | av_init_packet(&outPacket); 174 | outPacket.size = 0; 175 | outPacket.data = NULL; 176 | 177 | avcodec_send_frame(outCodecCtx, frame); 178 | int ret = -1; 179 | while (ret < 0) { 180 | ret = avcodec_receive_packet(outCodecCtx, &outPacket); 181 | } 182 | 183 | filename = filename + ".png"; 184 | FILE* outPng = fopen(filename.c_str(), "wb"); 185 | fwrite(outPacket.data, outPacket.size, 1, outPng); 186 | fclose(outPng); 187 | } 188 | 189 | void GoProVideoExtractor::save_raw(AVFrame* pFrame, int width, int height, std::string filename) { 190 | FILE* pFile; 191 | int y; 192 | 193 | // Open file 194 | filename = filename + ".ppm"; 195 | pFile = fopen(filename.c_str(), "wb"); 196 | if (pFile == NULL) return; 197 | 198 | // Write header 199 | fprintf(pFile, "P6\n%d %d\n255\n", width, height); 200 | 201 | // Write pixel data 202 | for (y = 0; y < height; y++) 203 | fwrite(pFrame->data[0] + y * pFrame->linesize[0], 1, width * 3, pFile); 204 | 205 | // Close file 206 | fclose(pFile); 207 | } 208 | 209 | int GoProVideoExtractor::extractFrames(const std::string& image_folder, 210 | uint64_t last_image_stamp_ns) { 211 | std::string image_data_folder = image_folder + "/data"; 212 | if (!std::experimental::filesystem::is_directory(image_data_folder)) { 213 | std::experimental::filesystem::create_directories(image_data_folder); 214 | } 215 | std::string image_file = image_folder + "/data.csv"; 216 | std::ofstream image_stream; 217 | image_stream.open(image_file); 218 | image_stream << std::fixed << std::setprecision(19); 219 | image_stream << "#timestamp [ns],filename" << std::endl; 220 | 221 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 222 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 223 | return -1; 224 | } 225 | video_stream = pFormatContext->streams[videoStreamIndex]; 226 | 227 | // PIX_FMT_RGB24 228 | // Determine required buffer size and allocate buffer 229 | int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); 230 | uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t)); 231 | 232 | sws_ctx = sws_getContext(pCodecContext->width, 233 | pCodecContext->height, 234 | pCodecContext->pix_fmt, 235 | image_width, 236 | image_height, 237 | AV_PIX_FMT_RGB24, 238 | SWS_BILINEAR, 239 | nullptr, 240 | nullptr, 241 | nullptr); 242 | // 243 | // Assign appropriate parts of buffer to image planes in pFrameRGB 244 | // Note that pFrameRGB is an AVFrame, but AVFrame is a superset 245 | // of AVPicture 246 | av_image_fill_arrays( 247 | pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_RGB24, image_width, image_height, 1); 248 | 249 | // std::cout << "Video Creation Time: " << uint64_to_string(start_time) << std::endl; 250 | 251 | double global_clock; 252 | uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; 253 | 254 | int frameFinished; 255 | while (av_read_frame(pFormatContext, &packet) >= 0) { 256 | // Is this a packet from the video stream? 257 | if (packet.stream_index == videoStreamIndex) { 258 | // Decode video frame 259 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 260 | 261 | // Did we get a video frame? 262 | 263 | if (packet.dts != AV_NOPTS_VALUE) { 264 | global_clock = av_frame_get_best_effort_timestamp(pFrame); 265 | global_video_pkt_pts = packet.pts; 266 | } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { 267 | global_clock = global_video_pkt_pts; 268 | } else { 269 | global_clock = 0; 270 | } 271 | 272 | double frame_delay = av_q2d(video_stream->time_base); 273 | global_clock *= frame_delay; 274 | 275 | // Only if we are repeating the 276 | if (pFrame->repeat_pict > 0) { 277 | double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); 278 | global_clock += extra_delay; 279 | } 280 | 281 | if (frameFinished) { 282 | // Convert the image from its native format to RGB 283 | sws_scale(sws_ctx, 284 | (uint8_t const* const*)pFrame->data, 285 | pFrame->linesize, 286 | 0, 287 | pCodecContext->height, 288 | pFrameRGB->data, 289 | pFrameRGB->linesize); 290 | 291 | // Save the frame to disk 292 | // std::cout << "Global Clock: " << global_clock << std::endl; 293 | uint64_t nanosecs = (uint64_t)(global_clock * 1e9); 294 | // std::cout << "Nano secs: " << nanosecs << std::endl; 295 | if (nanosecs > last_image_stamp_ns) { 296 | break; 297 | } 298 | uint64_t current_stamp = video_creation_time + nanosecs; 299 | std::string string_stamp = uint64_to_string(current_stamp); 300 | std::string stamped_image_filename = image_data_folder + "/" + string_stamp; 301 | image_stream << string_stamp << "," << string_stamp + ".png" << std::endl; 302 | save_to_png(pFrameRGB, 303 | pCodecContext, 304 | image_width, 305 | image_height, 306 | video_stream->time_base, 307 | stamped_image_filename); 308 | } 309 | } 310 | 311 | // Free the packet that was allocated by av_read_frame 312 | av_packet_unref(&packet); 313 | } 314 | 315 | image_stream.close(); 316 | 317 | // Free the RGB image 318 | av_free(buffer); 319 | 320 | // Close the video file 321 | avformat_close_input(&pFormatContext); 322 | 323 | return 0; 324 | } 325 | 326 | int GoProVideoExtractor::getFrameStamps(std::vector& stamps) { 327 | ProgressBar progress(std::clog, 70u, "Progress", '#'); 328 | stamps.clear(); 329 | 330 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 331 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 332 | } 333 | video_stream = pFormatContext->streams[videoStreamIndex]; 334 | 335 | double global_clock; 336 | uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; 337 | int frameFinished; 338 | uint32_t frame_count = 0; 339 | while (av_read_frame(pFormatContext, &packet) >= 0) { 340 | // Is this a packet from the video stream? 341 | if (packet.stream_index == videoStreamIndex) { 342 | // Decode video frame 343 | // avcodec_send_packet(pCodecContext, &packet); 344 | // frameFinished = avcodec_receive_frame(pCodecContext, pFrameRGB); 345 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 346 | 347 | // Did we get a video frame? 348 | 349 | if (packet.dts != AV_NOPTS_VALUE) { 350 | global_clock = av_frame_get_best_effort_timestamp(pFrame); 351 | global_video_pkt_pts = packet.pts; 352 | } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { 353 | global_clock = global_video_pkt_pts; 354 | } else { 355 | global_clock = 0; 356 | } 357 | 358 | double frame_delay = av_q2d(video_stream->time_base); 359 | global_clock *= frame_delay; 360 | 361 | // Only if we are repeating the 362 | if (pFrame->repeat_pict > 0) { 363 | double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); 364 | global_clock += extra_delay; 365 | } 366 | 367 | uint64_t usecs = (uint64_t)(global_clock * 1000000); 368 | // std::cout << "Stamp: " << usecs << std::endl; 369 | stamps.push_back(usecs); 370 | 371 | frame_count++; 372 | double percent = (double)frame_count / (double)num_frames; 373 | progress.write(percent); 374 | } 375 | } 376 | 377 | // Close the video file 378 | avformat_close_input(&pFormatContext); 379 | 380 | return 0; 381 | } 382 | 383 | void GoProVideoExtractor::displayImages() { 384 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 385 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 386 | exit(1); 387 | } 388 | 389 | video_stream = pFormatContext->streams[videoStreamIndex]; 390 | 391 | // PIX_FMT_RGB24 392 | // Determine required buffer size and allocate buffer 393 | int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); 394 | uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t)); 395 | 396 | sws_ctx = sws_getContext(pCodecContext->width, 397 | pCodecContext->height, 398 | pCodecContext->pix_fmt, 399 | image_width, 400 | image_height, 401 | AV_PIX_FMT_RGB24, 402 | SWS_BILINEAR, 403 | nullptr, 404 | nullptr, 405 | nullptr); 406 | // 407 | // Assign appropriate parts of buffer to image planes in pFrameRGB 408 | // Note that pFrameRGB is an AVFrame, but AVFrame is a superset 409 | // of AVPicture 410 | av_image_fill_arrays( 411 | pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_RGB24, image_width, image_height, 1); 412 | 413 | // std::cout << "Video Creation Time: " << uint64_to_string(start_time) << std::endl; 414 | 415 | int frameFinished; 416 | // cv::namedWindow("GoPro Video", cv::WINDOW_GUI_EXPANDED); 417 | 418 | while (av_read_frame(pFormatContext, &packet) >= 0) { 419 | // Is this a packet from the video stream? 420 | if (packet.stream_index == videoStreamIndex) { 421 | // Decode video frame 422 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 423 | 424 | if (frameFinished) { 425 | // Convert the image from its native format to RGB 426 | sws_scale(sws_ctx, 427 | (uint8_t const* const*)pFrame->data, 428 | pFrame->linesize, 429 | 0, 430 | pCodecContext->height, 431 | pFrameRGB->data, 432 | pFrameRGB->linesize); 433 | 434 | cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); 435 | cv::cvtColor(img, img, cv::COLOR_RGB2BGR); 436 | // std::cout << "width: " << img.size().width << " height: " << img.size().height << 437 | // std::endl; 438 | cv::imshow("GoPro Video", img); 439 | cv::waitKey(1); 440 | } 441 | } 442 | 443 | // Free the packet that was allocated by av_read_frame 444 | av_packet_unref(&packet); 445 | } 446 | 447 | cv::destroyAllWindows(); 448 | 449 | // Free the RGB image 450 | av_free(buffer); 451 | 452 | // Close the video file 453 | avformat_close_input(&pFormatContext); 454 | } 455 | 456 | void GoProVideoExtractor::writeVideo(const std::string& bag_file, 457 | uint64_t last_image_stamp_ns, 458 | const std::string& image_topic) { 459 | ProgressBar progress(std::clog, 80u, "Progress"); 460 | 461 | rosbag::Bag bag; 462 | if (std::experimental::filesystem::exists(bag_file)) 463 | bag.open(bag_file, rosbag::bagmode::Append); 464 | else 465 | bag.open(bag_file, rosbag::bagmode::Write); 466 | 467 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 468 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 469 | return; 470 | } 471 | video_stream = pFormatContext->streams[videoStreamIndex]; 472 | 473 | // PIX_FMT_RGB24 474 | // Determine required buffer size and allocate buffer 475 | int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); 476 | uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t)); 477 | 478 | sws_ctx = sws_getContext(pCodecContext->width, 479 | pCodecContext->height, 480 | pCodecContext->pix_fmt, 481 | image_width, 482 | image_height, 483 | AV_PIX_FMT_RGB24, 484 | SWS_BILINEAR, 485 | nullptr, 486 | nullptr, 487 | nullptr); 488 | // 489 | // Assign appropriate parts of buffer to image planes in pFrameRGB 490 | // Note that pFrameRGB is an AVFrame, but AVFrame is a superset 491 | // of AVPicture 492 | av_image_fill_arrays( 493 | pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_RGB24, image_width, image_height, 1); 494 | 495 | // std::cout << "Video Creation Time: " << uint64_to_string(start_time) << std::endl; 496 | 497 | double global_clock; 498 | uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; 499 | 500 | int frameFinished; 501 | uint64_t seq = 0; 502 | while (av_read_frame(pFormatContext, &packet) >= 0) { 503 | // Is this a packet from the video stream? 504 | if (packet.stream_index == videoStreamIndex) { 505 | // Decode video frame 506 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 507 | 508 | // Did we get a video frame? 509 | 510 | if (packet.dts != AV_NOPTS_VALUE) { 511 | global_clock = av_frame_get_best_effort_timestamp(pFrame); 512 | global_video_pkt_pts = packet.pts; 513 | } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { 514 | global_clock = global_video_pkt_pts; 515 | } else { 516 | global_clock = 0; 517 | } 518 | 519 | double frame_delay = av_q2d(video_stream->time_base); 520 | global_clock *= frame_delay; 521 | 522 | // Only if we are repeating the 523 | if (pFrame->repeat_pict > 0) { 524 | double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); 525 | global_clock += extra_delay; 526 | } 527 | 528 | if (frameFinished) { 529 | // Convert the image from its native format to RGB 530 | sws_scale(sws_ctx, 531 | (uint8_t const* const*)pFrame->data, 532 | pFrame->linesize, 533 | 0, 534 | pCodecContext->height, 535 | pFrameRGB->data, 536 | pFrameRGB->linesize); 537 | 538 | // Save the frame to disk 539 | uint64_t nanosecs = (uint64_t)(global_clock * 1e9); 540 | if (nanosecs > last_image_stamp_ns) { 541 | break; 542 | } 543 | uint64_t current_stamp = video_creation_time + nanosecs; 544 | uint32_t secs = current_stamp * 1e-9; 545 | uint32_t n_secs = current_stamp % 1000000000; 546 | ros::Time ros_time(secs, n_secs); 547 | 548 | cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); 549 | cv::cvtColor(img, img, cv::COLOR_RGB2BGR); 550 | 551 | std_msgs::Header header; 552 | header.stamp = ros_time; 553 | header.frame_id = "gopro"; 554 | header.seq = seq++; 555 | 556 | sensor_msgs::ImagePtr imgmsg = 557 | cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, img).toImageMsg(); 558 | bag.write(image_topic, ros_time, imgmsg); 559 | 560 | double percent = (double)seq / (double)num_frames; 561 | progress.write(percent); 562 | } 563 | } 564 | 565 | // Free the packet that was allocated by av_read_frame 566 | av_packet_unref(&packet); 567 | } 568 | 569 | // Free the RGB image 570 | av_free(buffer); 571 | 572 | // Close the video file 573 | avformat_close_input(&pFormatContext); 574 | 575 | bag.close(); 576 | } 577 | 578 | void GoProVideoExtractor::writeVideo(rosbag::Bag& bag, 579 | uint64_t last_image_stamp_ns, 580 | const std::string& image_topic, 581 | bool grayscale, 582 | bool compress_image, 583 | bool display_images) { 584 | ProgressBar progress(std::clog, 80u, "Progress"); 585 | 586 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 587 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 588 | return; 589 | } 590 | video_stream = pFormatContext->streams[videoStreamIndex]; 591 | 592 | // PIX_FMT_RGB24 593 | // Determine required buffer size and allocate buffer 594 | int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); 595 | uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t)); 596 | 597 | sws_ctx = sws_getContext(pCodecContext->width, 598 | pCodecContext->height, 599 | pCodecContext->pix_fmt, 600 | image_width, 601 | image_height, 602 | AV_PIX_FMT_RGB24, 603 | SWS_BILINEAR, 604 | nullptr, 605 | nullptr, 606 | nullptr); 607 | // 608 | // Assign appropriate parts of buffer to image planes in pFrameRGB 609 | // Note that pFrameRGB is an AVFrame, but AVFrame is a superset 610 | // of AVPicture 611 | av_image_fill_arrays( 612 | pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_RGB24, image_width, image_height, 1); 613 | 614 | // std::cout << "Video Creation Time: " << uint64_to_string(start_time) << std::endl; 615 | 616 | double global_clock; 617 | uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; 618 | 619 | int frameFinished; 620 | uint32_t frame_count = 0; 621 | while (av_read_frame(pFormatContext, &packet) >= 0) { 622 | // Is this a packet from the video stream? 623 | if (packet.stream_index == videoStreamIndex) { 624 | // Decode video frame 625 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 626 | 627 | // Did we get a video frame? 628 | 629 | if (packet.dts != AV_NOPTS_VALUE) { 630 | global_clock = av_frame_get_best_effort_timestamp(pFrame); 631 | global_video_pkt_pts = packet.pts; 632 | } else if (global_video_pkt_pts && global_video_pkt_pts != AV_NOPTS_VALUE) { 633 | global_clock = global_video_pkt_pts; 634 | } else { 635 | global_clock = 0; 636 | } 637 | 638 | double frame_delay = av_q2d(video_stream->time_base); 639 | global_clock *= frame_delay; 640 | 641 | // Only if we are repeating the 642 | if (pFrame->repeat_pict > 0) { 643 | double extra_delay = pFrame->repeat_pict * (frame_delay * 0.5); 644 | global_clock += extra_delay; 645 | } 646 | 647 | if (frameFinished) { 648 | // Convert the image from its native format to RGB 649 | sws_scale(sws_ctx, 650 | (uint8_t const* const*)pFrame->data, 651 | pFrame->linesize, 652 | 0, 653 | pCodecContext->height, 654 | pFrameRGB->data, 655 | pFrameRGB->linesize); 656 | 657 | // Save the frame to disk 658 | uint64_t nanosecs = (uint64_t)(global_clock * 1e9); 659 | if (nanosecs > last_image_stamp_ns) { 660 | break; 661 | } 662 | uint64_t current_stamp = video_creation_time + nanosecs; 663 | uint32_t secs = current_stamp * 1e-9; 664 | uint32_t n_secs = current_stamp % 1000000000; 665 | ros::Time ros_time(secs, n_secs); 666 | 667 | cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); 668 | cv::cvtColor(img, img, cv::COLOR_RGB2BGR); 669 | 670 | std::string encoding = sensor_msgs::image_encodings::BGR8; 671 | if (grayscale) { 672 | cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); 673 | encoding = sensor_msgs::image_encodings::MONO8; 674 | } 675 | 676 | if (display_images) { 677 | cv::imshow("GoPro Video", img); 678 | cv::waitKey(1); 679 | } 680 | 681 | std_msgs::Header header; 682 | header.stamp = ros_time; 683 | header.frame_id = "gopro"; 684 | 685 | if (compress_image) { 686 | sensor_msgs::CompressedImagePtr img_msg = 687 | cv_bridge::CvImage(header, encoding, img).toCompressedImageMsg(); 688 | bag.write(image_topic + "/compressed", ros_time, img_msg); 689 | } else { 690 | sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); 691 | bag.write(image_topic, ros_time, imgmsg); 692 | } 693 | } 694 | 695 | double percent = (double)frame_count++ / (double)num_frames; 696 | progress.write(percent); 697 | } 698 | 699 | // Free the packet that was allocated by av_read_frame 700 | av_packet_unref(&packet); 701 | } 702 | 703 | cv::destroyAllWindows(); 704 | 705 | // Free the RGB image 706 | av_free(buffer); 707 | 708 | // Close the video file 709 | avformat_close_input(&pFormatContext); 710 | } 711 | 712 | void GoProVideoExtractor::writeVideo(rosbag::Bag& bag, 713 | const std::string& image_topic, 714 | const std::vector image_stamps, 715 | bool grayscale, 716 | bool compress_image, 717 | bool display_images) { 718 | ProgressBar progress(std::clog, 80u, "Progress"); 719 | 720 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 721 | ROS_ERROR_STREAM("Could not open file" << video_file.c_str()); 722 | return; 723 | } 724 | video_stream = pFormatContext->streams[videoStreamIndex]; 725 | 726 | // PIX_FMT_RGB24 727 | // Determine required buffer size and allocate buffer 728 | int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); 729 | uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t)); 730 | 731 | sws_ctx = sws_getContext(pCodecContext->width, 732 | pCodecContext->height, 733 | pCodecContext->pix_fmt, 734 | image_width, 735 | image_height, 736 | AV_PIX_FMT_RGB24, 737 | SWS_BILINEAR, 738 | nullptr, 739 | nullptr, 740 | nullptr); 741 | // 742 | // Assign appropriate parts of buffer to image planes in pFrameRGB 743 | // Note that pFrameRGB is an AVFrame, but AVFrame is a superset 744 | // of AVPicture 745 | av_image_fill_arrays( 746 | pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_RGB24, image_width, image_height, 1); 747 | 748 | // std::cout << "Video Creation Time: " << uint64_to_string(start_time) << std::endl; 749 | 750 | double global_clock; 751 | uint64_t global_video_pkt_pts = AV_NOPTS_VALUE; 752 | 753 | int frameFinished; 754 | uint32_t frame_count = 0; 755 | while (av_read_frame(pFormatContext, &packet) >= 0) { 756 | // Is this a packet from the video stream? 757 | if (packet.stream_index == videoStreamIndex) { 758 | // Decode video frame 759 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 760 | 761 | if (frame_count == image_stamps.size()) { 762 | ROS_WARN_STREAM( 763 | "Number of images does not match number of timestamps. This should only happen in " 764 | "last/single GoPro Video !!"); 765 | ROS_WARN_STREAM("Skipping " << num_frames - image_stamps.size() << "/" << num_frames 766 | << " images"); 767 | break; 768 | } 769 | // Did we get a video frame? 770 | 771 | if (frameFinished) { 772 | // Convert the image from its native format to RGB 773 | sws_scale(sws_ctx, 774 | (uint8_t const* const*)pFrame->data, 775 | pFrame->linesize, 776 | 0, 777 | pCodecContext->height, 778 | pFrameRGB->data, 779 | pFrameRGB->linesize); 780 | 781 | // Save the frame to disk 782 | uint64_t current_stamp = image_stamps[frame_count]; 783 | uint32_t secs = current_stamp * 1e-9; 784 | uint32_t n_secs = current_stamp % 1000000000; 785 | ros::Time ros_time(secs, n_secs); 786 | 787 | cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); 788 | cv::cvtColor(img, img, cv::COLOR_RGB2BGR); 789 | 790 | std::string encoding = sensor_msgs::image_encodings::BGR8; 791 | if (grayscale) { 792 | cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); 793 | encoding = sensor_msgs::image_encodings::MONO8; 794 | } 795 | 796 | if (display_images) { 797 | cv::imshow("GoPro Video", img); 798 | cv::waitKey(1); 799 | } 800 | 801 | std_msgs::Header header; 802 | header.stamp = ros_time; 803 | header.frame_id = "gopro"; 804 | header.seq = frame_count++; 805 | 806 | if (compress_image) { 807 | sensor_msgs::CompressedImagePtr img_msg = 808 | cv_bridge::CvImage(header, encoding, img).toCompressedImageMsg(); 809 | bag.write(image_topic + "/compressed", ros_time, img_msg); 810 | } else { 811 | sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(header, encoding, img).toImageMsg(); 812 | bag.write(image_topic, ros_time, imgmsg); 813 | } 814 | } 815 | 816 | double percent = (double)frame_count / (double)num_frames; 817 | progress.write(percent); 818 | } 819 | 820 | // Free the packet that was allocated by av_read_frame 821 | av_packet_unref(&packet); 822 | } 823 | 824 | cv::destroyAllWindows(); 825 | 826 | // Free the RGB image 827 | av_free(buffer); 828 | 829 | // Close the video file 830 | avformat_close_input(&pFormatContext); 831 | } 832 | 833 | int GoProVideoExtractor::extractFrames(const std::string& image_folder, 834 | const std::vector image_stamps, 835 | bool grayscale, 836 | bool display_images) { 837 | ProgressBar progress(std::clog, 80u, "Progress"); 838 | 839 | std::string image_data_folder = image_folder + "/data"; 840 | std::string image_file = image_folder + "/data.csv"; 841 | std::ofstream image_stream; 842 | image_stream.open(image_file, std::ofstream::app); 843 | image_stream << std::fixed << std::setprecision(19); 844 | 845 | if (avformat_open_input(&pFormatContext, video_file.c_str(), NULL, NULL) != 0) { 846 | std::cout << RED << "Could not open file" << video_file.c_str() << RESET << std::endl; 847 | return -1; 848 | } 849 | video_stream = pFormatContext->streams[videoStreamIndex]; 850 | 851 | // PIX_FMT_RGB24 852 | // Determine required buffer size and allocate buffer 853 | int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); 854 | uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t)); 855 | 856 | sws_ctx = sws_getContext(pCodecContext->width, 857 | pCodecContext->height, 858 | pCodecContext->pix_fmt, 859 | image_width, 860 | image_height, 861 | AV_PIX_FMT_RGB24, 862 | SWS_BILINEAR, 863 | nullptr, 864 | nullptr, 865 | nullptr); 866 | // 867 | // Assign appropriate parts of buffer to image planes in pFrameRGB 868 | // Note that pFrameRGB is an AVFrame, but AVFrame is a superset 869 | // of AVPicture 870 | av_image_fill_arrays( 871 | pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_RGB24, image_width, image_height, 1); 872 | 873 | // std::cout << "Video Creation Time: " << uint64_to_string(start_time) << std::endl; 874 | 875 | int frameFinished; 876 | uint32_t frame_count = 0; 877 | while (av_read_frame(pFormatContext, &packet) >= 0) { 878 | // Is this a packet from the video stream? 879 | if (packet.stream_index == videoStreamIndex) { 880 | // Decode video frame 881 | avcodec_decode_video2(pCodecContext, pFrame, &frameFinished, &packet); 882 | 883 | if (frame_count == image_stamps.size()) { 884 | ROS_WARN_STREAM( 885 | "Number of images does not match number of timestamps. This should only happen in " 886 | "last/single GoPro Video !!"); 887 | ROS_WARN_STREAM("Skipping " << num_frames - image_stamps.size() << "/" << num_frames 888 | << " images"); 889 | break; 890 | } 891 | 892 | // Did we get a video frame? 893 | 894 | if (frameFinished) { 895 | // Convert the image from its native format to RGB 896 | sws_scale(sws_ctx, 897 | (uint8_t const* const*)pFrame->data, 898 | pFrame->linesize, 899 | 0, 900 | pCodecContext->height, 901 | pFrameRGB->data, 902 | pFrameRGB->linesize); 903 | 904 | uint64_t current_stamp = image_stamps[frame_count]; 905 | std::string string_stamp = uint64_to_string(current_stamp); 906 | std::string stamped_image_filename = image_data_folder + "/" + string_stamp; 907 | image_stream << string_stamp << "," << string_stamp + ".png" << std::endl; 908 | 909 | cv::Mat img(image_height, image_width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]); 910 | cv::cvtColor(img, img, cv::COLOR_RGB2BGR); 911 | 912 | if (grayscale) { 913 | cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); 914 | } 915 | 916 | if (display_images) { 917 | cv::imshow("GoPro Video", img); 918 | cv::waitKey(1); 919 | } 920 | 921 | cv::imwrite(stamped_image_filename + ".png", img); 922 | frame_count++; 923 | } 924 | 925 | double percent = (double)frame_count / (double)num_frames; 926 | progress.write(percent); 927 | } 928 | 929 | // Free the packet that was allocated by av_read_frame 930 | av_packet_unref(&packet); 931 | } 932 | 933 | image_stream.close(); 934 | 935 | // Free the RGB image 936 | av_free(buffer); 937 | 938 | // Close the video file 939 | avformat_close_input(&pFormatContext); 940 | 941 | return 0; 942 | } 943 | -------------------------------------------------------------------------------- /src/GPMF_mp4reader.c: -------------------------------------------------------------------------------- 1 | // clang-format off 2 | 3 | /*! @file mp4reader.c 4 | * 5 | * @brief Way Too Crude MP4|MOV reader 6 | * 7 | * @version 1.8.3 8 | * 9 | * (C) Copyright 2017-2020 GoPro Inc (http://gopro.com/). 10 | * 11 | * Licensed under either: 12 | * - Apache License, Version 2.0, http://www.apache.org/licenses/LICENSE-2.0 13 | * - MIT license, http://opensource.org/licenses/MIT 14 | * at your option. 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | * 22 | */ 23 | 24 | /* This is not an elegant MP4 parser, only used to help demonstrate extraction of GPMF */ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "GPMF_mp4reader.h" 34 | #include "GPMF_common.h" 35 | 36 | #define PRINT_MP4_STRUCTURE 0 37 | 38 | #ifdef _WINDOWS 39 | #define LONGTELL _ftelli64 40 | #else 41 | #define LONGTELL ftell 42 | #endif 43 | 44 | uint32_t GetNumberPayloads(size_t mp4handle) 45 | { 46 | mp4object *mp4 = (mp4object *)mp4handle; 47 | 48 | if (mp4) 49 | { 50 | return mp4->indexcount; 51 | } 52 | 53 | return 0; 54 | } 55 | 56 | size_t GetPayloadResource(size_t mp4handle, size_t resHandle, uint32_t payloadsize) 57 | { 58 | resObject *res = (resObject *)resHandle; 59 | 60 | if (res == NULL) 61 | { 62 | res = (resObject *)malloc(sizeof(resObject)); 63 | if (res) 64 | { 65 | memset(res, 0, sizeof(resObject)); 66 | resHandle = (size_t)res; 67 | } 68 | } 69 | 70 | if (res) 71 | { 72 | uint32_t myBufferSize = payloadsize + 256; 73 | 74 | if (res->buffer == NULL) 75 | { 76 | res->buffer = malloc(myBufferSize); 77 | if (res->buffer) 78 | { 79 | res->bufferSize = myBufferSize; 80 | } 81 | else 82 | { 83 | free(res); 84 | resHandle = 0; 85 | } 86 | } 87 | else if (payloadsize > res->bufferSize) 88 | { 89 | res->buffer = realloc(res->buffer, myBufferSize); 90 | res->bufferSize = myBufferSize; 91 | if (res->buffer == NULL) 92 | { 93 | free(res); 94 | resHandle = 0; 95 | } 96 | } 97 | } 98 | 99 | return resHandle; 100 | } 101 | 102 | void FreePayloadResource(size_t mp4handle, size_t resHandle) 103 | { 104 | resObject *res = (resObject *)resHandle; 105 | 106 | if (res) 107 | { 108 | if (res->buffer) 109 | free(res->buffer); 110 | free(res); 111 | } 112 | } 113 | 114 | uint32_t *GetPayload(size_t mp4handle, size_t resHandle, uint32_t index) 115 | { 116 | mp4object *mp4 = (mp4object *)mp4handle; 117 | resObject *res = (resObject *)resHandle; 118 | 119 | if (mp4 == NULL) 120 | return NULL; 121 | if (res == NULL) 122 | return NULL; 123 | 124 | if (index < mp4->indexcount && mp4->mediafp) 125 | { 126 | if ((mp4->filesize >= mp4->metaoffsets[index] + mp4->metasizes[index]) && (mp4->metasizes[index] > 0)) 127 | { 128 | uint32_t buffsizeneeded = mp4->metasizes[index]; // Add a little more to limit reallocations 129 | 130 | resHandle = GetPayloadResource(mp4handle, resHandle, buffsizeneeded); 131 | if (resHandle) 132 | { 133 | #ifdef _WINDOWS 134 | _fseeki64(mp4->mediafp, (__int64)mp4->metaoffsets[index], SEEK_SET); 135 | #else 136 | fseeko(mp4->mediafp, (off_t)mp4->metaoffsets[index], SEEK_SET); 137 | #endif 138 | fread(res->buffer, 1, mp4->metasizes[index], mp4->mediafp); 139 | mp4->filepos = mp4->metaoffsets[index] + mp4->metasizes[index]; 140 | return res->buffer; 141 | } 142 | } 143 | } 144 | 145 | return NULL; 146 | } 147 | 148 | uint32_t WritePayload(size_t handle, uint32_t *payload, uint32_t payloadsize, uint32_t index) 149 | { 150 | mp4object *mp4 = (mp4object *)handle; 151 | if (mp4 == NULL) 152 | return 0; 153 | 154 | if (index < mp4->indexcount && mp4->mediafp) 155 | { 156 | if ((mp4->filesize >= mp4->metaoffsets[index] + mp4->metasizes[index]) && mp4->metasizes[index] == payloadsize) 157 | { 158 | #ifdef _WINDOWS 159 | _fseeki64(mp4->mediafp, (__int64)mp4->metaoffsets[index], SEEK_SET); 160 | #else 161 | fseeko(mp4->mediafp, (off_t)mp4->metaoffsets[index], SEEK_SET); 162 | #endif 163 | fwrite(payload, 1, payloadsize, mp4->mediafp); 164 | mp4->filepos = mp4->metaoffsets[index] + payloadsize; 165 | return payloadsize; 166 | } 167 | } 168 | 169 | return 0; 170 | } 171 | 172 | void LongSeek(mp4object *mp4, int64_t offset) 173 | { 174 | if (mp4 && offset) 175 | { 176 | if (mp4->filepos + offset < mp4->filesize) 177 | { 178 | #ifdef _WINDOWS 179 | _fseeki64(mp4->mediafp, (__int64)offset, SEEK_CUR); 180 | #else 181 | fseeko(mp4->mediafp, (off_t)offset, SEEK_CUR); 182 | #endif 183 | mp4->filepos += offset; 184 | } 185 | else 186 | { 187 | mp4->filepos = mp4->filesize; 188 | } 189 | } 190 | } 191 | 192 | void FreePayload(uint32_t *lastpayload) 193 | { 194 | if (lastpayload) 195 | free(lastpayload); 196 | } 197 | 198 | uint32_t GetPayloadSize(size_t handle, uint32_t index) 199 | { 200 | mp4object *mp4 = (mp4object *)handle; 201 | if (mp4 == NULL) 202 | return 0; 203 | 204 | if (mp4->metasizes && mp4->metasize_count > index) 205 | return mp4->metasizes[index] & (uint32_t)~0x3; //All GPMF payloads are 32-bit aligned and sized 206 | 207 | return 0; 208 | } 209 | 210 | #define MAX_NEST_LEVEL 20 211 | 212 | size_t OpenMP4Source(char *filename, uint32_t traktype, uint32_t traksubtype, int32_t flags) //RAW or within MP4 213 | { 214 | mp4object *mp4 = (mp4object *)malloc(sizeof(mp4object)); 215 | if (mp4 == NULL) 216 | return 0; 217 | 218 | memset(mp4, 0, sizeof(mp4object)); 219 | 220 | #ifdef _WINDOWS 221 | struct _stat64 mp4stat; 222 | _stat64(filename, &mp4stat); 223 | #else 224 | struct stat mp4stat; 225 | stat(filename, &mp4stat); 226 | #endif 227 | mp4->filesize = (uint64_t)mp4stat.st_size; 228 | // printf("filesize = %ld\n", mp4->filesize); 229 | if (mp4->filesize < 64) 230 | { 231 | free(mp4); 232 | return 0; 233 | } 234 | 235 | const char *mode = (flags & MP4_FLAG_READ_WRITE_MODE) ? "rb+" : "rb"; 236 | #ifdef _WINDOWS 237 | fopen_s(&mp4->mediafp, filename, mode); 238 | #else 239 | mp4->mediafp = fopen(filename, mode); 240 | #endif 241 | 242 | if (mp4->mediafp) 243 | { 244 | uint32_t qttag, qtsize32, skip, type = 0, subtype = 0, num; 245 | size_t len; 246 | int32_t nest = 0; 247 | uint64_t nestsize[MAX_NEST_LEVEL] = {0}; 248 | uint64_t lastsize = 0, qtsize; 249 | 250 | do 251 | { 252 | len = fread(&qtsize32, 1, 4, mp4->mediafp); 253 | len += fread(&qttag, 1, 4, mp4->mediafp); 254 | mp4->filepos += len; 255 | if (len == 8 && mp4->filepos < mp4->filesize) 256 | { 257 | if (mp4->filepos == 8 && qttag != MAKEID('f', 't', 'y', 'p')) 258 | { 259 | CloseSource((size_t)mp4); 260 | mp4 = NULL; 261 | break; 262 | } 263 | 264 | if (!VALID_FOURCC(qttag) && (qttag & 0xff) != 0xa9) // ©xyz and ©swr are allowed 265 | { 266 | CloseSource((size_t)mp4); 267 | mp4 = NULL; 268 | break; 269 | } 270 | 271 | qtsize32 = BYTESWAP32(qtsize32); 272 | 273 | if (qtsize32 == 1) // 64-bit Atom 274 | { 275 | len = fread(&qtsize, 1, 8, mp4->mediafp); 276 | mp4->filepos += len; 277 | qtsize = BYTESWAP64(qtsize) - 8; 278 | } 279 | else 280 | qtsize = qtsize32; 281 | 282 | if (qtsize - len > (mp4->filesize - mp4->filepos)) // not parser truncated files. 283 | { 284 | CloseSource((size_t)mp4); 285 | mp4 = NULL; 286 | break; 287 | } 288 | 289 | nest++; 290 | 291 | if (qtsize < 8) 292 | break; 293 | if (nest >= MAX_NEST_LEVEL) 294 | break; 295 | if (nest > 1 && qtsize > nestsize[nest - 1]) 296 | break; 297 | 298 | nestsize[nest] = qtsize; 299 | lastsize = qtsize; 300 | 301 | #if PRINT_MP4_STRUCTURE 302 | 303 | for (int i = 1; i < nest; i++) 304 | printf("%5d ", nestsize[i]); //printf(" "); 305 | printf(" %c%c%c%c (%lld)\n", (qttag & 0xff), ((qttag >> 8) & 0xff), ((qttag >> 16) & 0xff), ((qttag >> 24) & 0xff), qtsize); 306 | 307 | if (qttag == MAKEID('m', 'd', 'a', 't') || 308 | qttag == MAKEID('f', 't', 'y', 'p') || 309 | qttag == MAKEID('u', 'd', 't', 'a') || 310 | qttag == MAKEID('f', 'r', 'e', 'e')) 311 | { 312 | LongSeek(mp4, qtsize - 8); 313 | 314 | NESTSIZE(qtsize); 315 | 316 | continue; 317 | } 318 | #endif 319 | if (qttag != MAKEID('m', 'o', 'o', 'v') && //skip over all but these atoms 320 | qttag != MAKEID('m', 'v', 'h', 'd') && 321 | qttag != MAKEID('t', 'r', 'a', 'k') && 322 | qttag != MAKEID('m', 'd', 'i', 'a') && 323 | qttag != MAKEID('m', 'd', 'h', 'd') && 324 | qttag != MAKEID('m', 'i', 'n', 'f') && 325 | qttag != MAKEID('g', 'm', 'i', 'n') && 326 | qttag != MAKEID('d', 'i', 'n', 'f') && 327 | qttag != MAKEID('a', 'l', 'i', 's') && 328 | qttag != MAKEID('s', 't', 's', 'd') && 329 | qttag != MAKEID('s', 't', 'b', 'l') && 330 | qttag != MAKEID('s', 't', 't', 's') && 331 | qttag != MAKEID('s', 't', 's', 'c') && 332 | qttag != MAKEID('s', 't', 's', 'z') && 333 | qttag != MAKEID('s', 't', 'c', 'o') && 334 | qttag != MAKEID('c', 'o', '6', '4') && 335 | qttag != MAKEID('h', 'd', 'l', 'r') && 336 | qttag != MAKEID('e', 'd', 't', 's')) 337 | { 338 | LongSeek(mp4, qtsize - 8); 339 | 340 | NESTSIZE(qtsize); 341 | } 342 | else 343 | { 344 | 345 | if (qttag == MAKEID('m', 'v', 'h', 'd')) //mvhd movie header 346 | { 347 | len = fread(&skip, 1, 4, mp4->mediafp); 348 | len += fread(&mp4->movie_creation_time, 1, 4, mp4->mediafp); 349 | mp4->movie_creation_time = BYTESWAP32(mp4->movie_creation_time); 350 | len += fread(&skip, 1, 4, mp4->mediafp); 351 | len += fread(&mp4->clockdemon, 1, 4, mp4->mediafp); 352 | mp4->clockdemon = BYTESWAP32(mp4->clockdemon); 353 | len += fread(&mp4->clockcount, 1, 4, mp4->mediafp); 354 | mp4->clockcount = BYTESWAP32(mp4->clockcount); 355 | 356 | mp4->filepos += len; 357 | LongSeek(mp4, qtsize - 8 - len); // skip over mvhd 358 | 359 | NESTSIZE(qtsize); 360 | } 361 | else if (qttag == MAKEID('t', 'r', 'a', 'k')) //trak header 362 | { 363 | 364 | if (mp4->trak_num + 1 < MAX_TRACKS) 365 | mp4->trak_num++; 366 | 367 | NESTSIZE(8); 368 | } 369 | else if (qttag == MAKEID('m', 'd', 'h', 'd')) //mdhd media header 370 | { 371 | media_header md; 372 | len = fread(&md, 1, sizeof(md), mp4->mediafp); 373 | if (len == sizeof(md)) 374 | { 375 | md.creation_time = BYTESWAP32(md.creation_time); 376 | md.modification_time = BYTESWAP32(md.modification_time); 377 | md.time_scale = BYTESWAP32(md.time_scale); 378 | md.duration = BYTESWAP32(md.duration); 379 | 380 | mp4->trak_clockdemon = md.time_scale; 381 | mp4->trak_clockcount = md.duration; 382 | 383 | if (mp4->trak_clockdemon == 0 || mp4->trak_clockcount == 0) 384 | { 385 | CloseSource((size_t)mp4); 386 | mp4 = NULL; 387 | break; 388 | } 389 | 390 | if (mp4->videolength == 0.0) // Get the video length from the first track 391 | { 392 | mp4->videolength = (float)((double)mp4->trak_clockcount / (double)mp4->trak_clockdemon); 393 | } 394 | } 395 | 396 | mp4->filepos += len; 397 | LongSeek(mp4, qtsize - 8 - len); // skip over mvhd 398 | 399 | NESTSIZE(qtsize); 400 | } 401 | else if (qttag == MAKEID('h', 'd', 'l', 'r')) //hldr 402 | { 403 | uint32_t temp; 404 | len = fread(&skip, 1, 4, mp4->mediafp); 405 | len += fread(&skip, 1, 4, mp4->mediafp); 406 | len += fread(&temp, 1, 4, mp4->mediafp); // type will be 'meta' for the correct trak. 407 | 408 | if (temp != MAKEID('a', 'l', 'i', 's') && temp != MAKEID('u', 'r', 'l', ' ')) 409 | type = temp; 410 | 411 | mp4->filepos += len; 412 | LongSeek(mp4, qtsize - 8 - len); // skip over hldr 413 | 414 | NESTSIZE(qtsize); 415 | } 416 | else if (qttag == MAKEID('e', 'd', 't', 's')) //edit list 417 | { 418 | uint32_t elst, temp, readnum, i; 419 | len = fread(&skip, 1, 4, mp4->mediafp); 420 | len += fread(&elst, 1, 4, mp4->mediafp); 421 | if (elst == MAKEID('e', 'l', 's', 't')) 422 | { 423 | len += fread(&temp, 1, 4, mp4->mediafp); 424 | if (temp == 0) 425 | { 426 | len += fread(&readnum, 1, 4, mp4->mediafp); 427 | readnum = BYTESWAP32(readnum); 428 | // printf("num_of_entries: %d \t size: %d\n", readnum, qtsize); 429 | if (readnum <= (qtsize / 12) && mp4->trak_clockdemon) 430 | { 431 | uint32_t segment_duration; //integer that specifies the duration of this edit segment in units of the movieÂs time scale. 432 | uint32_t segment_mediaTime; //integer containing the starting time within the media of this edit segment(in media timescale units).If this field is set to Â1, it is an empty edit.The last edit in a track should never be an empty edit.Any difference between the movieÂs duration and the trackÂs duration is expressed as an implicit empty edit. 433 | uint32_t segment_mediaRate; //point number that specifies the relative rate at which to play the media corresponding to this edit segment.This rate value cannot be 0 or negative. 434 | for (i = 0; i < readnum; i++) 435 | { 436 | len += fread(&segment_duration, 1, 4, mp4->mediafp); 437 | len += fread(&segment_mediaTime, 1, 4, mp4->mediafp); 438 | len += fread(&segment_mediaRate, 1, 4, mp4->mediafp); 439 | 440 | segment_duration = BYTESWAP32(segment_duration); // in MP4 clock base 441 | segment_mediaTime = BYTESWAP32(segment_mediaTime); // in trak clock base 442 | segment_mediaRate = BYTESWAP32(segment_mediaRate); // Fixed-point 65536 = 1.0X 443 | 444 | if (segment_mediaTime == 0xffffffff) // the segment_duration for blanked time 445 | mp4->trak_edit_list_offsets[mp4->trak_num] += (int32_t)segment_duration; //samples are delay, data starts after presentation time zero. 446 | else if (i == 0) // If the first editlst starts after zero, the track is offset by this time (time before presentation time zero.) 447 | mp4->trak_edit_list_offsets[mp4->trak_num] -= (int32_t)((double)segment_mediaTime / (double)mp4->trak_clockdemon * (double)mp4->clockdemon); //convert to MP4 clock base. 448 | } 449 | if (type == traktype) // GPMF metadata 450 | { 451 | mp4->metadataoffset_clockcount = mp4->trak_edit_list_offsets[mp4->trak_num]; //leave in MP4 clock base 452 | } 453 | } 454 | } 455 | } 456 | mp4->filepos += len; 457 | LongSeek(mp4, qtsize - 8 - len); // skip over edts 458 | 459 | NESTSIZE(qtsize); 460 | } 461 | else if (qttag == MAKEID('s', 't', 's', 'd')) //read the sample decription to determine the type of metadata 462 | { 463 | if (type == traktype) //like meta 464 | { 465 | len = fread(&skip, 1, 4, mp4->mediafp); 466 | len += fread(&skip, 1, 4, mp4->mediafp); 467 | len += fread(&skip, 1, 4, mp4->mediafp); 468 | len += fread(&subtype, 1, 4, mp4->mediafp); // type will be 'meta' for the correct trak. 469 | if (len == 16) 470 | { 471 | if (subtype != traksubtype) // not MP4 metadata 472 | { 473 | type = 0; // MP4 474 | } 475 | } 476 | mp4->filepos += len; 477 | LongSeek(mp4, qtsize - 8 - len); // skip over stsd 478 | } 479 | else 480 | LongSeek(mp4, qtsize - 8); 481 | 482 | NESTSIZE(qtsize); 483 | } 484 | else if (qttag == MAKEID('s', 't', 's', 'c')) // metadata stsc - offset chunks 485 | { 486 | if (type == traktype) // meta 487 | { 488 | len = fread(&skip, 1, 4, mp4->mediafp); 489 | len += fread(&num, 1, 4, mp4->mediafp); 490 | 491 | num = BYTESWAP32(num); 492 | if (num <= (qtsize / sizeof(SampleToChunk))) 493 | { 494 | mp4->metastsc_count = num; 495 | if (mp4->metastsc) 496 | { 497 | free(mp4->metastsc); 498 | mp4->metastsc = 0; 499 | } 500 | if (num > 0 && qtsize > (num * sizeof(SampleToChunk))) 501 | { 502 | mp4->metastsc = (SampleToChunk *)malloc(num * sizeof(SampleToChunk)); 503 | if (mp4->metastsc) 504 | { 505 | len += fread(mp4->metastsc, 1, num * sizeof(SampleToChunk), mp4->mediafp); 506 | 507 | do 508 | { 509 | num--; 510 | mp4->metastsc[num].chunk_num = BYTESWAP32(mp4->metastsc[num].chunk_num); 511 | mp4->metastsc[num].samples = BYTESWAP32(mp4->metastsc[num].samples); 512 | mp4->metastsc[num].id = BYTESWAP32(mp4->metastsc[num].id); 513 | } while (num > 0); 514 | } 515 | } 516 | else 517 | { 518 | //size of null 519 | CloseSource((size_t)mp4); 520 | mp4 = NULL; 521 | break; 522 | } 523 | } 524 | mp4->filepos += len; 525 | LongSeek(mp4, qtsize - 8 - len); // skip over stsx 526 | } 527 | else 528 | LongSeek(mp4, qtsize - 8); 529 | 530 | NESTSIZE(qtsize); 531 | } 532 | else if (qttag == MAKEID('s', 't', 's', 'z')) // metadata stsz - sizes 533 | { 534 | if (type == traktype) // meta 535 | { 536 | uint32_t equalsamplesize; 537 | 538 | len = fread(&skip, 1, 4, mp4->mediafp); 539 | len += fread(&equalsamplesize, 1, 4, mp4->mediafp); 540 | len += fread(&num, 1, 4, mp4->mediafp); 541 | 542 | num = BYTESWAP32(num); 543 | // if equalsamplesize != 0, it is the size of all the samples and the length should be 20 (size,fourcc,flags,samplesize,samplecount) 544 | if (qtsize >= (20 + (num * sizeof(uint32_t))) || (equalsamplesize != 0 && qtsize == 20)) 545 | { 546 | if (mp4->metasizes) 547 | { 548 | free(mp4->metasizes); 549 | mp4->metasizes = 0; 550 | } 551 | //either the samples are different sizes or they are all the same size 552 | if (num > 0) 553 | { 554 | mp4->metasize_count = num; 555 | mp4->metasizes = (uint32_t *)malloc(num * 4); 556 | if (mp4->metasizes) 557 | { 558 | if (equalsamplesize == 0) 559 | { 560 | len += fread(mp4->metasizes, 1, num * 4, mp4->mediafp); 561 | do 562 | { 563 | num--; 564 | mp4->metasizes[num] = BYTESWAP32(mp4->metasizes[num]); 565 | } while (num > 0); 566 | } 567 | else 568 | { 569 | equalsamplesize = BYTESWAP32(equalsamplesize); 570 | do 571 | { 572 | num--; 573 | mp4->metasizes[num] = equalsamplesize; 574 | } while (num > 0); 575 | } 576 | } 577 | } 578 | else 579 | { 580 | //size of null 581 | CloseSource((size_t)mp4); 582 | mp4 = NULL; 583 | break; 584 | } 585 | } 586 | mp4->filepos += len; 587 | LongSeek(mp4, qtsize - 8 - len); // skip over stsz 588 | } 589 | else 590 | LongSeek(mp4, qtsize - 8); 591 | 592 | NESTSIZE(qtsize); 593 | } 594 | else if (qttag == MAKEID('s', 't', 'c', 'o')) // metadata stco - offsets 595 | { 596 | if (type == traktype) // meta 597 | { 598 | len = fread(&skip, 1, 4, mp4->mediafp); 599 | len += fread(&num, 1, 4, mp4->mediafp); 600 | num = BYTESWAP32(num); 601 | if (num <= ((qtsize - 8 - len) / sizeof(uint32_t))) 602 | { 603 | mp4->metastco_count = num; 604 | 605 | if (mp4->metastsc_count > 0 && num != mp4->metasize_count) 606 | { 607 | if (mp4->metaoffsets) 608 | { 609 | free(mp4->metaoffsets); 610 | mp4->metaoffsets = 0; 611 | } 612 | if (mp4->metastco_count && qtsize > (num * 4)) 613 | { 614 | mp4->metaoffsets = (uint64_t *)malloc(mp4->metasize_count * 8); 615 | if (mp4->metaoffsets) 616 | { 617 | uint32_t *metaoffsets32 = NULL; 618 | metaoffsets32 = (uint32_t *)malloc(num * 4); 619 | if (metaoffsets32) 620 | { 621 | uint64_t fileoffset = 0; 622 | int stsc_pos = 0; 623 | int stco_pos = 0; 624 | len += fread(metaoffsets32, 1, num * 4, mp4->mediafp); 625 | do 626 | { 627 | num--; 628 | metaoffsets32[num] = BYTESWAP32(metaoffsets32[num]); 629 | } while (num > 0); 630 | 631 | fileoffset = metaoffsets32[stco_pos]; 632 | mp4->metaoffsets[0] = fileoffset; 633 | 634 | num = 1; 635 | while (num < mp4->metasize_count) 636 | { 637 | if (num != mp4->metastsc[stsc_pos].chunk_num - 1 && 0 == (num - (mp4->metastsc[stsc_pos].chunk_num - 1)) % mp4->metastsc[stsc_pos].samples) 638 | { 639 | stco_pos++; 640 | fileoffset = (uint64_t)metaoffsets32[stco_pos]; 641 | } 642 | else 643 | { 644 | fileoffset += (uint64_t)mp4->metasizes[num - 1]; 645 | } 646 | 647 | mp4->metaoffsets[num] = fileoffset; 648 | //int delta = metaoffsets[num] - metaoffsets[num - 1]; 649 | //printf("%3d:%08x, delta = %08x\n", num, (int)fileoffset, delta); 650 | 651 | num++; 652 | } 653 | 654 | if (mp4->metastsc) 655 | free(mp4->metastsc); 656 | mp4->metastsc = NULL; 657 | mp4->metastsc_count = 0; 658 | 659 | free(metaoffsets32); 660 | } 661 | } 662 | } 663 | else 664 | { 665 | //size of null 666 | CloseSource((size_t)mp4); 667 | mp4 = NULL; 668 | break; 669 | } 670 | } 671 | else 672 | { 673 | if (mp4->metaoffsets) 674 | { 675 | free(mp4->metaoffsets); 676 | mp4->metaoffsets = 0; 677 | } 678 | if (num > 0 && mp4->metasize_count && mp4->metasizes && qtsize > (num * 4)) 679 | { 680 | mp4->metaoffsets = (uint64_t *)malloc(num * 8); 681 | if (mp4->metaoffsets) 682 | { 683 | uint32_t *metaoffsets32 = NULL; 684 | metaoffsets32 = (uint32_t *)malloc(num * 4); 685 | if (metaoffsets32) 686 | { 687 | size_t readlen = fread(metaoffsets32, 1, num * 4, mp4->mediafp); 688 | len += readlen; 689 | do 690 | { 691 | num--; 692 | mp4->metaoffsets[num] = BYTESWAP32(metaoffsets32[num]); 693 | } while (num > 0); 694 | 695 | free(metaoffsets32); 696 | } 697 | } 698 | } 699 | else 700 | { 701 | //size of null 702 | CloseSource((size_t)mp4); 703 | mp4 = NULL; 704 | break; 705 | } 706 | } 707 | } 708 | mp4->filepos += len; 709 | LongSeek(mp4, qtsize - 8 - len); // skip over stco 710 | } 711 | else 712 | LongSeek(mp4, qtsize - 8); 713 | 714 | NESTSIZE(qtsize); 715 | } 716 | else if (qttag == MAKEID('c', 'o', '6', '4')) // metadata stco - offsets 717 | { 718 | if (type == traktype) // meta 719 | { 720 | len = fread(&skip, 1, 4, mp4->mediafp); 721 | len += fread(&num, 1, 4, mp4->mediafp); 722 | num = BYTESWAP32(num); 723 | 724 | if (num == 0) 725 | { 726 | //size of null 727 | CloseSource((size_t)mp4); 728 | mp4 = NULL; 729 | break; 730 | } 731 | 732 | if (num <= ((qtsize - 8 - len) / sizeof(uint64_t))) 733 | { 734 | mp4->metastco_count = num; 735 | 736 | if (mp4->metastsc_count > 0 && num != mp4->metasize_count) 737 | { 738 | if (mp4->metaoffsets) 739 | { 740 | free(mp4->metaoffsets); 741 | mp4->metaoffsets = 0; 742 | } 743 | if (mp4->metasize_count && mp4->metasizes && qtsize > (num * 8)) 744 | { 745 | mp4->metaoffsets = (uint64_t *)malloc(mp4->metasize_count * 8); 746 | if (mp4->metaoffsets) 747 | { 748 | uint64_t *metaoffsets64 = NULL; 749 | metaoffsets64 = (uint64_t *)malloc(num * 8); 750 | if (metaoffsets64) 751 | { 752 | uint64_t fileoffset = 0; 753 | int stsc_pos = 0; 754 | int stco_pos = 0; 755 | len += fread(metaoffsets64, 1, num * 8, mp4->mediafp); 756 | do 757 | { 758 | num--; 759 | metaoffsets64[num] = BYTESWAP64(metaoffsets64[num]); 760 | } while (num > 0); 761 | 762 | fileoffset = metaoffsets64[0]; 763 | mp4->metaoffsets[0] = fileoffset; 764 | 765 | num = 1; 766 | while (num < mp4->metasize_count) 767 | { 768 | if (num != mp4->metastsc[stsc_pos].chunk_num - 1 && 0 == (num - (mp4->metastsc[stsc_pos].chunk_num - 1)) % mp4->metastsc[stsc_pos].samples) 769 | { 770 | stco_pos++; 771 | fileoffset = (uint64_t)metaoffsets64[stco_pos]; 772 | } 773 | else 774 | { 775 | fileoffset += (uint64_t)mp4->metasizes[num - 1]; 776 | } 777 | 778 | mp4->metaoffsets[num] = fileoffset; 779 | //int delta = metaoffsets[num] - metaoffsets[num - 1]; 780 | //printf("%3d:%08x, delta = %08x\n", num, (int)fileoffset, delta); 781 | 782 | num++; 783 | } 784 | 785 | if (mp4->metastsc) 786 | free(mp4->metastsc); 787 | mp4->metastsc = NULL; 788 | mp4->metastsc_count = 0; 789 | 790 | free(metaoffsets64); 791 | } 792 | } 793 | } 794 | else 795 | { 796 | //size of null 797 | CloseSource((size_t)mp4); 798 | mp4 = NULL; 799 | break; 800 | } 801 | } 802 | else 803 | { 804 | if (mp4->metaoffsets) 805 | { 806 | free(mp4->metaoffsets); 807 | mp4->metaoffsets = 0; 808 | } 809 | if (num > 0 && mp4->metasize_count && mp4->metasizes && qtsize > (num * 8)) 810 | { 811 | mp4->metaoffsets = (uint64_t *)malloc(num * 8); 812 | if (mp4->metaoffsets) 813 | { 814 | len += fread(mp4->metaoffsets, 1, num * 8, mp4->mediafp); 815 | do 816 | { 817 | num--; 818 | mp4->metaoffsets[num] = BYTESWAP64(mp4->metaoffsets[num]); 819 | } while (num > 0); 820 | } 821 | } 822 | } 823 | } 824 | mp4->filepos += len; 825 | LongSeek(mp4, qtsize - 8 - len); // skip over stco 826 | } 827 | else 828 | LongSeek(mp4, qtsize - 8); 829 | 830 | NESTSIZE(qtsize); 831 | } 832 | else if (qttag == MAKEID('s', 't', 't', 's')) // time to samples 833 | { 834 | if (type == MAKEID('v', 'i', 'd', 'e')) // video trak to get frame rate 835 | { 836 | uint32_t samples = 0; 837 | uint32_t entries = 0; 838 | len = fread(&skip, 1, 4, mp4->mediafp); 839 | len += fread(&num, 1, 4, mp4->mediafp); 840 | num = BYTESWAP32(num); 841 | 842 | if (num <= (qtsize / 8)) 843 | { 844 | entries = num; 845 | 846 | while (entries > 0) 847 | { 848 | uint32_t samplecount; 849 | uint32_t duration; 850 | len += fread(&samplecount, 1, 4, mp4->mediafp); 851 | samplecount = BYTESWAP32(samplecount); 852 | len += fread(&duration, 1, 4, mp4->mediafp); 853 | duration = BYTESWAP32(duration); 854 | 855 | samples += samplecount; 856 | entries--; 857 | 858 | if (mp4->video_framerate_numerator == 0) 859 | { 860 | mp4->video_framerate_numerator = mp4->trak_clockdemon; 861 | mp4->video_framerate_denominator = duration; 862 | } 863 | } 864 | mp4->video_frames = samples; 865 | } 866 | mp4->filepos += len; 867 | LongSeek(mp4, qtsize - 8 - len); // skip over stco 868 | } 869 | else if (type == traktype) // meta 870 | { 871 | uint32_t totaldur = 0, samples = 0; 872 | uint32_t entries = 0; 873 | len = fread(&skip, 1, 4, mp4->mediafp); 874 | len += fread(&num, 1, 4, mp4->mediafp); 875 | num = BYTESWAP32(num); 876 | if (num <= (qtsize / 8)) 877 | { 878 | entries = num; 879 | 880 | mp4->meta_clockdemon = mp4->trak_clockdemon; 881 | mp4->meta_clockcount = mp4->trak_clockcount; 882 | 883 | if (mp4->meta_clockdemon == 0) 884 | { 885 | //prevent divide by zero 886 | CloseSource((size_t)mp4); 887 | mp4 = NULL; 888 | break; 889 | } 890 | 891 | while (entries > 0) 892 | { 893 | uint32_t samplecount; 894 | uint32_t duration; 895 | len += fread(&samplecount, 1, 4, mp4->mediafp); 896 | samplecount = BYTESWAP32(samplecount); 897 | len += fread(&duration, 1, 4, mp4->mediafp); 898 | duration = BYTESWAP32(duration); 899 | 900 | samples += samplecount; 901 | entries--; 902 | 903 | totaldur += duration; 904 | mp4->metadatalength += (double)((double)samplecount * (double)duration / (double)mp4->meta_clockdemon); 905 | if (samplecount > 1 || num == 1) 906 | mp4->basemetadataduration = mp4->metadatalength * (double)mp4->meta_clockdemon / (double)samples; 907 | } 908 | } 909 | mp4->filepos += len; 910 | LongSeek(mp4, qtsize - 8 - len); // skip over stco 911 | } 912 | else 913 | LongSeek(mp4, qtsize - 8); 914 | 915 | NESTSIZE(qtsize); 916 | } 917 | else 918 | { 919 | NESTSIZE(8); 920 | } 921 | } 922 | } 923 | else 924 | { 925 | break; 926 | } 927 | } while (len > 0); 928 | 929 | if (mp4) 930 | { 931 | if (mp4->metasizes == NULL || mp4->metaoffsets == NULL) 932 | { 933 | CloseSource((size_t)mp4); 934 | mp4 = NULL; 935 | } 936 | 937 | // set the numbers of payload with both size and offset 938 | if (mp4 != NULL) 939 | { 940 | mp4->indexcount = mp4->metasize_count; 941 | } 942 | } 943 | } 944 | else 945 | { 946 | // printf("Could not open %s for input\n", filename); 947 | // exit(1); 948 | 949 | free(mp4); 950 | mp4 = NULL; 951 | } 952 | 953 | return (size_t)mp4; 954 | } 955 | 956 | float GetDuration(size_t handle) 957 | { 958 | mp4object *mp4 = (mp4object *)handle; 959 | if (mp4 == NULL) 960 | return 0.0; 961 | 962 | return (float)mp4->metadatalength; 963 | } 964 | 965 | uint32_t getCreationtime(size_t handle) 966 | { 967 | mp4object *mp4 = (mp4object *)handle; 968 | if (mp4 == NULL) 969 | return 0; 970 | 971 | return mp4->movie_creation_time; 972 | } 973 | 974 | uint32_t GetVideoFrameRateAndCount(size_t handle, uint32_t *numer, uint32_t *demon) 975 | { 976 | mp4object *mp4 = (mp4object *)handle; 977 | if (mp4 == NULL) 978 | return 0; 979 | 980 | if (numer != NULL && demon != NULL && mp4->video_frames > 0) 981 | { 982 | *numer = mp4->video_framerate_numerator; 983 | *demon = mp4->video_framerate_denominator; 984 | return mp4->video_frames; 985 | } 986 | return 0; 987 | } 988 | 989 | void CloseSource(size_t handle) 990 | { 991 | mp4object *mp4 = (mp4object *)handle; 992 | if (mp4 == NULL) 993 | { 994 | return; 995 | } 996 | 997 | if (mp4->mediafp) 998 | { 999 | fclose(mp4->mediafp); 1000 | mp4->mediafp = NULL; 1001 | } 1002 | if (mp4->metasizes) 1003 | { 1004 | free(mp4->metasizes); 1005 | mp4->metasizes = 0; 1006 | } 1007 | if (mp4->metaoffsets) 1008 | { 1009 | free(mp4->metaoffsets); 1010 | mp4->metaoffsets = 0; 1011 | } 1012 | if (mp4->metastsc) 1013 | { 1014 | free(mp4->metastsc); 1015 | mp4->metastsc = 0; 1016 | } 1017 | 1018 | free(mp4); 1019 | } 1020 | 1021 | uint32_t GetPayloadTime(size_t handle, uint32_t index, double *in, double *out) 1022 | { 1023 | mp4object *mp4 = (mp4object *)handle; 1024 | if (mp4 == NULL) 1025 | return MP4_ERROR_MEMORY; 1026 | 1027 | if (mp4->metaoffsets == 0 || mp4->basemetadataduration == 0 || mp4->meta_clockdemon == 0 || in == NULL || out == NULL) 1028 | return MP4_ERROR_MEMORY; 1029 | 1030 | *in = ((double)index * (double)mp4->basemetadataduration / (double)mp4->meta_clockdemon); 1031 | *out = ((double)(index + 1) * (double)mp4->basemetadataduration / (double)mp4->meta_clockdemon); 1032 | 1033 | if (*out > (double)mp4->metadatalength) 1034 | *out = (double)mp4->metadatalength; 1035 | 1036 | // Add any Edit List offset 1037 | *in += (double)mp4->metadataoffset_clockcount / (double)mp4->clockdemon; 1038 | *out += (double)mp4->metadataoffset_clockcount / (double)mp4->clockdemon; 1039 | return MP4_ERROR_OK; 1040 | } 1041 | 1042 | uint32_t GetPayloadRationalTime(size_t handle, uint32_t index, int32_t *in_numerator, int32_t *out_numerator, uint32_t *denominator) 1043 | { 1044 | mp4object *mp4 = (mp4object *)handle; 1045 | if (mp4 == NULL) 1046 | return MP4_ERROR_MEMORY; 1047 | 1048 | if (mp4->metaoffsets == 0 || mp4->basemetadataduration == 0 || mp4->meta_clockdemon == 0 || in_numerator == NULL || out_numerator == NULL) 1049 | return MP4_ERROR_MEMORY; 1050 | 1051 | *in_numerator = (int32_t)(index * mp4->basemetadataduration); 1052 | *out_numerator = (int32_t)((index + 1) * mp4->basemetadataduration); 1053 | 1054 | if (*out_numerator > (int32_t)((double)mp4->metadatalength * (double)mp4->meta_clockdemon)) 1055 | *out_numerator = (int32_t)((double)mp4->metadatalength * (double)mp4->meta_clockdemon); 1056 | 1057 | // Add any Edit List offset 1058 | *in_numerator += (int32_t)(((double)mp4->metadataoffset_clockcount / (double)mp4->clockdemon) * mp4->meta_clockdemon); 1059 | *out_numerator += (int32_t)(((double)mp4->metadataoffset_clockcount / (double)mp4->clockdemon) * mp4->meta_clockdemon); 1060 | 1061 | *denominator = mp4->meta_clockdemon; 1062 | 1063 | return MP4_ERROR_OK; 1064 | } 1065 | 1066 | uint32_t GetEditListOffset(size_t handle, double *offset) 1067 | { 1068 | mp4object *mp4 = (mp4object *)handle; 1069 | if (mp4 == NULL) 1070 | return MP4_ERROR_MEMORY; 1071 | 1072 | if (mp4->clockdemon == 0) 1073 | return MP4_ERROR_MEMORY; 1074 | 1075 | *offset = (double)mp4->metadataoffset_clockcount / (double)mp4->clockdemon; 1076 | 1077 | return MP4_ERROR_OK; 1078 | } 1079 | 1080 | uint32_t GetEditListOffsetRationalTime(size_t handle, int32_t *offset_numerator, uint32_t *denominator) 1081 | { 1082 | mp4object *mp4 = (mp4object *)handle; 1083 | if (mp4 == NULL) 1084 | return MP4_ERROR_MEMORY; 1085 | 1086 | if (mp4->clockdemon == 0) 1087 | return MP4_ERROR_MEMORY; 1088 | 1089 | *offset_numerator = mp4->metadataoffset_clockcount; 1090 | *denominator = mp4->clockdemon; 1091 | 1092 | return MP4_ERROR_OK; 1093 | } 1094 | 1095 | size_t OpenMP4SourceUDTA(char *filename, int32_t flags) 1096 | { 1097 | mp4object *mp4 = (mp4object *)malloc(sizeof(mp4object)); 1098 | if (mp4 == NULL) 1099 | return 0; 1100 | 1101 | memset(mp4, 0, sizeof(mp4object)); 1102 | 1103 | #ifdef _WINDOWS 1104 | struct _stat64 mp4stat; 1105 | _stat64(filename, &mp4stat); 1106 | #else 1107 | struct stat mp4stat; 1108 | stat(filename, &mp4stat); 1109 | #endif 1110 | mp4->filesize = (uint64_t)mp4stat.st_size; 1111 | if (mp4->filesize < 64) 1112 | { 1113 | free(mp4); 1114 | return 0; 1115 | } 1116 | 1117 | const char *mode = (flags & MP4_FLAG_READ_WRITE_MODE) ? "rb+" : "rb"; 1118 | #ifdef _WINDOWS 1119 | fopen_s(&mp4->mediafp, filename, mode); 1120 | #else 1121 | mp4->mediafp = fopen(filename, mode); 1122 | #endif 1123 | 1124 | if (mp4->mediafp) 1125 | { 1126 | uint32_t qttag, qtsize32; 1127 | size_t len; 1128 | int32_t nest = 0; 1129 | uint64_t nestsize[MAX_NEST_LEVEL] = {0}; 1130 | uint64_t lastsize = 0, qtsize; 1131 | 1132 | do 1133 | { 1134 | len = fread(&qtsize32, 1, 4, mp4->mediafp); 1135 | len += fread(&qttag, 1, 4, mp4->mediafp); 1136 | if (len == 8) 1137 | { 1138 | if (!VALID_FOURCC(qttag) && qttag != 0x7a7978a9) 1139 | { 1140 | mp4->filepos += len; 1141 | LongSeek(mp4, lastsize - 8 - len); 1142 | 1143 | NESTSIZE(lastsize - 8); 1144 | continue; 1145 | } 1146 | 1147 | qtsize32 = BYTESWAP32(qtsize32); 1148 | 1149 | if (qtsize32 == 1) // 64-bit Atom 1150 | { 1151 | len += fread(&qtsize, 1, 8, mp4->mediafp); 1152 | mp4->filepos += len; 1153 | qtsize = BYTESWAP64(qtsize) - 8; 1154 | } 1155 | else 1156 | qtsize = qtsize32; 1157 | 1158 | nest++; 1159 | 1160 | if (qtsize < 8) 1161 | break; 1162 | if (nest >= MAX_NEST_LEVEL) 1163 | break; 1164 | if (nest > 1 && qtsize > nestsize[nest - 1]) 1165 | break; 1166 | 1167 | nestsize[nest] = qtsize; 1168 | lastsize = qtsize; 1169 | 1170 | if (qttag == MAKEID('m', 'd', 'a', 't') || 1171 | qttag == MAKEID('f', 't', 'y', 'p')) 1172 | { 1173 | LongSeek(mp4, qtsize - 8); 1174 | NESTSIZE(qtsize); 1175 | continue; 1176 | } 1177 | 1178 | if (qttag == MAKEID('G', 'P', 'M', 'F')) 1179 | { 1180 | mp4->videolength += 1.0; 1181 | mp4->metadatalength += 1.0; 1182 | 1183 | mp4->indexcount = (uint32_t)mp4->metadatalength; 1184 | 1185 | mp4->metasizes = (uint32_t *)malloc(mp4->indexcount * 4 + 4); 1186 | memset(mp4->metasizes, 0, mp4->indexcount * 4 + 4); 1187 | mp4->metaoffsets = (uint64_t *)malloc(mp4->indexcount * 8 + 8); 1188 | memset(mp4->metaoffsets, 0, mp4->indexcount * 8 + 8); 1189 | 1190 | mp4->basemetadataduration = 1.0; 1191 | mp4->meta_clockdemon = 1; 1192 | 1193 | mp4->metasizes[0] = (uint32_t)qtsize - 8; 1194 | mp4->metaoffsets[0] = (uint64_t)LONGTELL(mp4->mediafp); 1195 | mp4->metasize_count = 1; 1196 | 1197 | return (size_t)mp4; // not an MP4, RAW GPMF which has not inherent timing, assigning a during of 1second. 1198 | } 1199 | if (qttag != MAKEID('m', 'o', 'o', 'v') && //skip over all but these atoms 1200 | qttag != MAKEID('u', 'd', 't', 'a')) 1201 | { 1202 | LongSeek(mp4, qtsize - 8); 1203 | NESTSIZE(qtsize); 1204 | continue; 1205 | } 1206 | else 1207 | { 1208 | NESTSIZE(8); 1209 | } 1210 | } 1211 | } while (len > 0); 1212 | } 1213 | return (size_t)mp4; 1214 | } 1215 | --------------------------------------------------------------------------------