From caa3f1f1c57b7876156eedbc3738e90765f72d14 Mon Sep 17 00:00:00 2001
From: Justin Berger <jdavidberger@gmail.com>
Date: Sun, 1 Jul 2018 05:26:29 +0000
Subject: Added kabsch / test for kabsch

---
 redist/Makefile         |  4 ++--
 redist/linmath.c        | 32 +++++++++++++++++++++++++++++++-
 redist/linmath.h        |  3 +++
 redist/lintest.c        | 28 ++++++++++++++++++++++++++++
 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
-- 
cgit v1.2.3