Commit 758f5d3e authored by Martin Reinecke's avatar Martin Reinecke

Merge branch 'quaternions' into 'ducc0'

add quaternions

See merge request !20
parents 3dd271fc ec85bf2d
Pipeline #78853 passed with stages
in 16 minutes and 23 seconds
FROM debian:testing-slim
RUN apt-get update && apt-get install -y git python3-pip python3-pytest python3-pybind11 pybind11-dev clang && rm -rf /var/lib/apt/lists/*
RUN apt-get update && apt-get install -y git python3-pip python3-scipy python3-pytest python3-pybind11 pybind11-dev clang && rm -rf /var/lib/apt/lists/*
......@@ -29,6 +29,7 @@ include src/ducc0/math/gl_integrator.h
include src/ducc0/math/math_utils.h
include src/ducc0/math/pointing.cc
include src/ducc0/math/pointing.h
include src/ducc0/math/quaternion.h
include src/ducc0/math/rangeset.h
include src/ducc0/math/space_filling.cc
include src/ducc0/math/space_filling.h
......@@ -60,9 +61,11 @@ include python/alm.h
include python/totalconvolve.h
include python/totalconvolve.cc
include python/misc.cc
include python/pointingprovider.cc
include python/test/test_fft.py
include python/test/test_healpix.py
include python/test/test_pointing.py
include python/test/test_sht.py
include python/test/test_totalconvolve.py
include python/test/test_wgridder.py
......
......@@ -19,6 +19,7 @@
#include "python/wgridder.cc"
#include "python/healpix.cc"
#include "python/misc.cc"
#include "python/pointingprovider.cc"
using namespace ducc0;
......@@ -32,4 +33,5 @@ PYBIND11_MODULE(PKGNAME, m)
add_wgridder(m);
add_healpix(m);
add_misc(m);
add_pointingprovider(m);
}
/*
* This code is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This code is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this code; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/*
* Copyright (C) 2020 Max-Planck-Society
* Author: Martin Reinecke
*/
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include "ducc0/math/quaternion.h"
namespace ducc0 {
namespace detail_pymodule_pointingprovider {
using namespace std;
namespace py = pybind11;
template<typename T> class PointingProvider
{
private:
double t0_, freq_;
vector<quaternion_t<T>> quat_;
vector<T> rangle, rxsin;
vector<bool> rotflip;
public:
PointingProvider(double t0, double freq, const mav<T,2> &quat)
: t0_(t0), freq_(freq), quat_(quat.shape(0)), rangle(quat.shape(0)),
rxsin(quat.shape(0)), rotflip(quat.shape(0))
{
MR_assert(quat.shape(0)>=2, "need at least 2 quaternions");
MR_assert(quat.shape(1)==4, "need 4 entries in quaternion");
quat_[0] = quaternion_t<T>(quat(0,0), quat(0,1), quat(0,2), quat(0,3)).normalized();
for (size_t m=0; m<quat_.size()-1; ++m)
{
quat_[m+1] = quaternion_t<T>(quat(m+1,0), quat(m+1,1), quat(m+1,2), quat(m+1,3)).normalized();
quaternion_t<T> delta(quat_[m+1]*quat_[m].conj());
rotflip[m]=false;
if (delta.w < 0.)
{ rotflip[m]=true; delta.flip(); }
auto [v, omega] = delta.toAxisAngle();
rangle[m]=omega*.5;
rxsin[m]=1./sin(rangle[m]);
}
}
void get_rotated_quaternions(double t0, double freq, const mav<T,1> &rot,
mav<T,2> &out)
{
MR_assert(rot.shape(0)==4, "need 4 entries in quaternion");
auto rot_ = quaternion_t<T>(rot(0), rot(1), rot(2), rot(3)).normalized();
MR_assert(out.shape(1)==4, "need 4 entries in quaternion");
double ofs = (t0-t0_)*freq_;
for (size_t i=0; i<out.shape(0); ++i)
{
double fi = ofs + (i/freq)*freq_;
MR_assert((fi>=0) && fi<=(quat_.size()-1+1e-7), "time outside available range");
size_t idx = size_t(fi);
idx = min(idx, quat_.size()-2);
double frac = fi-idx;
double omega = rangle[idx];
double xsin = rxsin[idx];
double w1 = sin((1.-frac)*omega)*xsin,
w2 = sin(frac*omega)*xsin;
if (rotflip[idx]) w1=-w1;
const quaternion_t<T> &q1(quat_[idx]), &q2(quat_[idx+1]);
quaternion_t<T> q(w1*q1.x + w2*q2.x,
w1*q1.y + w2*q2.y,
w1*q1.z + w2*q2.z,
w1*q1.w + w2*q2.w);
q = rot_*q;
out.v(i,0) = q.x;
out.v(i,1) = q.y;
out.v(i,2) = q.z;
out.v(i,3) = q.w;
}
}
};
template<typename T> class PyPointingProvider: public PointingProvider<T>
{
protected:
using PointingProvider<T>::get_rotated_quaternions;
public:
PyPointingProvider(double t0, double freq, const py::array &quat)
: PointingProvider<T>(t0, freq, to_mav<T,2>(quat)) {}
py::array pyget_rotated_quaternions_out(double t0, double freq,
const py::array &quat, py::array &out)
{
auto res2 = to_mav<T,2>(out,true);
auto quat2 = to_mav<T,1>(quat);
get_rotated_quaternions(t0, freq, quat2, res2);
return move(out);
}
py::array pyget_rotated_quaternions(double t0, double freq,
const py::array &quat, size_t nval)
{
auto res = make_Pyarr<T>({nval,4});
return pyget_rotated_quaternions_out(t0, freq, quat, res);
}
};
const char *pointingprovider_DS = R"""(
Functionality for converting satellite orientations to detector orientations
at a different frequency
)""";
const char *PointingProvider_init_DS = R"""(
Creates a PointingProvider object from a starting time, a sampling frequency
and a list of orientation quaternions.
Parameters
----------
t0 : float
the time of the first sample
This is arbitrary and just provides a reference to the starting times
of the requested detector orientations.
freq : float
the frequency at which the provided satellite orientations are sampled
quat : numpy.ndarray((nval, 4), dtype=numpy.float64)
the satellite orientation quaternions. Components are expecetd in the order
(x, y, z, w). The quaternions need not be normalized.
Returns
-------
PointongProvider : the constructed object
)""";
const char *get_rotated_quaternions_DS = R"""(
Produces quaternions started at the requested time, sampled at the requested
frequency, which are rotated relative to the satellite orientation according to
a provided quaternion.
Parameters
----------
t0 : float
the time of the first output sample
This must use the same reference system as the time passed to the
constructor.
freq : float
the frequency at which the output orientations should be sampled
rot : numpy.ndarray((4,), dtype=numpy.float64)
A single rotation quaternion describing the rotation from the satellite to
the detector reference system. Components are expecetd in the order
(x, y, z, w). The quaternion need not be normalized.
nval : int
the number of requested quaternions
Returns
-------
numpy.ndarray((nval, 4), dtype=numpy.float64) : the output quaternions
The quaternions are normalized and in the order (x, y, z, w)
)""";
const char *get_rotated_quaternions2_DS = R"""(
Produces quaternions started at the requested time, sampled at the requested
frequency, which are rotated relative to the satellite orientation according to
a provided quaternion.
Parameters
----------
t0 : float
the time of the first output sample
This must use the same reference system as the time passed to the
constructor.
freq : float
the frequency at which the output orientations should be sampled
rot : numpy.ndarray((4,), dtype=numpy.float64)
A single rotation quaternion describing the rotation from the satellite to
the detector reference system. Components are expecetd in the order
(x, y, z, w). The quaternion need not be normalized.
out : numpy.ndarray((nval, 4), dtype=numpy.float64)
the array to put the computed quaternions into
Returns
-------
numpy.ndarray((nval, 4), dtype=numpy.float64) : the output quaternions
The quaternions are normalized and in the order (x, y, z, w)
This is identical to the provided "out" array.
)""";
void add_pointingprovider(py::module &msup)
{
using namespace pybind11::literals;
auto m = msup.def_submodule("pointingprovider");
m.doc() = pointingprovider_DS;
using pp_d = PyPointingProvider<double>;
py::class_<pp_d>(m, "PointingProvider")
.def(py::init<double, double, const py::array &>(),
PointingProvider_init_DS, "t0"_a, "freq"_a, "quat"_a)
.def ("get_rotated_quaternions", &pp_d::pyget_rotated_quaternions,
get_rotated_quaternions_DS,"t0"_a, "freq"_a, "rot"_a, "nval"_a)
.def ("get_rotated_quaternions", &pp_d::pyget_rotated_quaternions_out,
get_rotated_quaternions2_DS,"t0"_a, "freq"_a, "rot"_a, "out"_a);
}
}
using detail_pymodule_pointingprovider::add_pointingprovider;
}
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
# Copyright(C) 2020 Max-Planck-Society
import ducc0.pointingprovider as pp
# import pyfftw
import numpy as np
import pytest
from numpy.testing import assert_
def _l2error(a, b):
return np.sqrt(np.sum(np.abs(a-b)**2)/np.sum(np.abs(a)**2))
def _assert_close(a, b, epsilon):
err = _l2error(a, b)
if (err >= epsilon):
print("Error: {} > {}".format(err, epsilon))
assert_(err < epsilon)
pmp = pytest.mark.parametrize
@pmp("size", (2, 10, 37, 1000))
@pmp("t0", (-45.3, 0, 10))
@pmp("freq", (1, 1.3e-7, 3e10))
def testp1(size, t0, freq):
rng = np.random.default_rng(42)
quat = rng.uniform(-.5, .5, (size, 4))
prov = pp.PointingProvider(t0, freq, quat)
rquat = np.array([0., 0., 0., 1.]) # a non-rotating quaternion
quat2 = prov.get_rotated_quaternions(t0, freq, rquat, size)
nquat = quat/np.sqrt(np.sum(quat**2, axis=1, keepdims=True))
# adjust signs
nquat = nquat * np.sign(nquat[:, 0]).reshape(-1, 1)
quat2 = quat2 * np.sign(quat2[:, 0]).reshape(-1, 1)
_assert_close(quat2, nquat, 1e-13)
def testp2():
rng = np.random.default_rng(42)
t01, f1, size1 = 0., 1., 200
quat1 = rng.uniform(-.5, .5, (size1, 4))
prov = pp.PointingProvider(t01, f1, quat1)
rquat = rng.uniform(-.5, .5, (4,))
t02, f2, size2 = 3.7, 10.2, 300
quat2 = prov.get_rotated_quaternions(t02, f2, rquat, size2)
quat3 = np.empty((size2, 4), dtype=np.float64)
quat3 = prov.get_rotated_quaternions(t02, f2, rquat, quat3)
assert_((quat2==quat3).all(), "problem")
quat2 *= np.sign(quat2[:,0]).reshape((-1,1))
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
times1 = t01 + 1./f1*np.arange(size1)
r1 = R.from_quat(quat1)
rrquat = R.from_quat(rquat)
slerp = Slerp(times1, r1)
times2 = t02 + 1./f2*np.arange(size2)
r2 = rrquat*slerp(times2)
squat2 = r2.as_quat()
squat2 *= np.sign(squat2[:,0]).reshape((-1,1))
_assert_close(quat2, squat2, 1e-13)
/*
* This code is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This code is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this code; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/*! \file quaternion.h
* Quaternion class for rotation transforms in 3D space
*
* Copyright (C) 2005-2020 Max-Planck-Society
* \author Martin Reinecke
*/
#ifndef DUCC0_QUATERNION_H
#define DUCC0_QUATERNION_H
#include <cmath>
#include "ducc0/math/vec3.h"
namespace ducc0 {
namespace detail_quaternion {
using namespace std;
/*! \defgroup quaterniongroup Quaternions */
/*! \{ */
/*! Quaternion class for rotation transforms in 3D space */
template<typename T> class quaternion_t
{
public:
T x, y, z, w;
quaternion_t() {}
quaternion_t(T X, T Y, T Z, T W)
: x(X), y(Y), z(Z), w(W) {}
quaternion_t(const vec3_t<T> &axis, T angle)
{
angle*=T(0.5);
T sa=sin(angle);
x = sa*axis.x;
y = sa*axis.y;
z = sa*axis.z;
w = cos(angle);
}
T norm() const
{ return x*x+y*y+z*z+w*w; }
quaternion_t normalized() const
{
T fct = sqrt(T(1)/norm());
return quaternion_t(x*fct, y*fct, z*fct, w*fct);
}
quaternion_t operator-() const
{ return quaternion_t(-x,-y,-z,-w); }
void flip()
{ x=-x; y=-y; z=-z; w=-w; }
quaternion_t conj() const
{ return quaternion_t(-x,-y,-z, w); }
quaternion_t inverse() const
{
T fct = T(1)/norm();
return quaternion_t(-x*fct, -y*fct, -z*fct, w*fct);
}
quaternion_t &operator*= (const quaternion_t &b)
{
quaternion_t a=*this;
x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
return *this;
}
quaternion_t operator* (const quaternion_t &b) const
{
return quaternion_t (w*b.x + x*b.w + y*b.z - z*b.y,
w*b.y - x*b.z + y*b.w + z*b.x,
w*b.z + x*b.y - y*b.x + z*b.w,
w*b.w - x*b.x - y*b.y - z*b.z);
}
T dot(const quaternion_t &other) const
{ return x*other.x + y*other.y + z*other.z + w*other.w; }
auto toAxisAngle() const
{
T norm = x*x + y*y + z*z;
if (norm==T(0))
return make_tuple(vec3_t<T>(0,0,1), T(0));
norm = sqrt(norm);
T inorm = T(1)/norm;
return make_tuple(vec3_t<T>(x*inorm,y*inorm,z*inorm), 2*atan2(norm, w));
}
};
/*! \} */
}
using detail_quaternion::quaternion_t;
}
#endif
......@@ -41,7 +41,7 @@ namespace ducc0 {
/*! \{ */
/*! Class representing a 3D cartesian vector. */
template<typename T>class vec3_t
template<typename T> class vec3_t
{
public:
T x, /*!< x-coordinate */
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment