BACKLOG.txt

// Inspiration:
// https://github.com/zmij/math
// http://tvmet.sourceforge.net/introduction.html

// Useful format:?
// http://netpbm.sourceforge.net/doc/pgm.html

// No-frills basic implementation for small (< 10x10) matrices targeting robotics and computer graphics.

// Unsorted:
// [ ] Notebook
//     [ ] Fix data presentation
//     [ ] Include camera images in data presentation
// [ ] Inference post-processing
//     [ ] Algorithm for mean and variance estimation of bounding boxes
// [ ] Model
//     [ ] Investigate Transformers (attention)
//     [ ] Investigate Performers (attention)
//     [ ] Custom loss function (weights per head and weights per class)
//     [ ] Remove "van" class and include in "car" category
// [ ] Implement lupdecomp
// [ ] Remove or implement depthmap

// Basics (feature complete)
// [x] matrix initialization
// [-] assignment operator (not a copy ctor, no assignment operator needed...)
// [-] fold instead of loop in copy ctor
// [x] expr() + remove at in expression
// [x] public inheritance and class instead of struct
// [x] matrix multiplication
// [x] dot
// [x] eval => auto m = eval(expression<E> u)
// [x] sort at() => operator()(size_t, size_t)
// [-] look at changing E::m, E::n to values instead
// [x] correct m x n confusion!
//
// Matrix manipulation
// [x] reshape matrix (redim)
// [x] submatrix<1, 1, 3, 3>(mat4f{}) [i, j, m, n]
// [x] "concat" (| column vectors) | for concatenating column vectors (matrices with same amount of rows), make same for row vectors + splicing functions...
// [x] "stack" (^ row vectors) (same amount of columns, arbitrary amount of rows) 
// [x] element_cast to cast expressions mat3f <=> mat3i tex...
// [x] type cast to elt_t for (1, 1) expression (as non-member?) as_element()/as_elt()??
//     constexpr operator elt_t() {
//       return (*this)(0, 0);
//     }
// [-] Rethink eval() as_elt() (should as_elt simply call eval?) - should they evaluate all the expressions or just one step?
// [x] Sort out expression_type, do not return matrix<> but rather expression? Maybe no support for matrices in matrices?
// [x] Move operations out of matrix.h
//
// More math
// [x] cross product (of column vectors, produces column vector)
//     return Vector<value_type, 3>(
//	     lhs(1) * rhs(2) - lhs(2) * rhs(1),
//	     lhs(2) * rhs(0) - lhs(0) * rhs(2),
//	     lhs(0) * rhs(1) - lhs(1) * rhs(0));
// [x] scalar multiplication
// [x] scalar division
// [x] work out references in expression tree, buggy now...
// [x] eval() function
// [x] sum
// [-] sumsq
// [x] BUG: matrix<matrix> doesn't work (yet!)
// [x] rethink colwise/rowwise/(eltwise) takes std::function<elt_t, size_t, size_t> as argument.
//	   [x] colwise
//     [x] rowwise
//     [x] eltwise
// [x] square, elementwise square
// [x] remove sumsq() + normsq<>
// [x] decide on as_element existence + immediate or delayed + implicit or explicit conversion to elt_t
//     all operations return matrixes. use as_element to get element value from 1x1 matrix!
// [x] inverse for affinity - including scaling! 
//     https://stackoverflow.com/questions/30536920/how-do-i-invert-an-affine-transformation-with-translation-rotation-and-scaling
//     http://negativeprobability.blogspot.com/2011/11/affine-transformations-and-their.html
// [x] aliasing x=expression(x) can eval() be used to mitigate? Yes!
// [+] create expressions for everything and delay execution where suitable
// [-] Forward decl of all expressions? (see submatrix)
// [x] implicit conversion for known 1x1 matrices? OR evaluated matrix<T,1,1> could be convertible to T through inheriting converter<T,M,N>...
// [x] matrix test cases
//
// Wrap-up (refactoring and polishing)
// [x] Use the ord_to_row/col structs everywhere, remove for_each/for loops?? (benchmark)
// [-] Use eltwise to implement other functions? (benchmark)
// [x] Check all uses of for_all_elts() and similar. Remove for_ and update users to sum<> status
//     [x] matrix ctor
//     [x] compare
// [x] expression_traits<matrix> + reference_traits<matrix> location?
// [x] Refactor elt_t/m/n/ref_t (expression_traits + reference_traits)
// [-] Refactor ref_t (see TODO near expr_traits)
// [x] operator[] (or operator()?) for one-dimensional matrices (maybe for two dimensional as well?) e0[0][1]
//	   operator[](std::size_t i) should return a pointer to element i * m::value for two dim matrices, element by value or by reference for one-dimensional matrices
//     operator[] could return elements from array<> using the index only. makes sense for matricies as well!
// [x] operator(i,j) on expression + remove liberal use of expr()?
//     [x] matrix
//     [x] rot2rpy
// [x] Refactor byval => val/value to be consistent with ref/reference (byval/byref => value/reference???)
// [x] Refactor element => elt everywhere?
// [x] Check todo:s
// [x] Check includes
// [x] Check static_assert everywhere
// [x] Check constexpr everywhere (incl ctors)
// [x] Improve/standardize matrix interface (to template aliases as well if possible), see:
//     [x] Structured binding:
//         https://en.cppreference.com/w/cpp/language/structured_binding
//         https://stackoverflow.com/questions/56184539/what-is-the-purpose-of-stdtuple-sizestdarray
//         https://en.cppreference.com/w/cpp/container/array/tuple_size
//     [x] Deduction guides: https://stackoverflow.com/questions/53203629/array-class-that-will-accept-an-braced-init-list-and-deduce-length
//     [x] Aggregate initialization: https://en.cppreference.com/w/cpp/language/aggregate_initialization
//	       auto const [a1, a2, a3] = vec3f{ 0,1,2 };
//         auto const [b1, b2, b3] = std::array{1, 2, 3};
//
// Transform
// [x] Create transform.h
// [x] Finish transform v1 (3x3 rotation matrix + 3x1 translation vector)
// [x] Specific operator*(transform, transform) for efficiency
// [x] Transform test cases (https://www.andre-gaschler.com/rotationconverter/)
// [ ] Implement transform explicit deduction guide (x2)
// [ ] Frame graph implementation
// [ ] Frame graph test cases
// [ ] Cleanup transform
//
// Point
// [x] Add function to transform point3 (and point3 sequence?)
// [ ] Point test cases
// [ ] Cleanup point
//
// Cuboid
// [x] Add cuboid.h
// [x] Cuboid test cases
// [ ] Cleanup cuboid
//
// Geometry (Robotics)
// [x] Create geometry.h
// [x] rotation matrix <=> ypr!
// [ ] Add traits for comparison epsilon?? (or find out there is one in STL) (see rot2rpy)
//     http://realtimecollisiondetection.net/blog/?p=89
// [x] Geometry test cases
// [ ] Cleanup geometry
//
// Even more math
// [ ] norm() (L2) + normsq() // L2 norm (magnitude)
//     https://en.wikipedia.org/wiki/Matrix_norm
//     http://mathworld.wolfram.com/VectorNorm.html
// [ ] normalize()
// [ ] diagonal() to extract the diagonal of a (square? no not necessarily!!) matrix as a vector
// [ ] determinant
//     https://www.mathsisfun.com/algebra/matrix-determinant.html
// [ ] LU(P) Decomposition/solving (needed for matrix inversion)
//     https://en.wikipedia.org/wiki/LU_decomposition
//     [ ] Mutable submatrix expressions (work started, see submatrix_, need to adjust ref_t?)
//     [ ] std::swap support for expressions
// [ ] full matrix inverse (hardcoded for 2D, 3D and 4D matrices?)
//     https://stackoverflow.com/questions/2624422/efficient-4x4-matrix-inverse-affine-transform
//     https://ncalculators.com/matrix/inverse-matrix.htm
//     https://www.scratchapixel.com/lessons/mathematics-physics-for-computer-graphics/matrix-inverse
// [ ] hadamard_product https://en.wikipedia.org/wiki/Hadamard_product_(matrices)
//
// Statistics:
// [ ] Variance and standard deviation
// [ ] Mahalobnis distance
//     https://en.wikipedia.org/wiki/Mahalanobis_distance
// [ ] Covariance matrix
//     https://stattrek.com/matrix-algebra/covariance-matrix.aspx
//     https://stackoverflow.com/questions/33268513/calculating-standard-deviation-variance-in-c
//
// Optimization
// [ ] Optimization
//     https://nfrechette.github.io/2017/04/13/modern_simd_matrix_multiplication/
// [ ] rowwise/colwise optimization! Will cause multiple calculations of same values
//     if used carelessly and reduction returns more than one element.
// [ ] Benchmarks
//
// Affinity
// [ ] Rename/remove??
// [ ] functions for creating affine matrices + affine.h
//     https://www.cs.utexas.edu/users/fussell/courses/cs384g-fall2011/lectures/lecture07-Affine.pdf
//     https://www.mathworks.com/discovery/affine-transformation.html
// [ ] affinity test cases
//
// Refactoring:
// [ ] Express operator() using operator[] and make it general.
//     Would it work as an expression member??
//     Would reduce the need for duplicated implementations of operator()...
// [x] Evaluate usefulness of operator[] -> USEFUL!
// [x] expression_base for common members
// [-] .x, .y, .z, .w for 2,3 and 4d vector expressions? -> NO, use structured bindings
// [ ] binary colwise/rowwise (combining two expressions to one)
// [ ] broadcast to dimension (i.e. duplicate rows, cols or add elements zeroes, ones...)
//     https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
// [ ] != ?
// [ ] Nice syntax for slicing and indexing? See numpy
// [ ] OpenGL/CUDA compatibility for vectors and matrices.
// [ ] Evaluate usefulness of implicit conversion of 1x1 expressions.
// [ ] Evaluate usefulness of compare expression implicit conversion and consider expansion to other expressions.
// [ ] Evaluate usefulness of as_elt() function
// [ ] operator(i, j) (should be same as operator[]!) return type const& in matrix is strange but necessary for matricies of matricies. Rethink together with operator(i,j) globally!
//     Can matrix return byref<matrix> as a value for matrix<matrix>> ??? -> matrix<matrix> is NOT supported!
// [ ] Rename elt => element (as_element, element_cast<> ...)
// [ ] Rename expression_type => expr_eval/return_type ?
// [ ] Rename u, v <=> lhs, rhs, other, e?
// [ ] Add dimension limitations to expressions where suitable (many places!).
//     template<typename E1, typename E2, std::size_t M, std::size_t N>
//     constexpr auto function(expression<E1, M, N>, expressio<E2, N, M>) ...
//     This should improve compiler error messages!
// [ ] Add expression binding (lightweight expression binding loose variables to an expression without copying)
// [ ] Add StorageType to matrix template (allow allocator) includes 1d <=> 2d conversion
// [ ] math (matrix) project + test project (separate from idun)?
// [ ] Move all operators to global namespace to avoid 'using idun'?
// [ ] Review and cleanup of all code
// [ ] Check error messages, improve?
// [ ] Remove const from expression type (E) in reference_traits. Should not store const expressions. Will allow for lupdecompose and friends.
//     Also use reference_traits in operation ctor to distinguish reference from value and trying to deduce const-ness...
//     Specialize const versions of reference_traits separately?
//
// Application Features:
// [ ] Bounding box regression
//     Based on distance to bb origin + angles (theta, phi) [normalized]
//     Need a custom loss function (trick is to add loss from regression only if return represents an object)
//     https://towardsdatascience.com/advanced-keras-constructing-complex-custom-losses-and-metrics-c07ca130a618
//     Clustering
//     https://www.dlology.com/blog/how-to-do-unsupervised-clustering-with-keras/
// [ ] Occupancy grid implementation
//     From "Probabilistic Robotics"
//     https://github.com/ydsf16/occ_grid_mapping
//     https://zhuanlan.zhihu.com/p/42995269
//     https://zhuanlan.zhihu.com/slamTech
//     Papers
//     http://mediatum.ub.tum.de/doc/1287438/document.pdf
//     http://kth.diva-portal.org/smash/get/diva2:1366449/FULLTEXT01.pdf
//     Training
//     https://www.coursera.org/lecture/motion-planning-self-driving-cars/lesson-1-occupancy-grids-oJcwU
//     [ ] rosbag file reader
// [ ] t3l (tiny tensor template library)
//     Implemented as separate project for now. Will be included as development is completed.
//     http://www.goldsborough.me/cuda/ml/cudnn/c++/2017/10/01/14-37-23-convolutions_with_cudnn/
//     https://docs.nvidia.com/deeplearning/sdk/cudnn-archived/cudnn_701/cudnn-user-guide/index.html#cudnnActivationForward
// [ ] OpenGL Visualization (based on grim?)
// [ ] Extended Kalman Filter
//     https://en.wikipedia.org/wiki/Extended_Kalman_filter
// [ ] Multi-object tracking support
//     https://cv-tricks.com/object-tracking/quick-guide-mdnet-goturn-rolo/
//     https://en.wikipedia.org/wiki/Logit
//     https://en.wikipedia.org/wiki/Mahalanobis_distance
//     Multi-object tracking using LSTM network with full frame input with coded ids for detected objects.
// [ ] latlong => UTM conversion (rater specialized but useful for localization)
//     https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html
// [ ] Weights for errors in loss function based on angle and distance to object.
//     Could differentiate between more important 

// Design idea: no automatic optimization of expression trees, use eval() to avoid aliasing or calculation 
// of results multiple times. Always build a tree for lazy evaluation by default and use eval() to
// create temporaries where you want/need as an optimization. Preferred to let the compiler optimize!
// Always keep references to matirices (constants???) and expressions by value. Use value()/reference()
// to change the default behaviour.

// https://en.cppreference.com/w/cpp/utility/functional/ref
// https://stackoverflow.com/questions/56261171/c-expression-templates-lifetime
// https://medium.com/@dr3wc/understanding-move-semantics-and-perfect-forwarding-part-2-6b8266b6cfa4
// https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
// https://github.com/zmij/math/tree/develop/include/psst/math

// T = [ M, T ] => represents rotation followed by translation
//       0, 1

// convention: v' = T * v
// 
// TransformedVector = TranslationMatrix * RotationMatrix * ScaleMatrix * OriginalVector;
// This line actually performs the scaling FIRST, and THEN the rotation, and THEN the translation.This is how matrix multiplication works.

// rotate around point : Tuv*R*T-u-v
// http://planning.cs.uiuc.edu/node99.html
// https://eigen.tuxfamily.org/dox-devel/classEigen_1_1Transform.html


// http://mathforcollege.com/nm/mws/gen/04sle/mws_gen_sle_txt_cholesky.pdf
// http://mathforcollege.com/nm/mws/gen/04sle/
// http://nm.mathforcollege.com/topics/cholesky_ldlt.html
// http://nm.mathforcollege.com/topics/primer_sle.html

	//auto const time1 = std::chrono::high_resolution_clock::now();
	//for (int i = 0; i < 100000000; ++i) {
	//	//sink(inverse_affinity(
	//	//	mat4i{
	//	//		0, 1, 0, 10,
	//	//		-1, 0, 0, 20,
	//	//		0, 0, 1, 30,
	//	//		0, 0, 0, 1
	//	//	}));
	//}
	//auto const time2 = std::chrono::high_resolution_clock::now();
	//for (int i = 0; i < 100000000; ++i) {
	//	//sink(opttest2(
	//	//	mat4i{
	//	//		0, 1, 0, 10,
	//	//		-1, 0, 0, 20,
	//	//		0, 0, 1, 30,
	//	//		0, 0, 0, 1
	//	//	}));
	//}
	//auto const time3 = std::chrono::high_resolution_clock::now();

	//std::cout << "opttest1 duration: " << std::chrono::duration_cast<std::chrono::milliseconds>(time2 - time1).count() << " (ms)" << std::endl;
	//std::cout << "opttest2 duration: " << std::chrono::duration_cast<std::chrono::milliseconds>(time3 - time2).count() << " (ms)" << std::endl;



	//std::cout << typeid(detail::is_expr_t<int>).name() << std::endl;
	//std::cout << typeid(detail::is_expr_t<mat3i>).name() << std::endl;
	//std::cout << typeid(detail::is_expr_t<detail::add<mat3i, mat3i>::elt_t>).name() << std::endl;
	//std::cout << typeid(detail::is_expr_t<detail::multiply<matrix<mat3i, 2, 2>, matrix<mat3i, 2, 2>>::elt_t>).name() << std::endl;

	// Non-expression
	//std::cout << typeid(detail::expr_type_t<int>).name() << std::endl << std::endl;

	// Expressions
	//print_eval<mat3i>();
	//print_eval<matrix<mat3i, 4, 4>>();
	//print_eval<detail::add<mat3i, mat3i>>();
	//print_eval<detail::negate<mat3i>>();
	//print_eval<detail::multiply<matrix<mat3i, 2, 2>, matrix<mat3i, 2, 2>>>();
	//print_eval<detail::multiply<matrix<matrix<mat3i, 2, 2>, 2, 2>, matrix<matrix<mat3i, 2, 2>, 2 ,2>>>();

	//auto c32 = as_element(normsq(vec3f{ 1.0f, 2.0f, 3.0f }));
	//auto c33 = eval(normsq(transpose(vec3f{ 1.0f, 2.0f, 3.0f })));


	//matrix<double, 0, 0> test = identity<double, 0>();

	//mat3f w0={0,1,2}; // compiler error - "matrix dimension mismatch"

	//vec3f v1{ 10.0f, 20.0f, 30.0f };
	//matrix<int, 3, 1> v2; // compiler warning "unreferenced local variable"
	//v2 = v1; // compiler warning "possible loss of data"
	//v1 = v2; // compiler warning "possible loss of data"


#if(0)

#include <chrono>

template<typename T> void print_eval() {
	std::cout << typeid(T).name() << std::endl;
	std::cout << typeid(T::elt_t).name() << std::endl;
	std::cout << typeid(detail::expr_type_t<T::elt_t>).name() << std::endl;
	std::cout << std::endl;
}

//////////////////////////////////////////////////////////





template<typename T>
struct Mat_ {
	Mat_(std::size_t i, std::size_t j) {}
};

using Mat = Mat_<double>

struct Vec3f {
};


// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f& theta)
{
	// Calculate rotation about x axis
	Mat R_x = (Mat_<double>(3, 3) <<
		1, 0, 0,
		0, cos(theta[0]), -sin(theta[0]),
		0, sin(theta[0]), cos(theta[0])
		);

	// Calculate rotation about y axis
	Mat R_y = (Mat_<double>(3, 3) <<
		cos(theta[1]), 0, sin(theta[1]),
		0, 1, 0,
		-sin(theta[1]), 0, cos(theta[1])
		);

	// Calculate rotation about z axis
	Mat R_z = (Mat_<double>(3, 3) <<
		cos(theta[2]), -sin(theta[2]), 0,
		sin(theta[2]), cos(theta[2]), 0,
		0, 0, 1);


	// Combined rotation matrix
	Mat R = R_z * R_y * R_x;

	return R;

}

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat& R)
{
	Mat Rt;
	transpose(R, Rt);
	Mat shouldBeIdentity = Rt * R;
	Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

	return  norm(I, shouldBeIdentity) < 1e-6;

}

// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat& R)
{

	assert(isRotationMatrix(R));

	float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));

	bool singular = sy < 1e-6; // If

	float x, y, z;
	if (!singular)
	{
		x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
		y = atan2(-R.at<double>(2, 0), sy);
		z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
	}
	else
	{
		x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
		y = atan2(-R.at<double>(2, 0), sy);
		z = 0;
	}
	return Vec3f(x, y, z);



}



///////////////////////////////////////////////////////////////////////////////////////////

http://docs.ros.org/api/tf/html/c++/Matrix3x3_8h_source.html#l00180

//void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX) {
		//	tfScalar ci(tfCos(eulerX));
		//	tfScalar cj(tfCos(eulerY));
		//	tfScalar ch(tfCos(eulerZ));
		//	tfScalar si(tfSin(eulerX));
		//	tfScalar sj(tfSin(eulerY));
		//	tfScalar sh(tfSin(eulerZ));
		//	tfScalar cc = ci * ch;
		//	tfScalar cs = ci * sh;
		//	tfScalar sc = si * ch;
		//	tfScalar ss = si * sh;

		//	setValue(cj * ch, sj * sc - cs, sj * cc + ss,
		//		cj * sh, sj * ss + cc, sj * cs - sc,
		//		-sj, cj * si, cj * ci);
		//}

template<typename T>
Eigen::Matrix3<T> setEulerYPR(T eulerZ, T eulerY, T eulerX) {
	T ci(cos(eulerX));
	T cj(cos(eulerY));
	T ch(cos(eulerZ));
	T si(sin(eulerX));
	T sj(sin(eulerY));
	T sh(sin(eulerZ));
	T cc = ci * ch;
	T cs = ci * sh;
	T sc = si * ch;
	T ss = si * sh;

	Eigen::Matrix3<T> rot;
	rot << cj * ch, sj* sc - cs, sj* cc + ss,
		cj* sh, sj* ss + cc, sj* cs - sc,
		-sj, cj* si, cj* ci;

	return rot;
}



#endif