// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm
#include "gpu/testing.hpp"
#include <memory>
#include <common/angles.hpp>
#include "libvideostitch/curves.hpp"
namespace VideoStitch {
namespace Testing {
void testCurveExtend() {
{
Core::Spline* sourceSplines = Core::Spline::point(50, 7.0);
sourceSplines->cubicTo(60, 6.0)->cubicTo(70, 5.0);
std::unique_ptr<Core::Curve> source(new Core::Curve(sourceSplines));
// S: x----x----x
// this: x-----x----x
// x-----x----x---x----x----x
{
Core::Spline* curveSplines = Core::Spline::point(20, 1.0);
curveSplines->cubicTo(30, 2.0)->cubicTo(40, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(20, 1.0);
expectedSplines->cubicTo(30, 2.0)->cubicTo(40, 3.0)->cubicTo(50, 7.0)->cubicTo(60, 6.0)->cubicTo(70, 5.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// S: x-----x----x
// this: x----x----x
// x-----x----x---x----x----x
{
Core::Spline* curveSplines = Core::Spline::point(80, 1.0);
curveSplines->cubicTo(90, 2.0)->cubicTo(100, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(50, 7.0);
expectedSplines->cubicTo(60, 6.0)->cubicTo(70, 5.0)->cubicTo(80, 1.0)->cubicTo(90, 2.0)->cubicTo(100, 3.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// This is a noop:
// S: x----x----x
// this: x-----x----x-----x------x----x
{
Core::Spline* curveSplines = Core::Spline::point(10, 1.0);
curveSplines->cubicTo(20, 2.0)->cubicTo(30, 3.0)->cubicTo(60, 3.0)->cubicTo(80, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
std::unique_ptr<Core::Curve> expected(curve->clone());
ENSURE(*expected == *curve);
}
// S: x----x----x
// this: x----x-----x
// x----x-----x--x----x
{
Core::Spline* curveSplines = Core::Spline::point(20, 1.0);
curveSplines->cubicTo(30, 2.0)->cubicTo(55, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(20, 1.0);
expectedSplines->cubicTo(30, 2.0)->cubicTo(55, 3.0)->cubicTo(60, 6.0)->cubicTo(70, 5.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// S: x----x-----x
// this: x----x----x
// x----x---x----x----x
{
Core::Spline* curveSplines = Core::Spline::point(65, 1.0);
curveSplines->cubicTo(80, 2.0)->cubicTo(90, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(50, 7.0);
expectedSplines->cubicTo(60, 6.0)->cubicTo(65, 1.0)->cubicTo(80, 2.0)->cubicTo(90, 3.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// S: x----x-----x
// this: x----x
// x--x----x--x
{
Core::Spline* curveSplines = Core::Spline::point(55, 1.0);
curveSplines->cubicTo(65, 2.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(50, 7.0);
expectedSplines->cubicTo(55, 1.0)->cubicTo(65, 2.0)->cubicTo(70, 5.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// S: x----x-----x
// this: x
// x--x-x-----x
{
std::unique_ptr<Core::Curve> curve(new Core::Curve(Core::Spline::point(55, 1.0)));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(50, 7.0);
expectedSplines->cubicTo(55, 1.0)->cubicTo(60, 6.0)->cubicTo(70, 5.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// S: x----x-----x
// this: ------------ (const)
// x----x-----x
{
std::unique_ptr<Core::Curve> curve(new Core::Curve(12.0));
curve->extend(source.get());
ENSURE(*source == *curve, "extending a constant curve should yield the source");
}
// S: ------------ (const)
// this: x----x-----x
// x----x-----x
{
std::unique_ptr<Core::Curve> constantSource(new Core::Curve(12.0));
std::unique_ptr<Core::Curve> curve(source->clone());
curve->extend(constantSource.get());
ENSURE(*source == *curve, "extending with a constant curve should be a noop");
}
}
// Single-point-curve versions of the above.
{
std::unique_ptr<Core::Curve> source(new Core::Curve(Core::Spline::point(50, 7.0)));
// S: x
// this: x----x----x
// x--------------x----x----x
{
Core::Spline* curveSplines = Core::Spline::point(80, 1.0);
curveSplines->cubicTo(90, 2.0)->cubicTo(100, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(50, 7.0);
expectedSplines->cubicTo(80, 1.0)->cubicTo(90, 2.0)->cubicTo(100, 3.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// S: x
// this: x
// x--------------x
{
Core::Spline* curveSplines = Core::Spline::point(80, 1.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
Core::Spline* expectedSplines = Core::Spline::point(50, 7.0);
expectedSplines->cubicTo(80, 1.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(expectedSplines));
ENSURE(*expected == *curve);
}
// This is a noop:
// S: x
// this: x
{
std::unique_ptr<Core::Curve> curve(new Core::Curve(Core::Spline::point(50, 2.0)));
curve->extend(source.get());
std::unique_ptr<Core::Curve> expected(curve->clone());
ENSURE(*expected == *curve);
}
// S: x
// this: x----x-----x
// x----x-----x
{
Core::Spline* curveSplines = Core::Spline::point(35, 1.0);
curveSplines->cubicTo(45, 2.0)->cubicTo(55, 3.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->extend(source.get());
std::unique_ptr<Core::Curve> expected(curve->clone());
ENSURE(*expected == *curve);
}
}
}
void testLinear() {
Core::Spline* curveSplines = Core::Spline::point(0, 0.0);
curveSplines->lineTo(100, 100.0)->lineTo(200, 500.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
for (int i = -100; i <= 0; ++i) {
ENSURE_APPROX_EQ(0.0, curve->at(i), 0.0001);
}
for (int i = 0; i <= 100; ++i) {
ENSURE_APPROX_EQ((double)i, curve->at(i), 0.0001);
}
for (int i = 100; i <= 200; ++i) {
ENSURE_APPROX_EQ(100.0 + (i - 100.0) * 4.0, curve->at(i), 0.0001);
}
}
void testCubic() {
Core::Spline* curveSplines = Core::Spline::point(0, 0.0);
curveSplines->cubicTo(100, 100.0)->cubicTo(200, 500.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
ENSURE_APPROX_EQ(0.0, curve->at(-100), 0.0001);
ENSURE_APPROX_EQ(0.0, curve->at(-1), 0.0001);
ENSURE_APPROX_EQ(0.0, curve->at(0), 0.0001);
ENSURE_APPROX_EQ(100.0, curve->at(100), 0.0001);
ENSURE_APPROX_EQ(500.0, curve->at(200), 0.0001);
ENSURE_APPROX_EQ(500.0, curve->at(201), 0.0001);
ENSURE_APPROX_EQ(500.0, curve->at(300), 0.0001);
ENSURE_EQ(curve->at(0) - curve->at(-1), 0., "Slope should be 0 before curve starts");
// approx value chosen after current implementation
ENSURE_APPROX_EQ(curve->at(1) - curve->at(0), 0., 0.02, "Ease-in: slope should be close to zero");
// approx value chosen after current implementation
ENSURE_APPROX_EQ(curve->at(200) - curve->at(199), 0., 0.15, "Ease-out: slope should be close to zero");
ENSURE_EQ(curve->at(200) - curve->at(201), 0., "Slope should be 0 after curve ends");
}
void testCubicGeometryDefinition() {
Core::SplineTemplate<Core::GeometryDefinition>* curveSplines =
Core::SplineTemplate<Core::GeometryDefinition>::point(0, Core::GeometryDefinition());
// Basic checks to ensure that all members are interpolated
Core::GeometryDefinition geom;
geom.setCenterX(1.);
geom.setCenterY(2.);
geom.setDistortA(3.);
geom.setDistortB(4.);
geom.setDistortC(5.);
geom.setDistortP1(6.);
geom.setDistortP2(7.);
geom.setDistortS1(8.);
geom.setDistortS2(9.);
geom.setDistortS3(10.);
geom.setDistortS4(11.);
geom.setDistortTau1(12.);
geom.setDistortTau2(13.);
geom.setHorizontalFocal(14.);
geom.setVerticalFocal(15.);
geom.setYaw(16.);
geom.setPitch(17.);
geom.setRoll(18.);
geom.setTranslationX(19.);
geom.setTranslationY(20.);
geom.setTranslationZ(21.);
curveSplines->cubicTo(100, geom);
std::unique_ptr<Core::GeometryDefinitionCurve> curve(new Core::GeometryDefinitionCurve(curveSplines));
Core::GeometryDefinition interpolated = curve->at(50);
ENSURE_APPROX_EQ(0.5, interpolated.getCenterX(), 0.0001);
ENSURE_APPROX_EQ(1.0, interpolated.getCenterY(), 0.0001);
ENSURE_APPROX_EQ(1.5, interpolated.getDistortA(), 0.0001);
ENSURE_APPROX_EQ(2.0, interpolated.getDistortB(), 0.0001);
ENSURE_APPROX_EQ(2.5, interpolated.getDistortC(), 0.0001);
ENSURE_APPROX_EQ(3.0, interpolated.getDistortP1(), 0.0001);
ENSURE_APPROX_EQ(3.5, interpolated.getDistortP2(), 0.0001);
ENSURE_APPROX_EQ(4.0, interpolated.getDistortS1(), 0.0001);
ENSURE_APPROX_EQ(4.5, interpolated.getDistortS2(), 0.0001);
ENSURE_APPROX_EQ(5.0, interpolated.getDistortS3(), 0.0001);
ENSURE_APPROX_EQ(5.5, interpolated.getDistortS4(), 0.0001);
ENSURE_APPROX_EQ(6.0, interpolated.getDistortTau1(), 0.0001);
ENSURE_APPROX_EQ(6.5, interpolated.getDistortTau2(), 0.0001);
ENSURE_APPROX_EQ(507.0, interpolated.getHorizontalFocal(),
0.0001); // 507.0 because GeometryDefinition at 0 has a default focal of 1000.0
ENSURE_APPROX_EQ(507.5, interpolated.getVerticalFocal(),
0.0001); // 507.5 because GeometryDefinition at 0 has a default focal of 1000.0
ENSURE_APPROX_EQ(8.0, interpolated.getYaw(), 0.0001);
ENSURE_APPROX_EQ(8.5, interpolated.getPitch(), 0.0001);
ENSURE_APPROX_EQ(9.0, interpolated.getRoll(), 0.0001);
ENSURE_APPROX_EQ(9.5, interpolated.getTranslationX(), 0.0001);
ENSURE_APPROX_EQ(10.0, interpolated.getTranslationY(), 0.0001);
ENSURE_APPROX_EQ(10.5, interpolated.getTranslationZ(), 0.0001);
}
void testVSA1090() {
Core::Spline* curveSplines = Core::Spline::point(50, 10.0);
curveSplines->lineTo(100, 100.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->splitAt(25);
curveSplines = Core::Spline::point(25, 10.0);
curveSplines->lineTo(50, 10.0)->lineTo(100, 100.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(curveSplines));
ENSURE(*expected == *curve);
}
void testVSA1181() {
Core::Spline* curveSplines = Core::Spline::point(25, 10.0);
curveSplines->lineTo(50, 10.0)->lineTo(100, 10.0);
std::unique_ptr<Core::Curve> curve(new Core::Curve(curveSplines));
curve->mergeAt(100);
curve->splitAt(60);
curve->mergeAt(101);
curve->splitAt(102);
curveSplines = Core::Spline::point(25, 10.0);
curveSplines->lineTo(50, 10.0)->lineTo(60, 10.0)->lineTo(102, 10.0);
std::unique_ptr<Core::Curve> expected(new Core::Curve(curveSplines));
ENSURE(*expected == *curve);
}
bool equalToRadian(const double expected, const double actual, double epsilon) {
return fabs(expected - actual) < epsilon || fabs(expected - actual + 2.0 * M_PI) < epsilon ||
fabs(expected - actual - 2.0 * M_PI) < epsilon;
}
/**
* Tests that euler <-> quaternion roundtrip is identity.
* @param yaw in radians
* @param pitch in radians
* @param roll in radians
*/
void testQuaternionToEulerRoundtrip(const double yaw, const double pitch, const double roll) {
const Quaternion<double> q = Quaternion<double>::fromEulerZXY(yaw, pitch, roll);
double yaw2 = 0.0;
double pitch2 = 0.0;
double roll2 = 0.0;
q.toEuler(yaw2, pitch2, roll2);
if (!(equalToRadian(yaw, yaw2, 0.001) && equalToRadian(pitch, pitch2, 0.001) && equalToRadian(roll, roll2, 0.001))) {
std::cerr << "TEST FAILED: Expected (" << yaw << ", " << pitch << ", " << roll << ") +- 2 PI', got (" << yaw2
<< ", " << pitch2 << ", " << roll2 << ")" << std::endl;
std::raise(SIGABRT);
}
}
/**
* Simple test with rotation on only one axis (yaw), constant rate, symetric.
* It is equivalent to a Slerp between q01 and q02
*/
void simpleCatmullRomTestYaw(double angle0, double angle1, double angle2, double angle3) {
const int numSteps = 100;
Quaternion<double> q00 = Quaternion<double>::fromEulerZXY(angle0, 0.0, 0.0);
Quaternion<double> q01 = Quaternion<double>::fromEulerZXY(angle1, 0.0, 0.0);
Quaternion<double> q02 = Quaternion<double>::fromEulerZXY(angle2, 0.0, 0.0);
Quaternion<double> q03 = Quaternion<double>::fromEulerZXY(angle3, 0.0, 0.0);
for (int i = 0; i <= numSteps; ++i) {
double expectedAngle = angle1 + (double)i * (angle2 - angle1) / (double)numSteps;
while (expectedAngle > M_PI) {
expectedAngle -= 2 * M_PI;
}
while (expectedAngle < -M_PI) {
expectedAngle += 2 * M_PI;
}
const double t = double(i) / numSteps;
Quaternion<double> q2 = Quaternion<double>::catmullRom(q00, q01, q02, q03, t);
double yaw, pitch, roll;
q2.toEuler(yaw, pitch, roll);
Quaternion<double> qSlerp = Quaternion<double>::slerp(q01, q02, t);
double yawS, pitchS, rollS;
qSlerp.toEuler(yawS, pitchS, rollS);
ENSURE_APPROX_EQ(expectedAngle, yaw, 1e-6);
ENSURE_APPROX_EQ(0.0, pitch, 1e-6);
ENSURE_APPROX_EQ(0.0, roll, 1e-6);
ENSURE_APPROX_EQ(expectedAngle, yawS, 1e-6);
ENSURE_APPROX_EQ(0.0, pitchS, 1e-6);
ENSURE_APPROX_EQ(0.0, rollS, 1e-6);
}
}
/**
* Simple test with rotation on only one axis (pitch), constant rate, symetric.
* It is equivalent to a Slerp between q01 and q02
*/
void simpleCatmullRomTestPitch(double angle0, double angle1, double angle2, double angle3) {
const int numSteps = 100;
Quaternion<double> q00 = Quaternion<double>::fromEulerZXY(0.0, angle0, 0.0);
Quaternion<double> q01 = Quaternion<double>::fromEulerZXY(0.0, angle1, 0.0);
Quaternion<double> q02 = Quaternion<double>::fromEulerZXY(0.0, angle2, 0.0);
Quaternion<double> q03 = Quaternion<double>::fromEulerZXY(0.0, angle3, 0.0);
for (int i = 0; i <= numSteps; ++i) {
double expectedAngle = angle1 + (double)i * (angle2 - angle1) / (double)numSteps;
while (expectedAngle > M_PI / 2) {
expectedAngle -= 2 * M_PI;
}
while (expectedAngle < -M_PI / 2) {
expectedAngle += 2 * M_PI;
}
const double t = double(i) / numSteps;
Quaternion<double> q2 = Quaternion<double>::catmullRom(q00, q01, q02, q03, t);
double yaw, pitch, roll;
q2.toEuler(yaw, pitch, roll);
Quaternion<double> qSlerp = Quaternion<double>::slerp(q01, q02, t);
double yawS, pitchS, rollS;
qSlerp.toEuler(yawS, pitchS, rollS);
ENSURE_APPROX_EQ(0.0, yaw, 1e-6);
ENSURE_APPROX_EQ(expectedAngle, pitch, 1e-6);
ENSURE_APPROX_EQ(0.0, roll, 1e-6);
ENSURE_APPROX_EQ(0.0, yawS, 1e-6);
ENSURE_APPROX_EQ(expectedAngle, pitchS, 1e-6);
ENSURE_APPROX_EQ(0.0, rollS, 1e-6);
}
}
/**
* Simple test with rotation on only one axis (roll), constant rate, symetric.
* It is equivalent to a Slerp between q01 and q02
*/
void simpleCatmullRomTestRoll(double angle0, double angle1, double angle2, double angle3) {
const int numSteps = 100;
Quaternion<double> q00 = Quaternion<double>::fromEulerZXY(0.0, 0.0, angle0);
Quaternion<double> q01 = Quaternion<double>::fromEulerZXY(0.0, 0.0, angle1);
Quaternion<double> q02 = Quaternion<double>::fromEulerZXY(0.0, 0.0, angle2);
Quaternion<double> q03 = Quaternion<double>::fromEulerZXY(0.0, 0.0, angle3);
for (int i = 0; i <= numSteps; ++i) {
double expectedAngle = angle1 + (double)i * (angle2 - angle1) / (double)numSteps;
while (expectedAngle > M_PI) {
expectedAngle -= 2 * M_PI;
}
while (expectedAngle < -M_PI) {
expectedAngle += 2 * M_PI;
}
const double t = double(i) / numSteps;
Quaternion<double> q2 = Quaternion<double>::catmullRom(q00, q01, q02, q03, t);
double yaw, pitch, roll;
q2.toEuler(yaw, pitch, roll);
Quaternion<double> qSlerp = Quaternion<double>::slerp(q01, q02, t);
double yawS, pitchS, rollS;
qSlerp.toEuler(yawS, pitchS, rollS);
ENSURE_APPROX_EQ(0.0, yaw, 1e-6);
ENSURE_APPROX_EQ(0.0, pitch, 1e-6);
ENSURE_APPROX_EQ(expectedAngle, roll, 1e-6);
ENSURE_APPROX_EQ(0.0, yawS, 1e-6);
ENSURE_APPROX_EQ(0.0, pitchS, 1e-6);
ENSURE_APPROX_EQ(expectedAngle, rollS, 1e-6);
}
}
/*
* Some wxmaxima code;
c(angle) := cos(angle/2);
s(angle) := sin(angle/2);
q0(y,p,r) := - c(r) * c(p) * c(y) - s(r) * s(p) * s(y);
q1(y,p,r) := c(r) * s(p) * c(y) + s(r) * c(p) * s(y);
q2(y,p,r) := c(r) * c(p) * s(y) - s(r) * s(p) * c(y);
q3(y,p,r) := - c(r) * s(p) * s(y) + s(r) * c(p) * c(y);
t1(y,p,r) := q1(y,p,r) * q3(y,p,r) - q0(y,p,r) * q2(y,p,r);
t2(y,p,r) := q3(y,p,r) * q3(y,p,r) - q2(y,p,r) * q2(y,p,r) - q1(y,p,r) * q1(y,p,r) + q0(y,p,r) * q0(y,p,r);
yout(y, p,r) := atan2(2 * t1(y,p,r), t2(y,p,r));
yout2(y, p,r) := atan(2 * t1(y,p,r) / t2(y,p,r));
trigsimp(2 * t1(y,p,r) / t2(y,p,r));
yout(0.21, 0.12, 0.42);
t3(y,p,r) := q1(y,p,r) * q2(y,p,r) - q0(y,p,r) * q3(y,p,r);
t4(y,p,r) := q2(y,p,r) * q2(y,p,r) - q3(y,p,r) * q3(y,p,r) - q1(y,p,r) * q1(y,p,r) + q0(y,p,r) * q0(y,p,r);
trigsimp(2 * t3(y,p,r) / t4(y,p,r));
rout(y, p,r) := atan2(2 * t3(y,p,r), t4(y,p,r));
q0(0, 1.5708, 0.0628319);
q1(0, 1.5708, 0.0628319);
q2(0, 1.5708, 0.0628319);
q3(0, 1.5708, 0.0628319);
t3(0, 1.5708, 0.0628319);
t4(0, 1.5708, 0.0628319);
2.0 * t3(0, 1.5708, 0.0628319) / t4(0, 1.5708, 0.0628319);
rout(0, 1.5708, 0.0628319) + 3.14159;
*/
void testQuaternion() {
const int steps = 7;
for (int i = 0; i < steps; ++i) {
const double yaw = 2.0 * M_PI * (double)i / (double)steps;
for (int j = 1; j < steps - 1; ++j) { // Avoid being too close to gimbal lock.
const double pitch = M_PI * (-0.5 + (double)j / (double)steps);
for (int k = 0; k < steps; ++k) {
const double roll = 2.0 * M_PI * (double)k / (double)steps;
testQuaternionToEulerRoundtrip(yaw, pitch, roll);
}
}
}
// Very simple catmullRom test.
double angle0 = 0 * M_PI / 6;
double angle1 = 1 * M_PI / 6;
double angle2 = 2 * M_PI / 6;
double angle3 = 3 * M_PI / 6;
simpleCatmullRomTestYaw(angle0, angle1, angle2, angle3);
simpleCatmullRomTestRoll(angle0, angle1, angle2, angle3);
angle0 = 0 * 2 * M_PI / 3;
angle1 = 1 * 2 * M_PI / 3;
angle2 = 2 * 2 * M_PI / 3;
angle3 = 3 * 2 * M_PI / 3;
simpleCatmullRomTestYaw(angle0, angle1, angle2, angle3);
simpleCatmullRomTestRoll(angle0, angle1, angle2, angle3);
angle0 = M_PI / 2;
angle1 = M_PI / 4;
angle2 = 0;
angle3 = -M_PI / 4;
simpleCatmullRomTestPitch(angle0, angle1, angle2, angle3);
angle0 = 5 * M_PI / 11;
angle1 = 2 * M_PI / 11;
angle2 = -1 * M_PI / 11;
angle3 = -4 * M_PI / 11;
simpleCatmullRomTestPitch(angle0, angle1, angle2, angle3);
}
void testQuaternionCurves() {
{
// Quaternion<double> q0(1,0,0,0);
// Quaternion<double> q1(0.974731,0.0899088,0.0735522,-0.190806);
const Quaternion<double> q0 =
Quaternion<double>::fromEulerZXY(degToRad(-1.18172), degToRad(-47.7996), degToRad(0.0));
const Quaternion<double> q1 =
Quaternion<double>::fromEulerZXY(degToRad(-1.18172), degToRad(-47.7996), degToRad(21.4936));
// Quaternion<double> q2(0.783536,0.343338,0.215238,-0.471023);
const Quaternion<double> q2 =
Quaternion<double>::fromEulerZXY(degToRad(-1.18172), degToRad(-47.7996), degToRad(61.5007));
// Quaternion<double> q3(-0.41248,-0.0943216,0.0365096,0.905335);
const Quaternion<double> q3 =
Quaternion<double>::fromEulerZXY(degToRad(-1.18172), degToRad(-47.7996), degToRad(131.079));
/*
{
std::cout << std::endl << "LINEAR" << std::endl;
Core::SphericalSpline* spline = Core::SphericalSpline::point(0, q0);
spline->lineTo(31, q1)->lineTo(62, q2)->lineTo(92, q3);
std::unique_ptr<Core::QuaternionCurve> curve(new Core::QuaternionCurve(spline));
for (int i = 0; i < 93; ++i) {
Quaternion<double> t = curve->at(i);
// std::cout << t.q0 << " " << t.q1 << " " << t.q2 << " " << t.q3 << std::endl;
if (i == 0 || i == 31 || i == 62 || i == 92) std::cout << " KEYFRAME ";
double yaw, pitch, roll;
t.toEuler(yaw, pitch, roll);
std::cout << radToDeg(yaw) << " " << radToDeg(pitch) << " " << radToDeg(roll) << std::endl;
}
}
*/
{
std::cout << std::endl << "CUBIC" << std::endl;
Core::SphericalSpline* spline = Core::SphericalSpline::point(0, q0);
spline->cubicTo(31, q1)->cubicTo(62, q2)->cubicTo(92, q3);
std::unique_ptr<Core::QuaternionCurve> curve(new Core::QuaternionCurve(spline));
for (int i = 0; i < 93; ++i) {
// std::cout << t.q0 << " " << t.q1 << " " << t.q2 << " " << t.q3 << std::endl;
if (i == 0 || i == 31 || i == 62 || i == 92) std::cout << " KEYFRAME ";
Quaternion<double> t = curve->at(i);
double yaw, pitch, roll;
t.toEuler(yaw, pitch, roll);
// std::cout << radToDeg(yaw) << " " << radToDeg(pitch) << " " << radToDeg(roll) << std::endl;
}
/*
Quaternion<double> t = curve->at(92);
double yaw, pitch, roll;
t.toEuler(yaw, pitch, roll);
std::cout << radToDeg(yaw) << " " << radToDeg(pitch) << " " << radToDeg(roll) << std::endl;
*/
}
{
Quaternion<double> q0(0.00264554, -0.0161071, -0.0133043, 0.0339792);
Quaternion<double> q1(0.953552, -0.264985, 0.139959, -0.0305589);
Core::SphericalSpline* spline = Core::SphericalSpline::point(5856, q0);
std::unique_ptr<Core::QuaternionCurve> curve(new Core::QuaternionCurve(spline));
spline->lineTo(5948, q1);
Quaternion<double> t = curve->at(5890);
double y, p, r;
std::cout << "FIRST MIDDLE LAST" << std::endl;
q0.toEuler(y, p, r);
std::cout << radToDeg(y) << " " << radToDeg(p) << " " << radToDeg(r) << std::endl;
t.toEuler(y, p, r);
std::cout << radToDeg(y) << " " << radToDeg(p) << " " << radToDeg(r) << std::endl;
q1.toEuler(y, p, r);
std::cout << radToDeg(y) << " " << radToDeg(p) << " " << radToDeg(r) << std::endl;
}
}
{
double yaw = 0.642, pitch = -0.312, roll = 1.765;
Quaternion<double> q = Quaternion<double>::fromEulerZXY(yaw, pitch, roll);
Matrix33<double> m1 = q.toRotationMatrix();
Matrix33<double> m2 = Matrix33<double>::fromEulerZXY(yaw, pitch, roll);
ENSURE_APPROX_EQ(m1(0, 0), m2(0, 0), 0.001);
ENSURE_APPROX_EQ(m1(0, 1), m2(0, 1), 0.001);
ENSURE_APPROX_EQ(m1(0, 2), m2(0, 2), 0.001);
ENSURE_APPROX_EQ(m1(1, 0), m2(1, 0), 0.001);
ENSURE_APPROX_EQ(m1(1, 1), m2(1, 1), 0.001);
ENSURE_APPROX_EQ(m1(1, 2), m2(1, 2), 0.001);
ENSURE_APPROX_EQ(m1(2, 0), m2(2, 0), 0.001);
ENSURE_APPROX_EQ(m1(2, 1), m2(2, 1), 0.001);
ENSURE_APPROX_EQ(m1(2, 2), m2(2, 2), 0.001);
}
}
void testConstant() {
Core::Curve curve(12.0);
ENSURE(!curve.splines(), "constant curve should not have splines");
ENSURE_APPROX_EQ(12.0, curve.at(0), 0.00001);
ENSURE_APPROX_EQ(12.0, curve.at(20), 0.00001);
ENSURE(!curve.splines(), "got splines, expected none");
curve.splitAt(10);
ENSURE(curve.splines(), "non-constant curve should have splines");
ENSURE_APPROX_EQ(12.0, curve.at(0), 0.00001);
ENSURE_APPROX_EQ(12.0, curve.at(20), 0.00001);
ENSURE(curve.splines(), "got no splines, expected one");
ENSURE(curve.splines()->getType() == Core::Spline::PointType);
curve.splines()->end.v = 3.0;
curve.mergeAt(10);
ENSURE(!curve.splines(), "constant curve should not have splines");
ENSURE_APPROX_EQ(3.0, curve.at(0), 0.00001);
ENSURE_APPROX_EQ(3.0, curve.at(20), 0.00001);
ENSURE(!curve.splines(), "got splines, expected none");
}
} // namespace Testing
} // namespace VideoStitch
int main() {
VideoStitch::Testing::initTest();
VideoStitch::Testing::testQuaternion();
VideoStitch::Testing::testCurveExtend();
VideoStitch::Testing::testLinear();
VideoStitch::Testing::testCubic();
VideoStitch::Testing::testCubicGeometryDefinition();
VideoStitch::Testing::testVSA1090();
VideoStitch::Testing::testVSA1181();
VideoStitch::Testing::testQuaternionCurves();
VideoStitch::Testing::testConstant();
return 0;
}