Commit 68cb7ed2 authored by Matt Clarkson's avatar Matt Clarkson

Issue #61: Add cross-eyed warning.

parent cddf4b3f
......@@ -1619,4 +1619,153 @@ double ComputeLeftToRight(const Model3D& model,
return fre;
}
//-----------------------------------------------------------------------------
bool IsCrossEyed(const cv::Mat& intrinsicLeft,
const cv::Mat& distortionLeft,
const cv::Mat& rvecLeft,
const cv::Mat& tvecLeft,
const cv::Mat& intrinsicRight,
const cv::Mat& distortionRight,
const cv::Mat& rvecRight,
const cv::Mat& tvecRight,
cv::Point3d* convergencePoint,
const double& maximumUsableDistance
)
{
bool isCrossEyed = false;
cv::Mat leftToRightRotationMatrix = cvCreateMat(3, 3, CV_64FC1);
cv::Mat leftToRightTranslationVector = cvCreateMat(3, 1, CV_64FC1);
niftk::GetLeftToRightMatrix(rvecLeft,
tvecLeft,
rvecRight,
tvecRight,
leftToRightRotationMatrix,
leftToRightTranslationVector);
niftk::Point2D p;
p.id = 0;
p.point.x = intrinsicLeft.at<double>(0, 2);
p.point.y = intrinsicLeft.at<double>(1, 2);
niftk::PointSet leftPrincipalPoint;
leftPrincipalPoint.insert(std::pair<NiftyCalIdType, Point2D>(0, p));
p.point.x = intrinsicRight.at<double>(0, 2);
p.point.y = intrinsicRight.at<double>(1, 2);
niftk::PointSet rightPrincipalPoint;
rightPrincipalPoint.insert(std::pair<NiftyCalIdType, Point2D>(0, p));
niftk::Model3D triangulatedPoints;
niftk::TriangulatePointPairs(leftPrincipalPoint,
rightPrincipalPoint,
intrinsicLeft,
distortionLeft,
rvecLeft,
tvecLeft,
leftToRightRotationMatrix,
leftToRightTranslationVector,
intrinsicRight,
distortionRight,
triangulatedPoints
);
if (triangulatedPoints.size() != 1)
{
niftkNiftyCalThrow() << "There should be exactly 1 triangulated vergence point.";
}
cv::Point3d tp = (*(triangulatedPoints.begin())).second.point;
if (convergencePoint != nullptr)
{
*convergencePoint = tp;
}
if (tp.z > 0 && tp.z < maximumUsableDistance)
{
isCrossEyed = true;
}
return isCrossEyed;
}
//-----------------------------------------------------------------------------
void CheckAgainstConvergencePoint(const std::list<PointSet>& leftDistortedPoints,
const std::list<PointSet>& rightDistortedPoints,
const cv::Mat& intrinsicLeft,
const cv::Mat& distortionLeft,
const std::vector<cv::Mat>& rvecsLeft,
const std::vector<cv::Mat>& tvecsLeft,
const cv::Mat& intrinsicRight,
const cv::Mat& distortionRight,
const std::vector<cv::Mat>& rvecsRight,
const std::vector<cv::Mat>& tvecsRight,
const cv::Point3d& convergencePoint,
bool& somePointsAreNearer,
bool& somePointsAreFurther
)
{
somePointsAreNearer = false;
somePointsAreFurther = false;
double depth = convergencePoint.z;
std::list<PointSet>::const_iterator leftIter;
std::list<PointSet>::const_iterator rightIter;
unsigned int viewCounter = 0;
for (leftIter = leftDistortedPoints.begin(),
rightIter = rightDistortedPoints.begin();
leftIter != leftDistortedPoints.end() &&
rightIter != rightDistortedPoints.end();
leftIter++,
rightIter++
)
{
cv::Mat leftToRightRotationMatrix = cvCreateMat(3, 3, CV_64FC1);
cv::Mat leftToRightTranslationVector = cvCreateMat(3, 1, CV_64FC1);
niftk::GetLeftToRightMatrix(rvecsLeft[viewCounter],
tvecsLeft[viewCounter],
rvecsRight[viewCounter],
tvecsRight[viewCounter],
leftToRightRotationMatrix,
leftToRightTranslationVector);
niftk::Model3D triangulatedPoints;
niftk::TriangulatePointPairs(*leftIter,
*rightIter,
intrinsicLeft,
distortionLeft,
rvecsLeft[viewCounter],
tvecsLeft[viewCounter],
leftToRightRotationMatrix,
leftToRightTranslationVector,
intrinsicRight,
distortionRight,
triangulatedPoints
);
niftk::Model3D::const_iterator modelIter;
for (modelIter = triangulatedPoints.begin();
modelIter != triangulatedPoints.end();
modelIter++
)
{
if (modelIter->second.point.z > depth)
{
somePointsAreFurther = true;
}
if (modelIter->second.point.z <= depth)
{
somePointsAreNearer = true;
}
}
viewCounter++;
}
}
} // end namespace
......@@ -427,6 +427,39 @@ NIFTYCAL_WINEXPORT double ComputeLeftToRight(const Model3D& model,
cv::Mat& leftToRightTranslationVector
);
/**
* \brief Checks if stereo cameras are cross eyed.
*/
NIFTYCAL_WINEXPORT bool IsCrossEyed(const cv::Mat& intrinsicLeft,
const cv::Mat& distortionLeft,
const cv::Mat& rvecLeft,
const cv::Mat& tvecLeft,
const cv::Mat& intrinsicRight,
const cv::Mat& distortionRight,
const cv::Mat& rvecRight,
const cv::Mat& tvecRight,
cv::Point3d* convergencePoint,
const double& maximumUsableDistance = std::numeric_limits<float>::max()
);
/**
* \brief Can be used to check if points are both
* nearer and further than the convergence point of a stereo pair of cameras.
*/
NIFTYCAL_WINEXPORT void CheckAgainstConvergencePoint(const std::list<PointSet>& leftDistortedPoints,
const std::list<PointSet>& rightDistortedPoints,
const cv::Mat& intrinsicLeft,
const cv::Mat& distortionLeft,
const std::vector<cv::Mat>& rvecsLeft,
const std::vector<cv::Mat>& tvecsLeft,
const cv::Mat& intrinsicRight,
const cv::Mat& distortionRight,
const std::vector<cv::Mat>& rvecsRight,
const std::vector<cv::Mat>& tvecsRight,
const cv::Point3d& convergencePoint,
bool& somePointsAreNearer,
bool& somePointsAreFurther
);
} // end namespace
#endif
......@@ -184,6 +184,50 @@ cv::Matx21d StereoCameraCalibration(const Model3D& model,
niftkNiftyCalThrow() << "No right image points extracted.";
}
// Check for cross-eyed cameras AND points both before and after convergence point.
cv::Point3d convergencePoint;
bool isCrossEyed = niftk::IsCrossEyed(intrinsicLeft,
distortionLeft,
rvecsLeft[0],
tvecsLeft[0],
intrinsicRight,
distortionRight,
rvecsRight[0],
tvecsRight[0],
&convergencePoint
);
bool somePointsAreNearer = false;
bool somePointsAreFurther = false;
niftk::CheckAgainstConvergencePoint(listOfLeftHandPointSets,
listOfRightHandPointSets,
intrinsicLeft,
distortionLeft,
rvecsLeft,
tvecsLeft,
intrinsicRight,
distortionRight,
rvecsRight,
tvecsRight,
convergencePoint,
somePointsAreNearer,
somePointsAreFurther
);
// Warnings for now?
if (isCrossEyed)
{
std::cerr << "WARNING: Cameras are cross eyed. Convergence depth is approx "
<< static_cast<int>(convergencePoint.z)
<< " mm."
<< std::endl;
if (somePointsAreFurther)
{
std::cerr << "WARNING: Cameras are cross eyed and data is beyond convergence point." << std::endl;
}
}
// Do standard OpenCV calibration
projectedRMS = cv::stereoCalibrate(objectPoints,
leftImagePoints,
......
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