Hello,
I am calling CPosePDFGaussian::bayesianFusion which uses matrixD inv()
Sometimes, the inv() returns a bad result.
I can not find the error but I have added logs in the source code (between //CRO and //end CRO) :
"
void CPosePDFGaussian::bayesianFusion(CPosePDF &p1_, CPosePDF &p2_,
const double& minMahalanobisDistToDrop) {
MRPT_TRY_START;
MRPT\_UNUSED\_PARAM\(minMahalanobisDistToDrop\); // Not used in this class\! ASSERT\_\(p1\_.GetRuntimeClass\(\) == CLASS\_ID\(CPosePDFGaussian \) \); ASSERT\_\(p2\_.GetRuntimeClass\(\) == CLASS\_ID\(CPosePDFGaussian \) \); const CPosePDFGaussian \*p1 =static\_cast <const CPosePDFGaussian\*>\( &p1\_ \); const CPosePDFGaussian \*p2 = static\_cast<const CPosePDFGaussian\*>\( &p2\_ \); CMatrixD x1\(3,1\),x2\(3,1\),x\(3,1\); CMatrixD C1\( p1->cov \); CMatrixD C2\( p2->cov \); CMatrixD C1\_inv = C1.inv\(\); CMatrixD C2\_inv = C2.inv\(\); // CRO 05/04/2009 std::cout << "C2: " << C2 << "\n"; std::cout << "C2.inv: " << C2\_inv << "\n"; CMatrixD verificationMatrix; verificationMatrix = C2\_inv \* C2; std::cout << "vérif C2 \* 1/C2 = " << verificationMatrix << "\n"; // end CRO 05/04/2009 x1\(0,0\) = p1->mean.x; x1\(1,0\) = p1->mean.y; x1\(2,0\) = p1->mean.phi; x2\(0,0\) = p2->mean.x; x2\(1,0\) = p2->mean.y; x2\(2,0\) = p2->mean.phi; CMatrixD auxC\( C1\_inv + C2\_inv \); this->cov = auxC.inv\(\); this->assureSymmetry\(\); x = this->cov \* \( C1\_inv\*x1 + C2\_inv\*x2 \); this->mean.x = x\(0,0\); this->mean.y = x\(1,0\); this->mean.phi = x\(2,0\); this->mean.normalizePhi\(\); MRPT\_TRY\_END;
}
"
And the result is :
"
C2: 0.419 0.03809 0
0.03809 0.003463 0
0 0 1e+04
C2.inv: 1.228e+16 -1.35e+17 0
-1.35e+17 1.485e+18 0
0 0 0.0001
vérif C2 * 1/C2 = 1.264 0.002472 0
1.363 0.7168 0
0 0 1
"
Christophe
J.L. Blanco
2009-04-16
J.L. Blanco
2009-04-16
Hi Christophe,
What version of MRPT are you using? I've tried to reproduce the bug in 0.6.5 and I have the correct answer:
C2:
0.4190 0.0381 0.0000
0.0381 0.0035 0.0000
0.0000 0.0000 10000.0000
C2.inv:
23257.2196 -255809.2680 0.0000
-255809.2680 2813969.1068 0.0000
0.0000 0.0000 0.0001
vérif C2 * 1/C2 =
1.0000 0.0000 0.0000
0.0000 1.0000 0.0000
0.0000 0.0000 1.0000
If you are using the latest version, try saving the problematic matrix to a file with "cov.saveToTextFile()" just to have all the numbers with full precision and check if there's something special with it.
Best,
Jose Luis
Nobody/Anonymous
2009-04-21
Hi Jose Luis,
I found the explanation for my bad result.
My cov matrix was "almost" singular
It is not exactly singular because of "double" precision approximations occurring when I compute the cov matrix (it is the composition of a singular cov).
But it is "singular enough" to generate very small and very big numbers which generate the bad result I had.
Maybe a test could return a warning when a matrix is "almost singular", but by the way, I should not try to inverse this kind of matrix and the bug was on my side.
I just want to say that I have been working for 2 month with mrpt0.6.5 and each time I thought there was a bug, I finally found the bug on my side. Thank you for releasing such a robust library.
Christophe
J.L. Blanco
2009-04-21
Hi Christophe,
I'm glad it was not a bug! Thanks a lot for being wary and reporting anyway.
Best regards,
JL
J.L. Blanco
2009-04-21