我不再相信下面的代码是正确的。我正在努力弄清楚这一点。我会在完成后发布最终的正确答案。我希望我下面的原始代码没有误导任何人。

• 原文 *：

我刚刚发现了问题所在。与其从stereoRectify获取每个摄像机的投影矩阵，我应该这样做：

cv::Mat Camera1RotationMatrix, Camera2RotationMatrix;
cv::Rodrigues(Camera1RotationVecs[0], Camera1RotationMatrix);
cv::Rodrigues(Camera2RotationVecs[0], Camera2RotationMatrix);

cv::Mat Camera1RTMat, Camera2RTMat;
cv::hconcat(Camera1RotationMatrix, Camera1TranslationVecs[0], Camera1RTMat);
cv::hconcat(Camera2RotationMatrix, Camera2TranslationVecs[0], Camera1RTMat);

const cv::Mat Camera1ProjectionMatrix = (Camera1IntrinsicMatrix * Camera1RTMat);
const cv::Mat Camera2ProjectionMatrix = (Camera2IntrinsicMatrix * Camera2RTMat);

然后，我可以像在我的原始帖子中那样在cv：：triangulatePoints调用中使用这些投影矩阵中的每一个。希望这对其他人有帮助。

好吧，我相信我有一个比我原来的更好的答案。希望它会对某些人有用。

// The four real world calibration points, expressed in millimeters
// The yellow circle is the origin
// The green circle is on the x-axis at 500mm away from the origin
// The red circle is on the y-axis at 500mm away from the origin
// The fourth calibration point is a small yellow sticker at (151mm, 194mm, 0mm)
// The z-axis is up, perpendicular to the floor
std::vector<cv::Point3f> _ObjectPointsWorldMM;
_ObjectPointsWorldMM.push_back(cv::Point3f(0.0f, 0.0f, 0.0f));
_ObjectPointsWorldMM.push_back(cv::Point3f(500.0f, 0.0f, 0.0f));
_ObjectPointsWorldMM.push_back(cv::Point3f(0.0f, 500.0f, 0.0f));
_ObjectPointsWorldMM.push_back(cv::Point3f(151.0f, 194.0f, 0.0f));
std::vector<std::vector<cv::Point3f>> ObjectPointsWorldMM;
ObjectPointsWorldMM.push_back(_ObjectPointsWorldMM);

//
// Camera 1 calibrateCamera()
//

// Get the camera 1 image
cv::Mat mCamera1Image = cv::imread(std::string("Camera1.jpg"), CV_LOAD_IMAGE_COLOR);

// The pixel locations in the camera 1 image that correspond to the four calibration points described above
std::vector<cv::Point2f> _Camera1ImagePointsPx;
_Camera1ImagePointsPx.push_back(cv::Point2f(791.0f, 220.0f)); // Corresponds to yellow origin sticker
_Camera1ImagePointsPx.push_back(cv::Point2f(864.0f, 643.0f)); // Corresponds to green x=500mm sticker
_Camera1ImagePointsPx.push_back(cv::Point2f(1277.0f, 113.0f)); // Corresponds to red y=500mm sticker
_Camera1ImagePointsPx.push_back(cv::Point2f(1010.0f, 287.0f)); // Corresponds to small yellow sticker at fourth calibration point (see above)
std::vector<std::vector<cv::Point2f>> Camera1ImagePointsPx;
Camera1ImagePointsPx.push_back(_Camera1ImagePointsPx);

// Calibrate camera 1
cv::Mat mCamera1IntrinsicMatrix;
cv::Mat mCamera1DistortionCoefficients;
std::vector<cv::Mat> Camera1RotationVecs;
std::vector<cv::Mat> Camera1TranslationVecs;
cv::calibrateCamera(
    ObjectPointsWorldMM, // In: The world millimeter locations of the four calibration points
    Camera1ImagePointsPx, // In: The pixel locations in the camera 1 image that correspond to the four calibration points
    mCamera1Image.size(), // In: The size of the camera 1 calibration image
    mCamera1IntrinsicMatrix, mCamera1DistortionCoefficients, // Out: The camera intrinsic matrix and distortion coefficients
    Camera1RotationVecs, Camera1TranslationVecs // Out: The camera rotation and translation vectors
    );

//
// Camera 2 calibrateCamera()
//

// Get the camera 2 image
cv::Mat mCamera2Image = cv::imread(std::string("Camera2.jpg"), CV_LOAD_IMAGE_COLOR);

// Check assumptions
assert((mCamera1Image.size() == mCamera2Image.size()));

// The pixel locations in the camera 2 image that correspond to the four calibration points described above
std::vector<cv::Point2f> _Camera2ImagePointsPx;
_Camera2ImagePointsPx.push_back(cv::Point2f(899.0f, 439.0f)); // Corresponds to yellow origin sticker
_Camera2ImagePointsPx.push_back(cv::Point2f(1472.0f, 608.0f)); // Corresponds to green x=500mm sticker
_Camera2ImagePointsPx.push_back(cv::Point2f(1101.0f, 74.0f)); // Corresponds to red y=500mm sticker
_Camera2ImagePointsPx.push_back(cv::Point2f(1136.0f, 322.0f)); // Corresponds to small yellow sticker at fourth calibration point (see above)
std::vector<std::vector<cv::Point2f>> Camera2ImagePointsPx;
Camera2ImagePointsPx.push_back(_Camera2ImagePointsPx);

// Calibrate camera 2
cv::Mat mCamera2IntrinsicMatrix;
cv::Mat mCamera2DistortionCoefficients;
std::vector<cv::Mat> Camera2RotationVecs;
std::vector<cv::Mat> Camera2TranslationVecs;
cv::calibrateCamera(
    ObjectPointsWorldMM, // In: The world millimeter locations of the four calibration points
    Camera2ImagePointsPx, // In: The pixel locations in the camera 2 image that correspond to the four calibration points
    mCamera2Image.size(), // In: The size of the camera 2 calibration image
    mCamera2IntrinsicMatrix, mCamera2DistortionCoefficients, // Out: The camera intrinsic matrix and distortion coefficients
    Camera2RotationVecs, Camera2TranslationVecs // Out: The camera rotation and translation vectors
    );

//
// stereoCalibrate()
//

// Calibrate the stereo camera set up
cv::Mat InterCameraRotationMatrix, InterCameraTranslationMatrix;
cv::Mat InterCameraEssentialMatrix, InterCameraFundamentalMatrix;
cv::stereoCalibrate(
    ObjectPointsWorldMM, // In: The world millimeter locations of the four calibration points
    Camera1ImagePointsPx, // In: The pixel locations in the camera 1 image that correspond to the four calibration points
    Camera2ImagePointsPx, // In: The pixel locations in the camera 2 image that correspond to the four calibration points
    mCamera1IntrinsicMatrix, mCamera1DistortionCoefficients, // In: Camera 1's intrinsic matrix and distortion coefficients
    mCamera2IntrinsicMatrix, mCamera2DistortionCoefficients, // In: Camera 2's intrinsic matrix and distortion coefficients
    mCamera1Image.size(), // In: The size of each image
    InterCameraRotationMatrix, InterCameraTranslationMatrix, // Out: The inter-camera rotation and translation matrices
    InterCameraEssentialMatrix, InterCameraFundamentalMatrix // Out: The inter-camera essential and fundamental matrices
    );

//
// solvePNP() to get the final projection matrices and R|t matrix
// to convert points from the camera frame to the world frame
//

// Build the identity R|t matrix
cv::Mat IdentityRT(3, 4, CV_64F);
IdentityRT.at<double>(0, 0) = 1.0;
IdentityRT.at<double>(1, 0) = 0.0;
IdentityRT.at<double>(2, 0) = 0.0;
IdentityRT.at<double>(0, 1) = 0.0;
IdentityRT.at<double>(1, 1) = 1.0;
IdentityRT.at<double>(2, 1) = 0.0;
IdentityRT.at<double>(0, 2) = 0.0;
IdentityRT.at<double>(1, 2) = 0.0;
IdentityRT.at<double>(2, 2) = 1.0;
IdentityRT.at<double>(0, 3) = 0.0;
IdentityRT.at<double>(1, 3) = 0.0;
IdentityRT.at<double>(2, 3) = 0.0;

// Compute the camera 1 projection matrix
const cv::Mat Camera1ProjectionMatrix = (mCamera1IntrinsicMatrix * IdentityRT);

// Build the camera 2 R|t matrix
cv::Mat Camera2RT;
cv::hconcat(InterCameraRotationMatrix, InterCameraTranslationMatrix, Camera2RT);

// Compute the camera 2 projection matrix
const cv::Mat Camera2ProjectionMatrix = (mCamera2IntrinsicMatrix * Camera2RT);

// Solve for the rotation and translation vectors
cv::Mat SolvedRotation, SolvedTranslation;
cv::solvePnP(
    ObjectPointsWorldMM[0], // In: A vector of world object points
    Camera1ImagePointsPx[0], // In: The corresponding vector of the camera's image points
    mCamera1IntrinsicMatrix, // In: The corresponding camera's intrinsic matrix
    mCamera1DistortionCoefficients, // In: The corresponding camera's distortion coefficients
    SolvedRotation, // Out: The computed rotation vector
    SolvedTranslation // Out: The computed translation vector
    );

// Get the solved rotation vector as a 3x3 matrix
cv::Mat SolvedRotationMatrix;
cv::Rodrigues(SolvedRotation, SolvedRotationMatrix);

// Build the R|t matrix
cv::Mat SolvedRT(4, 4, CV_64F);
SolvedRT.at<double>(0, 0) = SolvedRotationMatrix.at<double>(0, 0);
SolvedRT.at<double>(0, 1) = SolvedRotationMatrix.at<double>(0, 1);
SolvedRT.at<double>(0, 2) = SolvedRotationMatrix.at<double>(0, 2);
SolvedRT.at<double>(1, 0) = SolvedRotationMatrix.at<double>(1, 0);
SolvedRT.at<double>(1, 1) = SolvedRotationMatrix.at<double>(1, 1);
SolvedRT.at<double>(1, 2) = SolvedRotationMatrix.at<double>(1, 2);
SolvedRT.at<double>(2, 0) = SolvedRotationMatrix.at<double>(2, 0);
SolvedRT.at<double>(2, 1) = SolvedRotationMatrix.at<double>(2, 1);
SolvedRT.at<double>(2, 2) = SolvedRotationMatrix.at<double>(2, 2);
SolvedRT.at<double>(0, 3) = SolvedTranslation.at<double>(0, 0);
SolvedRT.at<double>(1, 3) = SolvedTranslation.at<double>(1, 0);
SolvedRT.at<double>(2, 3) = SolvedTranslation.at<double>(2, 0);
SolvedRT.at<double>(3, 0) = 0.0;
SolvedRT.at<double>(3, 1) = 0.0;
SolvedRT.at<double>(3, 2) = 0.0;
SolvedRT.at<double>(3, 3) = 1.0;

// Invert this R|t matrix
cv::Mat SolvedRTInverted = SolvedRT.inv();

//
// triangulatePoints()
//

// The pixel location of each feature to find in the camera 1 image
std::vector<cv::Point2f> FeaturePointsCamera1;
FeaturePointsCamera1.push_back(cv::Point2f(1175.0f, 440.0f)); // The pixel location of the small red sticker

// The pixel location of each feature to find in the camera 2 image
std::vector<cv::Point2f> FeaturePointsCamera2;
FeaturePointsCamera2.push_back(cv::Point2f(1401.0f, 298.0f)); // The pixel location of the small red sticker

// Perform triangulation to find the feature(s)
cv::Mat FeatureLocationHomog;
cv::triangulatePoints(
    Camera1ProjectionMatrix, Camera2ProjectionMatrix, // In: The 3x4 projection matrices for each camera
    FeaturePointsCamera1, FeaturePointsCamera2, // In: The feature pixel points for each camera
    FeatureLocationHomog // Out: The reconstructed feature locations in homogeneous coordinates
    );

//
// Convert from the camera frame coordinates returned by triangulatePoints to the world frame of reference
//

// Check assumptions
assert((1 == FeatureLocationHomog.cols));

// Get the homogeneous coordinates
cv::Mat FeatureHomog = FeatureLocationHomog.col(0);

// Check assumptions
assert((1 == FeatureHomog.cols) && (4 == FeatureHomog.rows));

// Get the divisor for computing camera frame coordinates from the homogeneous coordinates
long double dDivisor = static_cast<long double>(FeatureHomog.at<float>(3));

// If we have a non-zero divisor
if (0.0L != dDivisor)
{
    // Get the camera frame location
    const long double dCameraFrameX = (static_cast<long double>(FeatureHomog.at<float>(0)) / dDivisor);
    const long double dCameraFrameY = (static_cast<long double>(FeatureHomog.at<float>(1)) / dDivisor);
    const long double dCameraFrameZ = (static_cast<long double>(FeatureHomog.at<float>(2)) / dDivisor);

    // Pack these camera frame coordinates into a vector
    cv::Mat TriangulatedResultsCameraFrame(4, 1, CV_64F);
    TriangulatedResultsCameraFrame.at<double>(0, 0) = static_cast<double>(dCameraFrameX);
    TriangulatedResultsCameraFrame.at<double>(0, 1) = static_cast<double>(dCameraFrameY);
    TriangulatedResultsCameraFrame.at<double>(0, 2) = static_cast<double>(dCameraFrameZ);
    TriangulatedResultsCameraFrame.at<double>(0, 3) = 1.0;

    // Compute the final world location in homogeneous coordinates
    const cv::Mat FinalWorldLocation = (SolvedRTInverted * TriangulatedResultsCameraFrame);

    // Check assumptions
    assert((4 == FinalWorldLocation.rows));
    assert((1 == FinalWorldLocation.cols));

    // Get the divisor to convert from homogeneous coordinates to the final world frame
    dDivisor = static_cast<long double>(FinalWorldLocation.at<double>(3, 0));

    // If we have a non-zero divisor
    if (0.0L != dDivisor)
    {
        // Compute the final answer
        const long double dFinal3DLocX = static_cast<double>(static_cast<long double>(FinalWorldLocation.at<double>(0, 0)) / dDivisor);
        const long double dFinal3DLocY = static_cast<double>(static_cast<long double>(FinalWorldLocation.at<double>(1, 0)) / dDivisor);
        const long double dFinal3DLocZ = static_cast<double>(static_cast<long double>(FinalWorldLocation.at<double>(2, 0)) / dDivisor);
        printf("\n\tFinal answer: (%Lf, %Lf, %Lf)\n\n", dFinal3DLocX, dFinal3DLocY, dFinal3DLocZ);
    }
}