Added in/extrinsics into Datum

doc/release_notes.md

@@ -215,7 +215,8 @@ OpenPose Library - Release Notes
 1. Main improvements:
     1. Flir cameras: Added software trigger and a dedicated thread to keep reading images to remove latency (analogously to webcamReader).
     2. 3-D reconstruction: Added non-linear minimization to further improve 3-D triangulation accuracy by ~5% (Ubuntu only).
-    2. CMake: All libraries as single variable (simpler to add/remove libraries).
+    3. CMake: All libraries as single variable (simpler to add/remove libraries).
+    4. Datum includes extrinsic and intrinsic camera parameters.
 2. Functions or parameters renamed:
     1. Removed scale parameter from hand and face rectangle extractor (causing wrong results if custom `--output_resolution`).
 3. Main bugs fixed:

include/openpose/3d/cameraParameterReader.hpp

@@ -18,6 +18,8 @@ namespace op
 
         const std::vector<cv::Mat>& getCameraMatrices() const;
 
+        const std::vector<cv::Mat>& getCameraExtrinsics() const;
+
         const std::vector<cv::Mat>& getCameraIntrinsics() const;
 
         const std::vector<cv::Mat>& getCameraDistortions() const;
@@ -26,6 +28,7 @@ namespace op
         std::vector<std::string> mSerialNumbers;
         unsigned long long mNumberCameras;
         std::vector<cv::Mat> mCameraMatrices;
+        std::vector<cv::Mat> mCameraExtrinsics;
         std::vector<cv::Mat> mCameraIntrinsics;
         std::vector<cv::Mat> mCameraDistortions;
 

include/openpose/core/datum.hpp

@@ -181,10 +181,20 @@ namespace op
         std::array<Array<float>, 2> handKeypoints3D;
 
         /**
-         * 3x4 camera matrix of the camera.
+         * 3x4 camera matrix of the camera (equivalent to cameraIntrinsics * cameraExtrinsics).
          */
         cv::Mat cameraMatrix;
 
+        /**
+         * 3x4 extrinsic parameters of the camera.
+         */
+        cv::Mat cameraExtrinsics;
+
+        /**
+         * 3x3 intrinsic parameters of the camera.
+         */
+        cv::Mat cameraIntrinsics;
+
         // ---------------------------------------- Other (internal) parameters ---------------------------------------- //
         /**
          * Scale ratio between the input Datum::cvInputData and the net input size.

include/openpose/producer/datumProducer.hpp

@@ -100,6 +100,8 @@ namespace op
                 auto nextFrameNumber = (unsigned long long)spProducer->get(CV_CAP_PROP_POS_FRAMES);
                 auto cvMats = spProducer->getFrames();
                 auto cameraMatrices = spProducer->getCameraMatrices();
+                auto cameraExtrinsics = spProducer->getCameraExtrinsics();
+                auto cameraIntrinsics = spProducer->getCameraIntrinsics();
                 // Check frames are not empty
                 checkIfTooManyConsecutiveEmptyFrames(mNumberConsecutiveEmptyFrames, cvMats.empty() || cvMats[0].empty());
                 if (!cvMats.empty())
@@ -112,7 +114,11 @@ namespace op
                     datum.frameNumber = nextFrameNumber;
                     datum.cvInputData = cvMats[0];
                     if (!cameraMatrices.empty())
+                    {
                         datum.cameraMatrix = cameraMatrices[0];
+                        datum.cameraExtrinsics = cameraExtrinsics[0];
+                        datum.cameraIntrinsics = cameraIntrinsics[0];
+                    }
                     // Image integrity
                     if (datum.cvInputData.channels() != 3)
                     {
@@ -139,7 +145,11 @@ namespace op
                             datumI.cvInputData = cvMats[i];
                             datumI.cvOutputData = datumI.cvInputData;
                             if (cameraMatrices.size() > i)
+                            {
                                 datumI.cameraMatrix = cameraMatrices[i];
+                                datumI.cameraExtrinsics = cameraExtrinsics[i];
+                                datumI.cameraIntrinsics = cameraIntrinsics[i];
+                            }
                         }
                     }
                     // Check producer is running

include/openpose/producer/flirReader.hpp

@@ -24,6 +24,10 @@ namespace op
 
         std::vector<cv::Mat> getCameraMatrices();
 
+        std::vector<cv::Mat> getCameraExtrinsics();
+
+        std::vector<cv::Mat> getCameraIntrinsics();
+
         std::string getNextFrameName();
 
         bool isOpened() const;

include/openpose/producer/imageDirectoryReader.hpp

@@ -29,6 +29,10 @@ namespace op
 
         std::vector<cv::Mat> getCameraMatrices();
 
+        std::vector<cv::Mat> getCameraExtrinsics();
+
+        std::vector<cv::Mat> getCameraIntrinsics();
+
         std::string getNextFrameName();
 
         inline bool isOpened() const

include/openpose/producer/ipCameraReader.hpp

@@ -20,6 +20,10 @@ namespace op
 
         std::vector<cv::Mat> getCameraMatrices();
 
+        std::vector<cv::Mat> getCameraExtrinsics();
+
+        std::vector<cv::Mat> getCameraIntrinsics();
+
         std::string getNextFrameName();
 
         inline double get(const int capProperty)

include/openpose/producer/producer.hpp

@@ -47,6 +47,18 @@ namespace op
          */
         virtual std::vector<cv::Mat> getCameraMatrices() = 0;
 
+        /**
+         * It retrieves and returns the camera extrinsic parameters from the frames producer.
+         * @return std::vector<cv::Mat> with the camera extrinsic parameters.
+         */
+        virtual std::vector<cv::Mat> getCameraExtrinsics() = 0;
+
+        /**
+         * It retrieves and returns the camera intrinsic parameters from the frames producer.
+         * @return std::vector<cv::Mat> with the camera intrinsic parameters.
+         */
+        virtual std::vector<cv::Mat> getCameraIntrinsics() = 0;
+
         /**
          * This function returns a unique frame name (e.g. the frame number for video, the
          * frame counter for webcam, the image name for image directory reader, etc.).

include/openpose/producer/spinnakerWrapper.hpp

@@ -23,6 +23,10 @@ namespace op
 
         std::vector<cv::Mat> getCameraMatrices() const;
 
+        std::vector<cv::Mat> getCameraExtrinsics() const;
+
+        std::vector<cv::Mat> getCameraIntrinsics() const;
+
         Point<int> getResolution() const;
 
         bool isOpened() const;

include/openpose/producer/videoReader.hpp

@@ -28,6 +28,10 @@ namespace op
 
         std::vector<cv::Mat> getCameraMatrices();
 
+        std::vector<cv::Mat> getCameraExtrinsics();
+
+        std::vector<cv::Mat> getCameraIntrinsics();
+
         std::string getNextFrameName();
 
         double get(const int capProperty);

include/openpose/producer/webcamReader.hpp

@@ -31,6 +31,10 @@ namespace op
 
         std::vector<cv::Mat> getCameraMatrices();
 
+        std::vector<cv::Mat> getCameraExtrinsics();
+
+        std::vector<cv::Mat> getCameraIntrinsics();
+
         std::string getNextFrameName();
 
         double get(const int capProperty);

src/openpose/3d/cameraParameterReader.cpp

@@ -111,6 +111,7 @@ namespace op
             // Load parameters
             mNumberCameras = serialNumbers.size();
             mCameraMatrices.clear();
+            mCameraExtrinsics.clear();
             mCameraIntrinsics.clear();
             mCameraDistortions.clear();
             // log("Camera matrices:");
@@ -118,7 +119,6 @@ namespace op
             {
                 const auto parameterPath = fileNameNoExtension + mSerialNumbers.at(i);
                 const auto cameraParameters = loadData(cvMatNames, parameterPath, dataFormat);
-                const auto& intrinsics = cameraParameters.at(1);
                 // Error if empty element
                 if (cameraParameters.empty() || cameraParameters.at(0).empty()
                     || cameraParameters.at(1).empty() || cameraParameters.at(2).empty())
@@ -140,9 +140,10 @@ namespace op
                         error("Error at reading the camera distortion parameters" + errorMessage,
                               __LINE__, __FUNCTION__, __FILE__);
                 }
-                mCameraMatrices.emplace_back(intrinsics * cameraParameters.at(0));
-                mCameraIntrinsics.emplace_back(intrinsics);
+                mCameraExtrinsics.emplace_back(cameraParameters.at(0));
+                mCameraIntrinsics.emplace_back(cameraParameters.at(1));
                 mCameraDistortions.emplace_back(cameraParameters.at(2));
+                mCameraMatrices.emplace_back(mCameraIntrinsics.back() * mCameraExtrinsics.back());
                 // log(cameraParameters.at(0));
             }
             // // mCameraMatrices
@@ -190,6 +191,19 @@ namespace op
         }
     }
 
+    const std::vector<cv::Mat>& CameraParameterReader::getCameraExtrinsics() const
+    {
+        try
+        {
+            return mCameraExtrinsics;
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return mCameraExtrinsics;
+        }
+    }
+
     const std::vector<cv::Mat>& CameraParameterReader::getCameraIntrinsics() const
     {
         try

src/openpose/producer/flirReader.cpp

@@ -48,6 +48,32 @@ namespace op
         }
     }
 
+    std::vector<cv::Mat> FlirReader::getCameraExtrinsics()
+    {
+        try
+        {
+            return mSpinnakerWrapper.getCameraExtrinsics();
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
+    std::vector<cv::Mat> FlirReader::getCameraIntrinsics()
+    {
+        try
+        {
+            return mSpinnakerWrapper.getCameraIntrinsics();
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
     std::string FlirReader::getNextFrameName()
     {
         try

src/openpose/producer/imageDirectoryReader.cpp

@@ -80,6 +80,32 @@ namespace op
         }
     }
 
+    std::vector<cv::Mat> ImageDirectoryReader::getCameraExtrinsics()
+    {
+        try
+        {
+            return mCameraParameterReader.getCameraExtrinsics();
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
+    std::vector<cv::Mat> ImageDirectoryReader::getCameraIntrinsics()
+    {
+        try
+        {
+            return mCameraParameterReader.getCameraIntrinsics();
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
     std::string ImageDirectoryReader::getNextFrameName()
     {
         try

src/openpose/producer/ipCameraReader.cpp

@@ -25,6 +25,32 @@ namespace op
         }
     }
 
+    std::vector<cv::Mat> IpCameraReader::getCameraExtrinsics()
+    {
+        try
+        {
+            return {};
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
+    std::vector<cv::Mat> IpCameraReader::getCameraIntrinsics()
+    {
+        try
+        {
+            return {};
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
     std::string IpCameraReader::getNextFrameName()
     {
         try

src/openpose/producer/spinnakerWrapper.cpp

@@ -780,6 +780,40 @@ namespace op
         }
     }
 
+    std::vector<cv::Mat> SpinnakerWrapper::getCameraExtrinsics() const
+    {
+        try
+        {
+            #ifdef WITH_FLIR_CAMERA
+                return upImpl->mCameraParameterReader.getCameraExtrinsics();
+            #else
+                return {};
+            #endif
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
+    std::vector<cv::Mat> SpinnakerWrapper::getCameraIntrinsics() const
+    {
+        try
+        {
+            #ifdef WITH_FLIR_CAMERA
+                return upImpl->mCameraParameterReader.getCameraIntrinsics();
+            #else
+                return {};
+            #endif
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
     Point<int> SpinnakerWrapper::getResolution() const
     {
         try

src/openpose/producer/videoReader.cpp

@@ -52,6 +52,32 @@ namespace op
         }
     }
 
+    std::vector<cv::Mat> VideoReader::getCameraExtrinsics()
+    {
+        try
+        {
+            return mCameraParameterReader.getCameraExtrinsics();
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
+    std::vector<cv::Mat> VideoReader::getCameraIntrinsics()
+    {
+        try
+        {
+            return mCameraParameterReader.getCameraIntrinsics();
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
     std::string VideoReader::getNextFrameName()
     {
         try

src/openpose/producer/webcamReader.cpp

@@ -72,6 +72,32 @@ namespace op
         }
     }
 
+    std::vector<cv::Mat> WebcamReader::getCameraExtrinsics()
+    {
+        try
+        {
+            return {};
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
+    std::vector<cv::Mat> WebcamReader::getCameraIntrinsics()
+    {
+        try
+        {
+            return {};
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return {};
+        }
+    }
+
     std::string WebcamReader::getNextFrameName()
     {
         try