Commit b4a3550b authored by Matt Clarkson's avatar Matt Clarkson

Issue #61: Move functions ComputeFundamentalMatrixFromCameraCalibration and...

Issue #61: Move functions ComputeFundamentalMatrixFromCameraCalibration and ComputeStereoExtrinsics.
parent 5c170d01
......@@ -34,94 +34,6 @@ int Signum(const double& x)
}
//-----------------------------------------------------------------------------
cv::Mat ComputeFundamentalMatrixFromCameraCalibration(const cv::Mat& leftIntrinsic,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
const cv::Mat& rightIntrinsic
)
{
cv::Mat C = cvCreateMat(3, 3, CV_64FC1);
C.at<double>(0, 0) = 0;
C.at<double>(0, 1) = -leftToRightTranslationVector.at<double>(2, 0);
C.at<double>(0, 2) = leftToRightTranslationVector.at<double>(1, 0);
C.at<double>(1, 0) = leftToRightTranslationVector.at<double>(2, 0);
C.at<double>(1, 1) = 0;
C.at<double>(1, 2) = -leftToRightTranslationVector.at<double>(0, 0);
C.at<double>(2, 0) = -leftToRightTranslationVector.at<double>(1, 0);
C.at<double>(2, 1) = leftToRightTranslationVector.at<double>(0, 0);
C.at<double>(2, 2) = 0;
cv::Mat E = C * leftToRightRotationMatrix;
cv::gemm(rightIntrinsic.inv(cv::DECOMP_SVD), E, 1, 0, 0, E, CV_GEMM_A_T);
cv::Mat F = E * (leftIntrinsic.inv(cv::DECOMP_SVD));
return F/F.at<double>(2,2);
}
//-----------------------------------------------------------------------------
void ComputeStereoExtrinsics(const std::vector<cv::Mat>& rvecsLeft,
const std::vector<cv::Mat>& tvecsLeft,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
std::vector<cv::Mat>& rvecsRight,
std::vector<cv::Mat>& tvecsRight
)
{
// First make sure we have the right number of rvecsRight and rvecsLeft allocated.
rvecsRight.clear();
tvecsRight.clear();
for (int i = 0; i < rvecsLeft.size(); i++)
{
rvecsRight.push_back(cvCreateMat(1, 3, CV_64FC1));
tvecsRight.push_back(cvCreateMat(1, 3, CV_64FC1));
}
// Then make sure rvecs and tvecs are consistent left and right.
for (int i = 0; i < rvecsLeft.size(); i++)
{
cv::Mat leftRot = cvCreateMat(3, 3, CV_64FC1);
cv::Rodrigues(rvecsLeft[i], leftRot);
cv::Matx44d leftExtrinsic = cv::Matx44d::eye();
cv::Matx44d leftToRight = cv::Matx44d::eye();
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
leftExtrinsic(r, c) = leftRot.at<double>(r, c);
leftToRight(r, c) = leftToRightRotationMatrix.at<double>(r, c);
}
leftExtrinsic(r, 3) = tvecsLeft[i].at<double>(0, r);
leftToRight(r, 3) = leftToRightTranslationVector.at<double>(r, 0);
}
cv::Matx44d rightExtrinsic = leftToRight * leftExtrinsic;
cv::Mat rightRotation = cvCreateMat(3, 3, CV_64FC1);
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
rightRotation.at<double>(r, c) = rightExtrinsic(r, c);
}
}
cv::Mat rightRotationVec = cvCreateMat(1, 3, CV_64FC1);
niftk::SafeRodrigues(rightRotation, rightRotationVec);
rvecsRight[i].at<double>(0, 0) = rightRotationVec.at<double>(0,0);
rvecsRight[i].at<double>(0, 1) = rightRotationVec.at<double>(0,1);
rvecsRight[i].at<double>(0, 2) = rightRotationVec.at<double>(0,2);
tvecsRight[i].at<double>(0, 0) = rightExtrinsic(0,3);
tvecsRight[i].at<double>(0, 1) = rightExtrinsic(1,3);
tvecsRight[i].at<double>(0, 2) = rightExtrinsic(2,3);
}
}
//-----------------------------------------------------------------------------
void ComputeMonoProjectionErrors(const niftk::Model3D& model,
const niftk::PointSet& points,
......
......@@ -33,30 +33,6 @@ int Signum(const double& x);
* \brief Private (as in 'deliberately not exported') utility functions.
*/
/**
* \brief Computes F from Camera Calibration.
*/
cv::Mat ComputeFundamentalMatrixFromCameraCalibration(const cv::Mat& leftIntrinsic,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
const cv::Mat& rightIntrinsic
);
/**
* \brief Computes a consistent set of left and right extrinsics,
* by taking the left extrinsics, and the left-to-right transformation
* and computing the corresponding right extrinsics.
*/
void ComputeStereoExtrinsics(const std::vector<cv::Mat>& rvecsLeft,
const std::vector<cv::Mat>& tvecsLeft,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
std::vector<cv::Mat>& rvecsRight,
std::vector<cv::Mat>& tvecsRight
);
/**
* \brief For a single view, calculates projection errors.
*/
......
......@@ -19,6 +19,7 @@
#include "niftkNiftyCalExceptionMacro.h"
#include "niftkHomographyUtilities.h"
#include "niftkPointUtilities.h"
#include "niftkMatrixUtilities.h"
#include <Internal/niftkTriangulationUtilities_p.h>
#include <Internal/niftkIterativeCalibrationUtilities_p.h>
#include <Internal/niftkCalibrationUtilities_p.h>
......
......@@ -406,4 +406,92 @@ void GetLeftToRightMatrix(const cv::Mat& leftRVec,
leftToRightTVec.at<double>(2, 0) = tmpTrans.at<double>(0, 2);
}
//-----------------------------------------------------------------------------
cv::Mat ComputeFundamentalMatrixFromCameraCalibration(const cv::Mat& leftIntrinsic,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
const cv::Mat& rightIntrinsic
)
{
cv::Mat C = cvCreateMat(3, 3, CV_64FC1);
C.at<double>(0, 0) = 0;
C.at<double>(0, 1) = -leftToRightTranslationVector.at<double>(2, 0);
C.at<double>(0, 2) = leftToRightTranslationVector.at<double>(1, 0);
C.at<double>(1, 0) = leftToRightTranslationVector.at<double>(2, 0);
C.at<double>(1, 1) = 0;
C.at<double>(1, 2) = -leftToRightTranslationVector.at<double>(0, 0);
C.at<double>(2, 0) = -leftToRightTranslationVector.at<double>(1, 0);
C.at<double>(2, 1) = leftToRightTranslationVector.at<double>(0, 0);
C.at<double>(2, 2) = 0;
cv::Mat E = C * leftToRightRotationMatrix;
cv::gemm(rightIntrinsic.inv(cv::DECOMP_SVD), E, 1, 0, 0, E, CV_GEMM_A_T);
cv::Mat F = E * (leftIntrinsic.inv(cv::DECOMP_SVD));
return F/F.at<double>(2,2);
}
//-----------------------------------------------------------------------------
void ComputeStereoExtrinsics(const std::vector<cv::Mat>& rvecsLeft,
const std::vector<cv::Mat>& tvecsLeft,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
std::vector<cv::Mat>& rvecsRight,
std::vector<cv::Mat>& tvecsRight
)
{
// First make sure we have the right number of rvecsRight and rvecsLeft allocated.
rvecsRight.clear();
tvecsRight.clear();
for (int i = 0; i < rvecsLeft.size(); i++)
{
rvecsRight.push_back(cvCreateMat(1, 3, CV_64FC1));
tvecsRight.push_back(cvCreateMat(1, 3, CV_64FC1));
}
// Then make sure rvecs and tvecs are consistent left and right.
for (int i = 0; i < rvecsLeft.size(); i++)
{
cv::Mat leftRot = cvCreateMat(3, 3, CV_64FC1);
cv::Rodrigues(rvecsLeft[i], leftRot);
cv::Matx44d leftExtrinsic = cv::Matx44d::eye();
cv::Matx44d leftToRight = cv::Matx44d::eye();
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
leftExtrinsic(r, c) = leftRot.at<double>(r, c);
leftToRight(r, c) = leftToRightRotationMatrix.at<double>(r, c);
}
leftExtrinsic(r, 3) = tvecsLeft[i].at<double>(0, r);
leftToRight(r, 3) = leftToRightTranslationVector.at<double>(r, 0);
}
cv::Matx44d rightExtrinsic = leftToRight * leftExtrinsic;
cv::Mat rightRotation = cvCreateMat(3, 3, CV_64FC1);
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
rightRotation.at<double>(r, c) = rightExtrinsic(r, c);
}
}
cv::Mat rightRotationVec = cvCreateMat(1, 3, CV_64FC1);
niftk::SafeRodrigues(rightRotation, rightRotationVec);
rvecsRight[i].at<double>(0, 0) = rightRotationVec.at<double>(0,0);
rvecsRight[i].at<double>(0, 1) = rightRotationVec.at<double>(0,1);
rvecsRight[i].at<double>(0, 2) = rightRotationVec.at<double>(0,2);
tvecsRight[i].at<double>(0, 0) = rightExtrinsic(0,3);
tvecsRight[i].at<double>(0, 1) = rightExtrinsic(1,3);
tvecsRight[i].at<double>(0, 2) = rightExtrinsic(2,3);
}
}
} // end namespace
......@@ -135,6 +135,30 @@ NIFTYCAL_WINEXPORT void GetLeftToRightMatrix(const cv::Mat& leftRVec1x3,
cv::Mat& leftToRightMatrix3x3,
cv::Mat& leftToRightTVec3x1
);
/**
* \brief Computes F from Camera Calibration.
*/
NIFTYCAL_WINEXPORT cv::Mat ComputeFundamentalMatrixFromCameraCalibration(const cv::Mat& leftIntrinsic,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
const cv::Mat& rightIntrinsic
);
/**
* \brief Computes a consistent set of left and right extrinsics,
* by taking the left extrinsics, and the left-to-right transformation
* and computing the corresponding right extrinsics.
*/
NIFTYCAL_WINEXPORT void ComputeStereoExtrinsics(const std::vector<cv::Mat>& rvecsLeft,
const std::vector<cv::Mat>& tvecsLeft,
const cv::Mat& leftToRightRotationMatrix,
const cv::Mat& leftToRightTranslationVector,
std::vector<cv::Mat>& rvecsRight,
std::vector<cv::Mat>& tvecsRight
);
} // end namespace
#endif
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment