get3DPoint: 2 methods implemented but only newer used
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.
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