In `src/extrCalibration.cpp` in the function `Point3f ExtrCalibration::get3DPoint(Point2f p2d, double h)` two different approaches are implemented but only the newer(?) is used. Either the old method should be removed if it yields worse results, or we need to add an user option to switch the method. If so, both methods should be moved to separate functions to reduce the length of the `get3DPoint` function. The code can be seen below. ```cpp 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(); Mat rvec(3,1,CV_64F, rvec_array), rot_inv; 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(DECOMP_LU); e = rot_inv*rot_mat; if( debug ) { debout << "\n-.- ESTIMATED ROTATION\n"; for ( size_t 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)); //cout << rot_mat.at<double>(p,0) << " , " << rot_mat.at<double>(p,1) << " , " << rot_mat.at<double>(p,2) << "\n"; debout << "\n-.- ESTIMATED ROTATION^-1\n"; for ( size_t 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)); //cout << rot_inv.at<double>(p,0) << " , " << rot_inv.at<double>(p,1) << " , " << rot_inv.at<double>(p,2) << "\n"; debout << "\n-.- ESTIMATED R^-1*R\n"; for ( size_t 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)); //cout << e.at<double>(p,0) << " , " << e.at<double>(p,1) << " , " << e.at<double>(p,2) << "\n"; 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) << endl; debout << "Det(rot_inv): "<< determinant(rot_inv) << 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 << endl; } z /= (rot_inv.at<double>(2,0)*(p2d.x-/*bS)-(*/(mControlWidget->getCalibCxValue()-bS)/*-bS*/)/mControlWidget->getCalibFxValue() + rot_inv.at<double>(2,1)*(p2d.y-/*bS)-(*/(mControlWidget->getCalibCyValue()-bS)/*-bS*/)/mControlWidget->getCalibFyValue() + rot_inv.at<double>(2,2)); if( debug ) cout << "###### z: "<< z << endl; resultPoint.x = (p2d.x-/*bS)-(*/(mControlWidget->getCalibCxValue()-bS)/*-bS*/); resultPoint.y = (p2d.y-/*bS)-(*/(mControlWidget->getCalibCyValue()-bS)/*-bS*/); resultPoint.z = z; if( debug ) cout << "###### (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")" << endl; resultPoint.x = resultPoint.x * z/mControlWidget->getCalibFxValue(); resultPoint.y = resultPoint.y * z/mControlWidget->getCalibFyValue(); if( debug ) cout << "###### After intern re-calibration: (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")" << endl; tmpPoint.x = resultPoint.x - translation_vector[0]; tmpPoint.y = resultPoint.y - translation_vector[1]; tmpPoint.z = resultPoint.z - translation_vector[2]; if( debug ) cout << "###### After translation: (" << tmpPoint.x << ", " << tmpPoint.y << ", " << tmpPoint.z << ")" << 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 ) cout << "#resultPoint: (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")" << endl; if( debug ) cout << "Coord Translation: x: " << mControlWidget->getCalibCoord3DTransX() << ", y: " << mControlWidget->getCalibCoord3DTransY() << ", z: " << mControlWidget->getCalibCoord3DTransZ() << 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 Point3f camInWorld = transformRT(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 ) cout << "Point before Camera: [" << pointBeforeCam.x << ", " << pointBeforeCam.y << ", " << pointBeforeCam.z << "]" << endl; // 3D-Punkt vor Kamera in Weltkoordinaten Point3f pBCInWorld = transformRT(pointBeforeCam); if( debug ) cout << "Point before Camera in World-Coordinatesystem: [" << pBCInWorld.x << ", " << pBCInWorld.y << ", " << pBCInWorld.z << "]" << endl; if( debug ) cout << "Camera in World-Coordinatesystem: [" << camInWorld.x << ", " << camInWorld.y << ", " << camInWorld.z << "]" << 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 ) cout << "G:x = (" << camInWorld.x << " / " << camInWorld.y << " / " << camInWorld.z << ") + lambda (" << pBCInWorld.x << " / " << pBCInWorld.y << " / " << pBCInWorld.z << ")" << endl; // Berechnung des Schnittpunktes: Hier lambda von der Geraden double lambda = (h -camInWorld.z) / (pBCInWorld.z); if( debug ) cout << "Lambda: " << lambda << 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 ```