added (dual) quaternion (iterative) blending

dualquat.hpp

@@ -3,6 +3,7 @@
 #define DUALQUAT_HPP
 
 #include <iostream>
+#include <vector>
 #include <limits>
 #include <cmath>
 
@@ -402,7 +403,7 @@ struct dualquat {
 
     value_t baddist (const dualquat<value_t>& other) const {
 
-        const bool I_know_what_I_do = false;
+        const bool I_know_what_I_do = true;
         assert(I_know_what_I_do);
 
         // TODO: could still be errornous
@@ -426,4 +427,200 @@ struct dualquat {
 
 };
 
+
+namespace average {
+
+template <class value_t>
+quat<value_t> QLA (const std::vector<quat<value_t>>& quats) {
+
+    quat<value_t> result(0, 0, 0, 0);
+
+    for (size_t i = 0; i < quats.size(); i++)
+        result += quats[i];
+
+    return result.N();
+}
+
+template <class value_t>
+quat<value_t> QIA (const std::vector<quat<value_t>>& quats) {
+
+    quat<value_t> b = QLA(quats);
+
+    auto logmean = [&]() {
+        quat<value_t> avg(0, 0, 0, 0);
+        for (size_t i = 0; i < quats.size(); i++)
+            avg += (b.C()^quats[i]).log();
+        return avg/quats.size();
+    };
+
+    auto x = logmean();
+    auto norm = x.dot(x);
+
+    for(;;){
+
+        b ^= x.exp();
+        auto xnew = logmean();
+
+        const auto newnorm = xnew.dot(xnew);
+        if(norm < newnorm || newnorm < EPS(value_t))
+            break;
+        else {
+            x = xnew;
+            norm = newnorm;
+        }
+    }
+
+    return b;
+}
+
+template <class value_t>
+quat<value_t> QLB (const std::vector<quat<value_t>>& quats, 
+                   const std::vector<value_t>& weights) {
+
+    assert(quats.size() == weights.size());
+
+    quat<value_t> result(0, 0, 0, 0);
+
+    for (size_t i = 0; i < quats.size(); i++)
+        result += quats[i]*weights[i];
+
+    return result.N();
+}
+
+template <class value_t>
+quat<value_t> QIB (const std::vector<quat<value_t>>& quats, 
+                   const std::vector<value_t>& weights) {
+
+
+    assert(quats.size() == weights.size());
+
+    quat<value_t> b = QLB(quats, weights);
+
+    auto logmean = [&]() {
+        quat<value_t> avg(0, 0, 0, 0);
+        for (size_t i = 0; i < quats.size(); i++)
+            avg += ((b.C())^quats[i]).log()*weights[i];
+        return avg;
+    };
+
+    auto x = logmean();
+    auto norm = x.dot(x);
+
+    for(;;){
+
+        b ^= x.exp();
+        auto xnew = logmean();
+
+        const auto newnorm = xnew.dot(xnew);
+        if(norm < newnorm || newnorm < EPS(value_t))
+            break;
+        else {
+            x = xnew;
+            norm = newnorm;
+        }
+    }    
+
+    return b;
+}
+
+template <class value_t>
+dualquat<value_t> DLA (const std::vector<dualquat<value_t>>& quats) {
+
+    dualquat<value_t> result(0, 0, 0, 0, 0, 0, 0, 0);
+
+    for (size_t i = 0; i < quats.size(); i++)
+        result += quats[i];
+
+    return (result/quats.size()).N();
+}
+
+template <class value_t>
+dualquat<value_t> DIA (const std::vector<dualquat<value_t>>& quats) {
+
+    dualquat<value_t> b = DLA(quats);
+
+    auto logmean = [&]() {
+        dualquat<value_t> avg(0, 0, 0, 0, 0, 0, 0, 0);
+        for (size_t i = 0; i < quats.size(); i++)
+            avg += (b.C()^quats[i]).log();
+        return avg/quats.size();
+    };
+
+    auto x = logmean();
+    auto norm = x.dot(x);
+
+    for(;;){
+
+        b ^= x.numexp();
+        auto xnew = logmean();
+
+        const auto newnorm = xnew.dot(xnew);
+        if(norm < newnorm || newnorm < EPS(value_t))
+            break;
+        else {
+            x = xnew;
+            norm = newnorm;
+        }
+    }
+
+    // std::cout << "precision: " << norm << std::endl;  
+
+    return b;
+}
+
+template <class value_t>
+dualquat<value_t> DLB (const std::vector<dualquat<value_t>>& quats, 
+                       const std::vector<value_t>& weights) {
+
+    assert(quats.size() == weights.size());
+
+    dualquat<value_t> result(0, 0, 0, 0, 0, 0, 0, 0);
+
+    for (size_t i = 0; i < quats.size(); i++)
+        result += quats[i]*weights[i];
+
+    return result.N();
+}
+
+template <class value_t>
+dualquat<value_t> DIB (const std::vector<dualquat<value_t>>& quats, 
+                       const std::vector<value_t>& weights) {
+
+
+    assert(quats.size() == weights.size());
+
+    dualquat<value_t> b = DLB(quats, weights);
+
+    auto logmean = [&]() {
+        dualquat<value_t> avg(0, 0, 0, 0, 0, 0, 0, 0);
+        for (size_t i = 0; i < quats.size(); i++)
+            avg += ((b.C())^quats[i]).log()*weights[i];
+        return avg;
+    };
+
+    auto x = logmean();
+    auto norm = x.dot(x);
+
+    for(;;){
+
+        b ^= x.numexp();
+        auto xnew = logmean();
+
+        const auto newnorm = xnew.dot(xnew);
+        if(norm < newnorm || newnorm < EPS(value_t))
+            break;
+        else {
+            x = xnew;
+            norm = newnorm;
+        }
+
+    }
+
+    // std::cout << "precision: " << norm << std::endl;    
+
+    return b;
+}
+
+
+}
 #endif

main.cpp

@@ -134,9 +134,10 @@ int main () {
         }
 
         const double eucdist = Q.eucdist(Q*(-1.0)),
-                     logdist = Q.logdist(Q*(-1.0));
+                     logdist = Q.logdist(Q*(-1.0)),
+                     baddist = Q.logdist(Q*(-1.0));
 
-        if (eucdist > THRESHOLD || logdist > THRESHOLD)
+        if (eucdist > THRESHOLD || logdist > THRESHOLD || baddist > THRESHOLD)
             std::cout << "distances: " <<  eucdist << ", " << logdist << std::endl;
 
     }

test.cpp

@@ -0,0 +1,104 @@
+#include "dualquat.hpp"
+#include <random>
+
+int main () {
+
+/*
+	dualquat<double> Q (0, 0, 0, 0, 0, 0, 0, 0); Q=Q.exp(); Q.print();
+	dualquat<double> T (1, 0, 0, 0, 0, 1, 1, 1); T=T.N(); T.print();
+
+	auto A = T^Q; A.print();
+	std::cout << A.lognorm() << std::endl;
+
+	auto B = T^T^Q; B.print();
+	std::cout << B.lognorm() << std::endl;
+
+	auto C = T^T^T^Q; B.print();
+	std::cout << C.lognorm() << std::endl;
+
+	auto D = T^T^T^T^Q; D.print();
+	std::cout << D.lognorm() << std::endl;
+*/
+
+/*
+	dualquat<double> Q (0, M_PI, M_PI, M_PI, 0, 0, 0, 0); Q=Q.exp(); Q.print();
+	dualquat<double> T (1, 0, 0, 0, 0, 0, 0, 0); T=T.N(); T.print();
+
+	auto A = T^Q; A.print();
+	std::cout << A.lognorm() << std::endl;
+
+	auto B = T^Q^Q; B.print();
+	std::cout << B.lognorm() << std::endl;
+
+	auto C = T^Q^Q^Q; B.print();
+	std::cout << C.lognorm() << std::endl;
+
+	auto D = T^Q^Q^Q^Q; D.print();
+	std::cout << D.lognorm() << std::endl;
+*/
+
+/*
+	dualquat<double> Q (0, M_PI, M_PI, M_PI, 0, 0, 0, 0); Q=Q.exp(); Q.print();
+	dualquat<double> T (1, 0, 0, 0, 0, 1, 1, 1); T=T.N(); T.print();
+
+	auto A = T^Q; A.print();
+	std::cout << A.lognorm() << std::endl;
+
+	auto B = T^T^Q^Q; B.print();
+	std::cout << B.lognorm() << std::endl;
+
+	auto C = T^T^T^Q^Q^Q; B.print();
+	std::cout << C.lognorm() << std::endl;
+
+	auto D = T^T^T^T^Q^Q^Q^Q; D.print();
+	std::cout << D.lognorm() << std::endl;
+*/
+
+	using namespace average;
+
+    std::mt19937 engine;
+    engine.seed(0);
+    std::uniform_real_distribution<double> dist(-1, 1);
+
+    size_t N = 10000;
+
+	auto q0 = quat<double>(0, 1, 1, 1).exp(); q0.print();
+
+    std::vector<double> weights(N, 1.0/N);
+    std::vector<quat<double>> quats;
+
+    for (size_t i = 0; i < N; i++)
+    	quats.push_back( q0^quat<double>(1+dist(engine), dist(engine), 
+    		                               dist(engine), dist(engine)).N());
+
+    QLA(quats).print();
+    std::cout << QLA(quats).logdist(q0) << std::endl;
+    QLB(quats, weights).print();
+    std::cout << QLB(quats, weights).logdist(q0) << std::endl;
+    QIA(quats).print();
+    std::cout << QIA(quats).logdist(q0) << std::endl;
+    QIB(quats, weights).print();
+    std::cout << QIB(quats, weights).logdist(q0) << std::endl;
+
+    std::cout << std::endl;
+
+    auto Q0 = dualquat<double>(0, 1, 1, 1, 0, 1, 2, 3).exp(); Q0.print();
+    std::vector<double> Weights(N, 1.0/N);
+    std::vector<dualquat<double>> Quats;
+
+    for (size_t i = 0; i < N; i++)
+    	Quats.push_back(Q0^dualquat<double>(1+dist(engine), dist(engine), 
+    		                                  dist(engine), dist(engine),
+    		                                  dist(engine), dist(engine),
+    		                                  dist(engine), dist(engine)).N());
+
+    DLA(Quats).print();
+    std::cout << DLA(Quats).logdist(Q0) << std::endl;
+    DLB(Quats, Weights).print();
+    std::cout << DLB(Quats, Weights).logdist(Q0) << std::endl;
+    DIA(Quats).print();
+    std::cout << DIA(Quats).logdist(Q0) << std::endl;
+    DIB(Quats, Weights).print();
+    std::cout << DIB(Quats, Weights).logdist(Q0) << std::endl;
+
+}