Added CameraMatrixInitial in the XML file

doc/release_notes.md

@@ -295,6 +295,9 @@ OpenPose Library - Release Notes
     25. Frame undistortion can be applied not only to FLIR cameras, but also to all other input sources (image, webcam, video, etc.).
     26. Calibration improvements:
         1. Improved chessboard orientation detection, more robust and less errors.
+        2. Triangulation functions (triangulate and triangulateWithOptimization) public, so calibration can use them for bundle adjustment.
+        3. Bundle adjustment refinement added for camera calibration.
+        4. Added `CameraMatrixInitial` field into the XML calibration files to keep the information of the original camera extrinsic parameters when bundle adjustment is run.
     27. Video with the 3D output can be saved with the new `--write_video_3d` flag.
 2. Functions or parameters renamed:
     1. By default, python example `tutorial_developer/python_2_pose_from_heatmaps.py` was using 2 scales starting at -1x736, changed to 1 scale at -1x368.

include/openpose/3d/cameraParameterReader.hpp

@@ -17,7 +17,8 @@ namespace op
         explicit CameraParameterReader(const std::string& serialNumber,
                                        const cv::Mat& cameraIntrinsics,
                                        const cv::Mat& cameraDistortion,
-                                       const cv::Mat& cameraExtrinsics = cv::Mat());
+                                       const cv::Mat& cameraExtrinsics = cv::Mat(),
+                                       const cv::Mat& cameraExtrinsicsInitial = cv::Mat());
 
         // serialNumbers is optional. If empty, it will load all the XML files available in the
         // cameraParameterPath folder
@@ -36,11 +37,13 @@ namespace op
 
         const std::vector<cv::Mat>& getCameraMatrices() const;
 
-        const std::vector<cv::Mat>& getCameraExtrinsics() const;
+        const std::vector<cv::Mat>& getCameraDistortions() const;
 
         const std::vector<cv::Mat>& getCameraIntrinsics() const;
 
-        const std::vector<cv::Mat>& getCameraDistortions() const;
+        const std::vector<cv::Mat>& getCameraExtrinsics() const;
+
+        const std::vector<cv::Mat>& getCameraExtrinsicsInitial() const;
 
         bool getUndistortImage() const;
 
@@ -51,9 +54,10 @@ namespace op
     private:
         std::vector<std::string> mSerialNumbers;
         std::vector<cv::Mat> mCameraMatrices;
-        std::vector<cv::Mat> mCameraExtrinsics;
-        std::vector<cv::Mat> mCameraIntrinsics;
         std::vector<cv::Mat> mCameraDistortions;
+        std::vector<cv::Mat> mCameraIntrinsics;
+        std::vector<cv::Mat> mCameraExtrinsics;
+        std::vector<cv::Mat> mCameraExtrinsicsInitial;
 
         // Undistortion (optional)
         bool mUndistortImage;

include/openpose/3d/poseTriangulation.hpp

@@ -6,6 +6,24 @@
 
 namespace op
 {
+    /**
+     * 3D triangulation given known camera parameter matrices and based on linear DLT algorithm.
+     * The returned cv::Mat is a 4x1 matrix, where the last coordinate is 1.
+     */
+    OP_API double triangulate(
+        cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& cameraMatrices,
+        const std::vector<cv::Point2d>& pointsOnEachCamera);
+
+    /**
+     * 3D triangulation given known camera parameter matrices and based on linear DLT algorithm with additional LMA
+     * non-linear refinement.
+     * The returned cv::Mat is a 4x1 matrix, where the last coordinate is 1.
+     * Note: If Ceres is not enabled, the LMA refinement is skipped and this function is equivalent to triangulate().
+     */
+    OP_API double triangulateWithOptimization(
+        cv::Mat& reconstructedPoint, const std::vector<cv::Mat>& cameraMatrices,
+        const std::vector<cv::Point2d>& pointsOnEachCamera, const double reprojectionMaxAcceptable);
+
     class OP_API PoseTriangulation
     {
     public:

src/openpose/3d/cameraParameterReader.cpp

@@ -21,15 +21,19 @@ namespace op
     CameraParameterReader::CameraParameterReader(const std::string& serialNumber,
                                                  const cv::Mat& cameraIntrinsics,
                                                  const cv::Mat& cameraDistortion,
-                                                 const cv::Mat& cameraExtrinsics) :
+                                                 const cv::Mat& cameraExtrinsics,
+                                                 const cv::Mat& cameraExtrinsicsInitial) :
         mUndistortImage{false}
     {
         try
         {
-            // Sanity check
-            if (serialNumber.empty() || cameraIntrinsics.empty() || cameraDistortion.empty())
-                error("Camera intrinsics, distortion, and/or serialNumber cannot be empty.",
-                      __LINE__, __FUNCTION__, __FILE__);
+            // Sanity checks
+            if (serialNumber.empty())
+                error("Camera serialNumber cannot be empty.", __LINE__, __FUNCTION__, __FILE__);
+            if (cameraIntrinsics.empty())
+                error("Camera intrinsics cannot be empty.", __LINE__, __FUNCTION__, __FILE__);
+            if (cameraDistortion.empty())
+                error("Camera distortion cannot be empty.", __LINE__, __FUNCTION__, __FILE__);
             // Add new matrices
             mSerialNumbers.emplace_back(serialNumber);
             mCameraIntrinsics.emplace_back(cameraIntrinsics.clone());
@@ -37,6 +41,9 @@ namespace op
             // Add extrinsics if not empty
             if (!cameraExtrinsics.empty())
                 mCameraExtrinsics.emplace_back(cameraExtrinsics.clone());
+            // Add extrinsics (initial) if not empty
+            if (!cameraExtrinsicsInitial.empty())
+                mCameraExtrinsicsInitial.emplace_back(cameraExtrinsicsInitial.clone());
             // Otherwise, add cv::eye
             else
                 mCameraExtrinsics.emplace_back(cv::Mat::eye(3, 4, cameraIntrinsics.type()));
@@ -69,14 +76,15 @@ namespace op
             // Commong saving/loading
             const auto dataFormat = DataFormat::Xml;
             const std::vector<std::string> cvMatNames {
-                "CameraMatrix", "Intrinsics", "Distortion"
+                "CameraMatrix", "Intrinsics", "Distortion", "CameraMatrixInitial"
             };
 
             // Load parameters
             mCameraMatrices.clear();
-            mCameraExtrinsics.clear();
-            mCameraIntrinsics.clear();
             mCameraDistortions.clear();
+            mCameraIntrinsics.clear();
+            mCameraExtrinsics.clear();
+            mCameraExtrinsicsInitial.clear();
             // log("Camera matrices:");
             for (auto i = 0ull ; i < mSerialNumbers.size() ; i++)
             {
@@ -84,7 +92,8 @@ namespace op
                 const auto cameraParameters = loadData(cvMatNames, parameterPath, dataFormat);
                 // Error if empty element
                 if (cameraParameters.empty() || cameraParameters.at(0).empty()
-                    || cameraParameters.at(1).empty() || cameraParameters.at(2).empty())
+                    || cameraParameters.at(1).empty() || cameraParameters.at(2).empty()
+                    || cameraParameters.at(3).empty())
                 {
                     const std::string errorMessage = " of the camera with serial number `" + mSerialNumbers[i]
                                                    + "` (file: " + parameterPath + "." + dataFormatToString(dataFormat)
@@ -102,10 +111,15 @@ namespace op
                     if (cameraParameters.at(2).empty())
                         error("Error at reading the camera distortion parameters" + errorMessage,
                               __LINE__, __FUNCTION__, __FILE__);
+                    // Commented for back-compatibility
+                    // if (cameraParameters.at(3).empty())
+                    //     error("Error at reading the camera distortion parameters" + errorMessage,
+                    //           __LINE__, __FUNCTION__, __FILE__);
                 }
                 mCameraExtrinsics.emplace_back(cameraParameters.at(0));
                 mCameraIntrinsics.emplace_back(cameraParameters.at(1));
                 mCameraDistortions.emplace_back(cameraParameters.at(2));
+                mCameraExtrinsicsInitial.emplace_back(cameraParameters.at(3));
                 mCameraMatrices.emplace_back(mCameraIntrinsics.back() * mCameraExtrinsics.back());
                 // log(cameraParameters.at(0));
             }
@@ -156,7 +170,7 @@ namespace op
             // Commong saving/loading
             const auto dataFormat = DataFormat::Xml;
             const std::vector<std::string> cvMatNames {
-                "CameraMatrix", "Intrinsics", "Distortion"
+                "CameraMatrix", "Intrinsics", "Distortion", "CameraMatrixInitial"
             };
             // Saving
             for (auto i = 0ull ; i < mSerialNumbers.size() ; i++)
@@ -165,6 +179,7 @@ namespace op
                 cameraParameters.emplace_back(mCameraExtrinsics[i]);
                 cameraParameters.emplace_back(mCameraIntrinsics[i]);
                 cameraParameters.emplace_back(mCameraDistortions[i]);
+                cameraParameters.emplace_back(mCameraExtrinsicsInitial[i]);
                 saveData(cameraParameters, cvMatNames, cameraParameterPath + mSerialNumbers[i], dataFormat);
             }
         }
@@ -213,16 +228,16 @@ namespace op
         }
     }
 
-    const std::vector<cv::Mat>& CameraParameterReader::getCameraExtrinsics() const
+    const std::vector<cv::Mat>& CameraParameterReader::getCameraDistortions() const
     {
         try
         {
-            return mCameraExtrinsics;
+            return mCameraDistortions;
         }
         catch (const std::exception& e)
         {
             error(e.what(), __LINE__, __FUNCTION__, __FILE__);
-            return mCameraExtrinsics;
+            return mCameraDistortions;
         }
     }
 
@@ -239,16 +254,29 @@ namespace op
         }
     }
 
-    const std::vector<cv::Mat>& CameraParameterReader::getCameraDistortions() const
+    const std::vector<cv::Mat>& CameraParameterReader::getCameraExtrinsics() const
     {
         try
         {
-            return mCameraDistortions;
+            return mCameraExtrinsics;
         }
         catch (const std::exception& e)
         {
             error(e.what(), __LINE__, __FUNCTION__, __FILE__);
-            return mCameraDistortions;
+            return mCameraExtrinsics;
+        }
+    }
+
+    const std::vector<cv::Mat>& CameraParameterReader::getCameraExtrinsicsInitial() const
+    {
+        try
+        {
+            return mCameraExtrinsicsInitial;
+        }
+        catch (const std::exception& e)
+        {
+            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
+            return mCameraExtrinsicsInitial;
         }
     }
 

src/openpose/3d/poseTriangulation.cpp

@@ -189,8 +189,8 @@ namespace op
                     cameraMatricesSubset.erase(cameraMatricesSubset.begin() + i);
                     pointsOnEachCameraSubset.erase(pointsOnEachCameraSubset.begin() + i);
                     // Remove camera i
-                    const auto projectionErrorSubset = triangulate(reconstructedPoint, cameraMatricesSubset,
-                                                                   pointsOnEachCameraSubset);
+                    const auto projectionErrorSubset = triangulate(
+                        reconstructedPoint, cameraMatricesSubset, pointsOnEachCameraSubset);
                     // If projection doesn't change much, it usually means all points are bad.
                     if (projectionErrorSubset > 0.9 * projectionError
                         && projectionErrorSubset < 1.1 * projectionError)
@@ -278,6 +278,7 @@ namespace op
             #else
                 UNUSED(reprojectionMaxAcceptable);
             #endif
+            // // This value is always 1
             // assert(reconstructedPoint.at<double>(3) == 1.);
 
             // // Check that our implementation gives similar result than OpenCV
@@ -396,13 +397,10 @@ namespace op
                     for (auto i = 0u; i < xyPoints.size(); i++)
                     {
                         cv::Mat reconstructedPoint;
-                        reprojectionErrors[i] = triangulateWithOptimization(reconstructedPoint,
-                                                                            cameraMatricesPerPoint[i],
-                                                                            xyPoints[i],
-                                                                            reprojectionMaxAcceptable);
+                        reprojectionErrors[i] = triangulateWithOptimization(
+                            reconstructedPoint, cameraMatricesPerPoint[i], xyPoints[i], reprojectionMaxAcceptable);
                         xyzPoints[i] = cv::Point3d{
-                            reconstructedPoint.at<double>(0),
-                            reconstructedPoint.at<double>(1),
+                            reconstructedPoint.at<double>(0), reconstructedPoint.at<double>(1),
                             reconstructedPoint.at<double>(2)};
                     }
                     const auto reprojectionErrorTotal = std::accumulate(