diff options
| -rw-r--r-- | redist/Makefile | 4 | ||||
| -rw-r--r-- | redist/linmath.c | 32 | ||||
| -rw-r--r-- | redist/linmath.h | 3 | ||||
| -rw-r--r-- | redist/lintest.c | 28 | ||||
| -rw-r--r-- | redist/minimal_opencv.c | 4 |
5 files changed, 66 insertions, 5 deletions
diff --git a/redist/Makefile b/redist/Makefile index 5598c9b..8f98911 100644 --- a/redist/Makefile +++ b/redist/Makefile @@ -3,8 +3,8 @@ all : jsmntest lintest test_dcl jsmntest : jsmntest.c jsmn.c gcc -g -O0 -o $@ $^ -lintest : lintest.c linmath.c linmath.h - gcc -g -O0 -o $@ $^ -lm +lintest : lintest.c linmath.c linmath.h minimal_opencv.c + gcc -DUSE_DOUBLE -DFLT=double -g -O0 -o $@ $^ -lcblas -lm -llapacke minimal_opencvtest : minimal_opencvtest.c minimal_opencv.c minimal_opencv.h gcc -g -O0 -o $@ $^ -lcblas -lm -llapacke diff --git a/redist/linmath.c b/redist/linmath.c index 80eeaa5..7d21e5c 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -1,5 +1,4 @@ // Copyright 2013,2016 <>< C. N. Lohr. This file licensed under the terms of the MIT license. - #include "linmath.h" #include <float.h> #include <math.h> @@ -678,5 +677,36 @@ void KabschCentered(LinmathQuat qout, const FLT *ptsA, const FLT *ptsB, int num_ quatfrommatrix33(qout, _R); } +LINMATH_EXPORT void Kabsch(LinmathPose *B2Atx, const FLT *_ptsA, const FLT *_ptsB, int num_pts) { + FLT centerA[3] = {}; + FLT centerB[3] = {}; + + for (int i = 0; i < num_pts; i++) { + for (int j = 0; j < 3; j++) { + centerA[j] += _ptsA[i * 3 + j]; + centerB[j] += _ptsB[i * 3 + j]; + } + } + + for (int j = 0; j < 3; j++) { + centerA[j] = centerA[j] / (FLT)num_pts; + centerB[j] = centerB[j] / (FLT)num_pts; + } + + FLT ptsA[num_pts * 3]; + FLT ptsB[num_pts * 3]; + + for (int i = 0; i < num_pts; i++) { + for (int j = 0; j < 3; j++) { + ptsA[i * 3 + j] = _ptsA[i * 3 + j] - centerA[j]; + ptsB[i * 3 + j] = _ptsB[i * 3 + j] - centerB[j]; + } + } + + KabschCentered(B2Atx->Rot, ptsA, ptsB, num_pts); + quatrotatevector(centerA, B2Atx->Rot, centerA); + sub3d(B2Atx->Pos, centerB, centerA); +} + LinmathQuat LinmathQuat_Identity = {1.0}; LinmathPose LinmathPose_Identity = {.Rot = {1.0}}; diff --git a/redist/linmath.h b/redist/linmath.h index f961568..188be99 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -145,6 +145,9 @@ LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); // // This assumes that the space A and B share an origin. LINMATH_EXPORT void KabschCentered(LinmathQuat B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts); + +// Same as above except it solves for the center for you +LINMATH_EXPORT void Kabsch(LinmathPose *B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts); // Matrix Stuff typedef struct { diff --git a/redist/lintest.c b/redist/lintest.c index 4c53da7..ee3082a 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -102,11 +102,39 @@ void testApplyPoseToPoint() { } } +void testKabsch() { + FLT pts[] = {0, 0, 0, 100, 100, 100, 10, 0, 10, 50, 50, 0, 0, 0, 1000, -100, 0, 100}; + + LinmathPose tx = {.Pos = {}, .Rot = {4, 3, 2, 1}}; + + LinmathPose tx2 = {.Pos = {1, 2, 3}, .Rot = {1, 2, 3, 4}}; + + quatnormalize(tx.Rot, tx.Rot); + quatnormalize(tx2.Rot, tx2.Rot); + + int N = sizeof(pts) / sizeof(FLT) / 3; + FLT txPts[N * 3]; + FLT txPts2[N * 3]; + for (int i = 0; i < N; i++) { + ApplyPoseToPoint(txPts + i * 3, &tx, pts + i * 3); + ApplyPoseToPoint(txPts2 + i * 3, &tx2, pts + i * 3); + } + + LinmathQuat should_be_tx = {}; + KabschCentered(should_be_tx, pts, txPts, N); + ASSERT_FLTA_EQUALS(should_be_tx, tx.Rot, 4); + + LinmathPose should_be_tx2 = {}; + Kabsch(&should_be_tx2, pts, txPts2, N); + ASSERT_FLTA_EQUALS(should_be_tx2.Pos, tx2.Pos, 7); +} + int main() { testInvertPose(); testApplyPoseToPoint(); testApplyPoseToPose(); + testKabsch(); #if 1 #define NONTRANSPOSED_DAVE diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c index da7681c..2e9e814 100644 --- a/redist/minimal_opencv.c +++ b/redist/minimal_opencv.c @@ -185,7 +185,7 @@ SURVIVE_LOCAL_ONLY double cvInvert(const CvMat *srcarr, CvMat *dstarr, int metho assert(inf >= 0); if (inf > 0) { printf("Warning: Singular matrix: \n"); - print_mat(srcarr); + // print_mat(srcarr); } free(ipiv); @@ -252,7 +252,7 @@ SURVIVE_LOCAL_ONLY int cvSolve(const CvMat *Aarr, const CvMat *xarr, CvMat *Barr assert(inf >= 0); if (inf > 0) { printf("Warning: Singular matrix: \n"); - print_mat(a_ws); + // print_mat(a_ws); } #ifdef DEBUG_PRINT |