diff --git a/include/extrCalibration.h b/include/extrCalibration.h index 495689185fdaf14b73f4a589aa641ce053a0aeeb..86183ad10b38ad57e4a0b647c6e1496fa41b7313 100644 --- a/include/extrCalibration.h +++ b/include/extrCalibration.h @@ -179,7 +179,7 @@ public: void calibExtrParams(); bool calcReprojectionError(); virtual cv::Point2f getImagePoint(cv::Point3f p3d); - cv::Point3f get3DPoint(cv::Point2f p2d, double h); + cv::Point3f get3DPoint(const cv::Point2f &p2d, double h) const; cv::Point3f transformRT(cv::Point3f p); bool isOutsideImage(cv::Point2f p2d); inline bool isOutsideImage(cv::Point3f p3d) { return isOutsideImage(getImagePoint(p3d)); } diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp index 722b13885b4b4274939f063f612e66cda6c405af..109e930e10cd342aa7651426146812266b51b1ce 100644 --- a/src/extrCalibration.cpp +++ b/src/extrCalibration.cpp @@ -861,262 +861,103 @@ cv::Point2f ExtrCalibration::getImagePoint(cv::Point3f p3d) * @param h height i.e. distance to xy-plane in cm * @return calculated 3D point in cm */ -cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h) +cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h) const { - bool debug = false; - - if(debug) - { - std::cout << "get3DPoint: Start Point2D: (" << p2d.x << ", " << p2d.y << ") h: " << h << std::endl; - } - int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0; - if(false && bS > 0) - { - p2d.x += bS; - p2d.y += bS; - } - - // Ergebnis 3D-Punkt cv::Point3f resultPoint, tmpPoint; - - bool newMethod = true; - /////////////// Start new method - if(newMethod) - { - double rvec_array[3], translation_vector[3]; - rvec_array[0] = mControlWidget->getCalibExtrRot1(); - rvec_array[1] = mControlWidget->getCalibExtrRot2(); - rvec_array[2] = mControlWidget->getCalibExtrRot3(); - - cv::Mat rvec(3, 1, CV_64F, rvec_array), rot_inv; - cv::Mat rot_mat(3, 3, CV_64F), e(3, 3, CV_64F); - // Transform the rotation vector into a rotation matrix with opencvs rodrigues method - Rodrigues(rvec, rot_mat); - - translation_vector[0] = rot_mat.at<double>(0, 0) * mControlWidget->getCalibExtrTrans1() + - rot_mat.at<double>(0, 1) * mControlWidget->getCalibExtrTrans2() + - rot_mat.at<double>(0, 2) * mControlWidget->getCalibExtrTrans3(); - translation_vector[1] = rot_mat.at<double>(1, 0) * mControlWidget->getCalibExtrTrans1() + - rot_mat.at<double>(1, 1) * mControlWidget->getCalibExtrTrans2() + - rot_mat.at<double>(1, 2) * mControlWidget->getCalibExtrTrans3(); - translation_vector[2] = rot_mat.at<double>(2, 0) * mControlWidget->getCalibExtrTrans1() + - rot_mat.at<double>(2, 1) * mControlWidget->getCalibExtrTrans2() + - rot_mat.at<double>(2, 2) * mControlWidget->getCalibExtrTrans3(); - - // use inverse Matrix - rot_inv = rot_mat.inv(cv::DECOMP_LU); - e = rot_inv * rot_mat; - - if(debug) - { - debout << "\n-.- ESTIMATED ROTATION\n"; - for(int p = 0; p < 3; p++) - { - printf( - "%20.18f, %20.18f, %20.18f\n", - rot_mat.at<double>(p, 0), - rot_mat.at<double>(p, 1), - rot_mat.at<double>(p, 2)); - } - - debout << "\n-.- ESTIMATED ROTATION^-1\n"; - for(int p = 0; p < 3; p++) - { - printf( - "%20.18f, %20.18f, %20.18f\n", - rot_inv.at<double>(p, 0), - rot_inv.at<double>(p, 1), - rot_inv.at<double>(p, 2)); - } - - debout << "\n-.- ESTIMATED R^-1*R\n"; - for(int p = 0; p < 3; p++) - { - printf("%20.18f, %20.18f, %20.18f\n", e.at<double>(p, 0), e.at<double>(p, 1), e.at<double>(p, 2)); - } - - debout << "\n-.- ESTIMATED TRANSLATION\n"; - debout << mControlWidget->getCalibExtrTrans1() << " , " << mControlWidget->getCalibExtrTrans2() << " , " - << mControlWidget->getCalibExtrTrans3() << "\n"; - debout << translation_vector[0] << " , " << translation_vector[1] << " , " << translation_vector[2] << "\n"; - - debout << "Det(rot_mat): " << determinant(rot_mat) << std::endl; - debout << "Det(rot_inv): " << determinant(rot_inv) << std::endl; - } - double z = h + rot_inv.at<double>(2, 0) * translation_vector[0] + - rot_inv.at<double>(2, 1) * translation_vector[1] + rot_inv.at<double>(2, 2) * translation_vector[2]; - if(debug) - { - debout << "##### z: " << h << " + " << rot_inv.at<double>(2, 0) << "*" << translation_vector[0] << " + " - << rot_inv.at<double>(2, 1) << "*" << translation_vector[1] << " + " << rot_inv.at<double>(2, 2) - << "*" << translation_vector[2] << " = " << z << std::endl; - } - z /= - (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) / - mControlWidget->getCalibFxValue() + - rot_inv.at<double>(2, 1) * (p2d.y - (mControlWidget->getCalibCyValue() - bS)) / - mControlWidget->getCalibFyValue() + - rot_inv.at<double>(2, 2)); - if(debug) - { - std::cout << "###### z: " << z << std::endl; - } - - resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS)); - resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS)); - resultPoint.z = z; - - if(debug) - { - std::cout << "###### (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")" - << std::endl; - } - - resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue(); - resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue(); - - if(debug) - { - std::cout << "###### After intern re-calibration: (" << resultPoint.x << ", " << resultPoint.y << ", " - << resultPoint.z << ")" << std::endl; - } - - tmpPoint.x = resultPoint.x - translation_vector[0]; - tmpPoint.y = resultPoint.y - translation_vector[1]; - tmpPoint.z = resultPoint.z - translation_vector[2]; - - if(debug) - { - std::cout << "###### After translation: (" << tmpPoint.x << ", " << tmpPoint.y << ", " << tmpPoint.z << ")" - << std::endl; - } - - resultPoint.x = rot_inv.at<double>(0, 0) * (tmpPoint.x) + rot_inv.at<double>(0, 1) * (tmpPoint.y) + - rot_inv.at<double>(0, 2) * (tmpPoint.z); - resultPoint.y = rot_inv.at<double>(1, 0) * (tmpPoint.x) + rot_inv.at<double>(1, 1) * (tmpPoint.y) + - rot_inv.at<double>(1, 2) * (tmpPoint.z); - resultPoint.z = rot_inv.at<double>(2, 0) * (tmpPoint.x) + rot_inv.at<double>(2, 1) * (tmpPoint.y) + - rot_inv.at<double>(2, 2) * (tmpPoint.z); - - if(debug) - { - std::cout << "#resultPoint: (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")" - << std::endl; - } - if(debug) - { - std::cout << "Coord Translation: x: " << mControlWidget->getCalibCoord3DTransX() - << ", y: " << mControlWidget->getCalibCoord3DTransY() - << ", z: " << mControlWidget->getCalibCoord3DTransZ() << std::endl; - } - - - // Coordinate Transformations - - resultPoint.x -= mControlWidget->getCalibCoord3DTransX(); - resultPoint.y -= mControlWidget->getCalibCoord3DTransY(); - resultPoint.z -= mControlWidget->getCalibCoord3DTransZ(); - - resultPoint.x *= mControlWidget->getCalibCoord3DSwapX() ? -1 : 1; - resultPoint.y *= mControlWidget->getCalibCoord3DSwapY() ? -1 : 1; - resultPoint.z *= mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1; - } - else //////////////// End new method - { - //////////////// Start old method - - cv::Point3f camInWorld = transformRT(cv::Point3f(0, 0, 0)); - - // 3D-Punkt vor der Kamera mit Tiefe 5 - CvPoint3D32f pointBeforeCam; - pointBeforeCam.x = (p2d.x - mControlWidget->cx->value()) / mControlWidget->fx->value() * 50; - pointBeforeCam.y = (p2d.y - mControlWidget->cy->value()) / mControlWidget->fy->value() * 50; - pointBeforeCam.z = 50; - if(debug) - { - std::cout << "Point before Camera: [" << pointBeforeCam.x << ", " << pointBeforeCam.y << ", " - << pointBeforeCam.z << "]" << std::endl; - } - // 3D-Punkt vor Kamera in Weltkoordinaten - cv::Point3f pBCInWorld = transformRT(pointBeforeCam); - if(debug) - { - std::cout << "Point before Camera in World-Coordinatesystem: [" << pBCInWorld.x << ", " << pBCInWorld.y - << ", " << pBCInWorld.z << "]" << std::endl; - } - if(debug) - { - std::cout << "Camera in World-Coordinatesystem: [" << camInWorld.x << ", " << camInWorld.y << ", " - << camInWorld.z << "]" << std::endl; - } - // Berechnung des Richtungsvektors der Gerade von der Kamera durch den Pixel - // Als Sttzvektor der Geraden wird die Position der Kamera gewhlt - pBCInWorld.x -= camInWorld.x; - pBCInWorld.y -= camInWorld.y; - pBCInWorld.z -= camInWorld.z; - if(debug) - { - std::cout << "G:x = (" << camInWorld.x << " / " << camInWorld.y << " / " << camInWorld.z << ") + lambda (" - << pBCInWorld.x << " / " << pBCInWorld.y << " / " << pBCInWorld.z << ")" << std::endl; - } - - // Berechnung des Schnittpunktes: Hier lambda von der Geraden - double lambda = (h - camInWorld.z) / (pBCInWorld.z); - if(debug) - { - std::cout << "Lambda: " << lambda << std::endl; - } - - // Lambda in Gerade einsetzen - resultPoint.x = (mControlWidget->getCalibCoord3DSwapX() ? -1 : 1) * (camInWorld.x + lambda * pBCInWorld.x); - resultPoint.y = (mControlWidget->getCalibCoord3DSwapY() ? -1 : 1) * (camInWorld.y + lambda * pBCInWorld.y); - resultPoint.z = (mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1) * (camInWorld.z + lambda * pBCInWorld.z); - - } //////////////// End old method - - return resultPoint; -} -/** - * Transformiert den angegebenen 3D-Punkt mit der Rotation und Translation - * um Umrechnungen zwischen verschiedenen Koordinatensystemen zu ermglichen - */ -cv::Point3f ExtrCalibration::transformRT(cv::Point3f p) -{ - // ToDo: use projectPoints(); - - double rvec_array[3], rotation_matrix[9]; - rvec_array[0] = mControlWidget->getCalibExtrRot1(); - rvec_array[1] = mControlWidget->getCalibExtrRot2(); - rvec_array[2] = mControlWidget->getCalibExtrRot3(); - - cv::Mat rvec(3, 1, CV_64F, rvec_array); - cv::Mat rot_mat(3, 3, CV_64F); // Transform the rotation vector into a rotation matrix with opencvs rodrigues method + cv::Matx<double, 3, 3> rot_inv; + cv::Matx<double, 3, 3> rot_mat(3, 3, CV_64F); + const cv::Mat rvec = + (cv::Mat_<double>(3, 1) << mControlWidget->getCalibExtrRot1(), + mControlWidget->getCalibExtrRot2(), + mControlWidget->getCalibExtrRot3()); Rodrigues(rvec, rot_mat); + rot_inv = rot_mat.inv(cv::DECOMP_LU, nullptr); + + // Create translation vector + cv::Vec3d translation{ + mControlWidget->getCalibExtrTrans1(), + mControlWidget->getCalibExtrTrans2(), + mControlWidget->getCalibExtrTrans3()}; + + // Subtract principal point and border, so we can assume pinhole camera + const cv::Vec2d centeredImagePoint{ + p2d.x - (mControlWidget->getCalibCxValue() - bS), p2d.y - (mControlWidget->getCalibCyValue() - bS)}; + + + /* Basic Idea: + * All points projecting onto a point on the image plane lie on the same + * line (cf. pinhole camera model). We can determine this line in the form: + * + * g: x = lambda * v + * + * This line exists in camera coordinates. Let v be the projection with + * depth 1 (i.e. v_3 = 1). Then lambda is the depth of the resulting point. + * We'll continue to call lambda z instead, to show this. + * We now want to determine the depth at which the resulting point has height h + * in world coordinates. The transformation from cam to world is: + * + * W = R * C - T + * W := Point in World Coords + * C := Point in Cam Coords + * R,T := Rotation and Translation of Cam + * + * By putting in our x = z * v, we get: + * W = R * (z * v) - T + * <=> W = z * Rv - T + * <=> W + T = z * Rv + * <=> (W + T)/Rv = z + * We select the third row of this to solve for z. Finally g(z) is transformed + * into World Coords. + */ + + // calc Rv, (R = rot_inv) + // rotatedProj = Rv + const cv::Vec2d focalLength{mControlWidget->getCalibFxValue(), mControlWidget->getCalibFyValue()}; + const cv::Vec2d pinholeProjectionXY = cv::Mat(centeredImagePoint.div(focalLength)); + const cv::Vec3d pinholeProjectionXY1{pinholeProjectionXY[0], pinholeProjectionXY[1], 1}; + const cv::Vec3d rotatedProj = rot_inv * pinholeProjectionXY1; + + // determine z via formula from comment above; using 3rd row + double z = (h + translation[2]) / rotatedProj[2]; + + // Evaluate line at depth z; calc point in camera coords + // written this way instead of z * pinholeProjectionXY1 (i.e. z * v) to not change test results due to floating + // point precision diff + resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS)); + resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS)); + resultPoint.z = z; + + resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue(); + resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue(); + + // We transform from cam coords to world coords with W = R * C - T + // we now calc: W = R * (C - R^-1*T), which is equivalent + translation = rot_mat * translation; + tmpPoint.x = resultPoint.x - translation[0]; + tmpPoint.y = resultPoint.y - translation[1]; + tmpPoint.z = resultPoint.z - translation[2]; + + resultPoint.x = rot_inv(0, 0) * (tmpPoint.x) + rot_inv(0, 1) * (tmpPoint.y) + rot_inv(0, 2) * (tmpPoint.z); + resultPoint.y = rot_inv(1, 0) * (tmpPoint.x) + rot_inv(1, 1) * (tmpPoint.y) + rot_inv(1, 2) * (tmpPoint.z); + resultPoint.z = rot_inv(2, 0) * (tmpPoint.x) + rot_inv(2, 1) * (tmpPoint.y) + rot_inv(2, 2) * (tmpPoint.z); + + + // Coordinate Transformations + resultPoint.x -= mControlWidget->getCalibCoord3DTransX(); + resultPoint.y -= mControlWidget->getCalibCoord3DTransY(); + resultPoint.z -= mControlWidget->getCalibCoord3DTransZ(); + + resultPoint.x *= mControlWidget->getCalibCoord3DSwapX() ? -1 : 1; + resultPoint.y *= mControlWidget->getCalibCoord3DSwapY() ? -1 : 1; + resultPoint.z *= mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1; - rotation_matrix[0] = rot_mat.at<double>(0, 0); - rotation_matrix[1] = rot_mat.at<double>(0, 1); - rotation_matrix[2] = rot_mat.at<double>(0, 2); - rotation_matrix[3] = rot_mat.at<double>(1, 0); - rotation_matrix[4] = rot_mat.at<double>(1, 1); - rotation_matrix[5] = rot_mat.at<double>(1, 2); - rotation_matrix[6] = rot_mat.at<double>(2, 0); - rotation_matrix[7] = rot_mat.at<double>(2, 1); - rotation_matrix[8] = rot_mat.at<double>(2, 2); - - cv::Point3f point3D; - - point3D.x = rotation_matrix[0] * p.x + rotation_matrix[3] * p.y + rotation_matrix[6] * p.z - - mControlWidget->trans1->value(); - point3D.y = rotation_matrix[1] * p.x + rotation_matrix[4] * p.y + rotation_matrix[7] * p.z - - mControlWidget->trans2->value(); - point3D.z = rotation_matrix[2] * p.x + rotation_matrix[5] * p.y + rotation_matrix[8] * p.z - - mControlWidget->trans3->value(); - return point3D; + return resultPoint; } + bool ExtrCalibration::isOutsideImage(cv::Point2f p2d) { int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;