cpp: introducing templates (and static_asserts)

git-svn-id: http://svnsrv.desy.de/public/GeneralBrokenLines/trunk@132 281f6f2b-e318-4fd1-8bce-1a4ba7aab212

cpp/CMakeLists.txt

@@ -6,8 +6,8 @@ PROJECT(GBL)
 
 # project version
 SET( ${PROJECT_NAME}_VERSION_MAJOR 2 )
-SET( ${PROJECT_NAME}_VERSION_MINOR 0 )
-SET( ${PROJECT_NAME}_VERSION_PATCH 1 )
+SET( ${PROJECT_NAME}_VERSION_MINOR 1 )
+SET( ${PROJECT_NAME}_VERSION_PATCH 0 )
 
 # make life easier and simply use the ilcsoft default settings
 # load default ilcsoft settings (install prefix, build type, rpath, etc.)

cpp/include/GblData.h

@@ -9,9 +9,11 @@
  *  GblData definition.
  *
  *  \author Claus Kleinwort, DESY, 2011 (Claus.Kleinwort@desy.de)
+ *  \author Gregor Mittag, DESY, 2017 (templates and other optimizations)
+ *
  *
  *  \copyright
- *  Copyright (c) 2011 - 2017 Deutsches Elektronen-Synchroton,
+ *  Copyright (c) 2011 - 2016 Deutsches Elektronen-Synchroton,
  *  Member of the Helmholtz Association, (DESY), HAMBURG, GERMANY \n\n
  *  This library is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU Library General Public License as
@@ -32,6 +34,7 @@
 
 #include<iostream>
 #include<vector>
+#include <array>
 #include<math.h>
 #include "VMatrix.h"
 
@@ -56,14 +59,49 @@ class GblData {
 public:
 	GblData(unsigned int aLabel, dataBlockType aType, double aMeas,
 			double aPrec, unsigned int aTraj = 0, unsigned int aPoint = 0);
+	GblData(const GblData&) = default;
+	GblData& operator=(const GblData&) = default;
+	GblData(GblData&&) = default;
+	GblData& operator=(GblData&&) = default;
 	virtual ~GblData();
+
+	/// Add derivatives from measurement.
+	/**
+	 * Add (non-zero) derivatives to data block. Fill list of labels of used fit parameters.
+	 * \tparam LocalDerivative  Local derivatives matrix
+	 * \tparam ExtDerivative  External derivatives matrix
+	 * \param [in] iRow Row index (0-4) in up to 5D measurement
+	 * \param [in] labDer Labels for derivatives
+	 * \param [in] matDer Derivatives (matrix) 'measurement vs track fit parameters'
+	 * \param [in] iOff Offset for row index for additional parameters
+	 * \param [in] derLocal Derivatives (matrix) for additional local parameters
+	 * \param [in] extOff Offset for external parameters
+	 * \param [in] extDer Derivatives for external Parameters
+	 */
+	template<typename LocalDerivative, typename ExtDerivative>
 	void addDerivatives(unsigned int iRow,
-			const std::vector<unsigned int> &labDer, const Matrix5d &matDer,
-			unsigned int iOff, const Eigen::MatrixXd &derLocal,
-			unsigned int nLocal, const Eigen::MatrixXd &derTrans);
+			const std::array<unsigned int, 5>& labDer, const Matrix5d &matDer,
+			unsigned int iOff,
+			const Eigen::MatrixBase<LocalDerivative>& derLocal,
+			unsigned int extOff,
+			const Eigen::MatrixBase<ExtDerivative>& extDer);
+
+	/// Add derivatives from kink.
+	/**
+	 * Add (non-zero) derivatives to data block. Fill list of labels of used fit parameters.
+	 * \tparam ExtDerivative  External derivatives matrix
+	 * \param [in] iRow Row index (0-1) in 2D kink
+	 * \param [in] labDer Labels for derivatives
+	 * \param [in] matDer Derivatives (matrix) 'kink vs track fit parameters'
+	 * \param [in] extOff Offset for external parameters
+	 * \param [in] extDer Derivatives for external Parameters
+	 */
+	template<typename ExtDerivative>
 	void addDerivatives(unsigned int iRow,
-			const std::vector<unsigned int> &labDer, const Matrix27d &matDer,
-			unsigned int nLocal, const Eigen::MatrixXd &derTrans);
+			const std::array<unsigned int, 7>& labDer, const Matrix27d &matDer,
+			unsigned int extOff,
+			const Eigen::MatrixBase<ExtDerivative>& extDer);
+
 	void addDerivatives(const std::vector<unsigned int> &index,
 			const std::vector<double> &derivatives);
 
@@ -100,5 +138,113 @@ private:
 	std::vector<unsigned int> moreParameters; ///< List of fit parameters (with non zero derivatives)
 	std::vector<double> moreDerivatives; ///< List of derivatives for fit
 };
+
+template<typename LocalDerivative, typename ExtDerivative>
+void GblData::addDerivatives(unsigned int iRow,
+		const std::array<unsigned int, 5>& labDer, const Matrix5d &matDer,
+		unsigned int iOff, const Eigen::MatrixBase<LocalDerivative>& derLocal,
+		unsigned int extOff, const Eigen::MatrixBase<ExtDerivative>& extDer) {
+
+	unsigned int nParMax = 5 + derLocal.cols() + extDer.cols();
+	theRow = iRow - iOff;
+	if (nParMax > 7) {
+		// dynamic data block size
+		moreParameters.reserve(nParMax); // have to be sorted
+		moreDerivatives.reserve(nParMax);
+
+		for (int i = 0; i < derLocal.cols(); ++i) // local derivatives
+				{
+			if (derLocal(iRow - iOff, i)) {
+				moreParameters.push_back(i + 1);
+				moreDerivatives.push_back(derLocal(iRow - iOff, i));
+			}
+		}
+
+		for (int i = 0; i < extDer.cols(); ++i) // external derivatives
+				{
+			if (extDer(iRow - iOff, i)) {
+				moreParameters.push_back(extOff + i + 1);
+				moreDerivatives.push_back(extDer(iRow - iOff, i));
+			}
+		}
+
+		for (size_t i = 0; i < labDer.size(); ++i) // curvature, offset derivatives
+				{
+			if (labDer[i] and matDer(iRow, i)) {
+				moreParameters.push_back(labDer[i]);
+				moreDerivatives.push_back(matDer(iRow, i));
+			}
+		}
+	} else {
+		// simple (static)  data block
+		for (int i = 0; i < derLocal.cols(); ++i) // local derivatives
+				{
+			if (derLocal(iRow - iOff, i)) {
+				theParameters[theNumLocal] = i + 1;
+				theDerivatives[theNumLocal] = derLocal(iRow - iOff, i);
+				theNumLocal++;
+			}
+		}
+
+		for (int i = 0; i < extDer.cols(); ++i) // external derivatives
+				{
+			if (extDer(iRow - iOff, i)) {
+				theParameters[theNumLocal] = extOff + i + 1;
+				theDerivatives[theNumLocal] = extDer(iRow - iOff, i);
+				theNumLocal++;
+			}
+		}
+		for (size_t i = 0; i < labDer.size(); ++i) // curvature, offset derivatives
+				{
+			if (labDer[i] and matDer(iRow, i)) {
+				theParameters[theNumLocal] = labDer[i];
+				theDerivatives[theNumLocal] = matDer(iRow, i);
+				theNumLocal++;
+			}
+		}
+	}
 }
+
+template<typename ExtDerivative>
+void GblData::addDerivatives(unsigned int iRow,
+		const std::array<unsigned int, 7>& labDer, const Matrix27d &matDer,
+		unsigned int extOff, const Eigen::MatrixBase<ExtDerivative>& extDer) {
+
+	unsigned int nParMax = 7 + extDer.cols();
+	theRow = iRow;
+	if (nParMax > 7) {
+		// dynamic data block size
+		moreParameters.reserve(nParMax); // have to be sorted
+		moreDerivatives.reserve(nParMax);
+
+		for (int i = 0; i < extDer.cols(); ++i) // external derivatives
+				{
+			if (extDer(iRow, i)) {
+				moreParameters.push_back(extOff + i + 1);
+				moreDerivatives.push_back(extDer(iRow, i));
+			}
+		}
+
+		for (size_t i = 0; i < labDer.size(); ++i) // curvature, offset derivatives
+				{
+			if (labDer[i] and matDer(iRow, i)) {
+				moreParameters.push_back(labDer[i]);
+				moreDerivatives.push_back(matDer(iRow, i));
+			}
+		}
+	} else {
+		// simple (static) data block
+		for (size_t i = 0; i < labDer.size(); ++i) // curvature, offset derivatives
+				{
+			if (labDer[i] and matDer(iRow, i)) {
+				theParameters[theNumLocal] = labDer[i];
+				theDerivatives[theNumLocal] = matDer(iRow, i);
+				theNumLocal++;
+			}
+		}
+	}
+}
+
+}
+
 #endif /* GBLDATA_H_ */

cpp/include/GblPoint.h

@@ -9,6 +9,8 @@
  *  GblPoint definition.
  *
  *  \author Claus Kleinwort, DESY, 2011 (Claus.Kleinwort@desy.de)
+ *  \author Gregor Mittag, DESY, 2017 (templates and other optimizations)
+ *
  *
  *  \copyright
  *  Copyright (c) 2011 - 2017 Deutsches Elektronen-Synchroton,
@@ -67,6 +69,10 @@ typedef Eigen::Matrix<double, 5, 5> Matrix5d;
 class GblPoint {
 public:
 	GblPoint(const Matrix5d &aJacobian);
+	GblPoint(const GblPoint&) = default;
+	GblPoint& operator=(const GblPoint&) = default;
+	GblPoint(GblPoint&&) = default;
+	GblPoint& operator=(GblPoint&&) = default;
 	virtual ~GblPoint();
 #ifdef GBL_EIGEN_SUPPORT_ROOT
 	// input via ROOT
@@ -87,16 +93,144 @@ public:
 			const TMatrixD &aDerivatives);
 #endif
 	// input via Eigen
-	void addMeasurement(const Eigen::MatrixXd &aProjection,
-			const Eigen::VectorXd &aResiduals,
-			const Eigen::MatrixXd &aPrecision, double minPrecision = 0.);
-	void addMeasurement(const Eigen::VectorXd &aResiduals,
-			const Eigen::MatrixXd &aPrecision, double minPrecision = 0.);
+
+	/// Add a measurement to a point.
+	/**
+	 * Add measurement (in measurement system) with arbitrary precision (inverse covariance) matrix.
+	 * Will be diagonalized.
+	 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
+	 * \tparam Projection  Projection matrix
+	 * \tparam Residuals   Residuals vector
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aProjection Projection from local to measurement system (derivative of measurement vs local parameters)
+	 * \param [in] aResiduals Measurement residuals
+	 * \param [in] aPrecision Measurement precision (matrix)
+	 * \param [in] minPrecision Minimal precision to accept measurement
+	 */
+	template<typename Projection, typename Residuals, typename Precision,
+			typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type* =
+					nullptr>
+	void addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
+			const Eigen::MatrixBase<Residuals>& aResiduals,
+			const Eigen::MatrixBase<Precision>& aPrecision,
+			double minPrecision = 0.);
+
+	/// Add a measurement to a point.
+	/**
+	 * Add measurement (in measurement system) with diagonal precision (inverse covariance) matrix.
+	 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
+	 * \tparam Projection  Projection matrix
+	 * \tparam Residuals   Residuals vector
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aProjection Projection from local to measurement system (derivative of measurement vs local parameters)
+	 * \param [in] aResiduals Measurement residuals
+	 * \param [in] aPrecision Measurement precision (vector with diagonal)
+	 * \param [in] minPrecision Minimal precision to accept measurement
+	 */
+	template<typename Projection, typename Residuals, typename Precision,
+			typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type* =
+					nullptr>
+	void addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
+			const Eigen::MatrixBase<Residuals>& aResiduals,
+			const Eigen::MatrixBase<Precision>& aPrecision,
+			double minPrecision = 0.);
+
+	/// Add a measurement to a point.
+	/**
+	 * Add measurement in local system with arbitrary precision (inverse covariance) matrix.
+	 * Will be diagonalized.
+	 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
+	 * \tparam Residuals   Residuals vector
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aResiduals Measurement residuals
+	 * \param [in] aPrecision Measurement precision (matrix)
+	 * \param [in] minPrecision Minimal precision to accept measurement
+	 */
+	template<typename Residuals, typename Precision, typename std::enable_if<
+			(Precision::ColsAtCompileTime != 1)>::type* = nullptr>
+	void addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
+			const Eigen::MatrixBase<Precision>& aPrecision,
+			double minPrecision = 0.);
+
+	/// Add a measurement to a point.
+	/**
+	 * Add measurement in local system with diagonal precision (inverse covariance) matrix.
+	 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
+	 * \tparam Residuals   Residuals vector
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aResiduals Measurement residuals
+	 * \param [in] aPrecision Measurement precision (vector with diagonal)
+	 * \param [in] minPrecision Minimal precision to accept measurement
+	 */
+	template<typename Residuals, typename Precision, typename std::enable_if<
+			(Precision::ColsAtCompileTime == 1)>::type* = nullptr>
+	void addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
+			const Eigen::MatrixBase<Precision>& aPrecision,
+			double minPrecision = 0.);
+
+	/// Add a (thin) scatterer to a point.
+	/**
+	 * Add scatterer with arbitrary precision (inverse covariance) matrix.
+	 * Will be diagonalized. Changes local track direction.
+	 *
+	 * The precision matrix for the local slopes is defined by the
+	 * angular scattering error theta_0 and the scalar products c_1, c_2 of the
+	 * offset directions in the local frame with the track direction:
+	 *
+	 *            (1 - c_1*c_1 - c_2*c_2)   |  1 - c_1*c_1     - c_1*c_2  |
+	 *       P =  ----------------------- * |                             |
+	 *                theta_0*theta_0       |    - c_1*c_2   1 - c_2*c_2  |
+	 *
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aResiduals Scatterer residuals
+	 * \param [in] aPrecision Scatterer precision (full matrix)
+	 */
+	template<typename Precision, typename std::enable_if<
+			(Precision::ColsAtCompileTime == 2)>::type* = nullptr>
+	void addScatterer(const Eigen::Vector2d &aResiduals,
+			const Eigen::MatrixBase<Precision>& aPrecision);
+
+	/// Add a (thin) scatterer to a point.
+	/**
+	 * Add scatterer with diagonal precision (inverse covariance) matrix.
+	 * Changes local track direction.
+	 *
+	 * The precision matrix for the local slopes is defined by the
+	 * angular scattering error theta_0 and the scalar products c_1, c_2 of the
+	 * offset directions in the local frame with the track direction:
+	 *
+	 *            (1 - c_1*c_1 - c_2*c_2)   |  1 - c_1*c_1     - c_1*c_2  |
+	 *       P =  ----------------------- * |                             |
+	 *                theta_0*theta_0       |    - c_1*c_2   1 - c_2*c_2  |
+	 *
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aResiduals Scatterer residuals
+	 * \param [in] aPrecision Scatterer precision (vector with diagonal)
+	 */
+	template<typename Precision, typename std::enable_if<
+			(Precision::ColsAtCompileTime == 1)>::type* = nullptr>
 	void addScatterer(const Eigen::Vector2d &aResiduals,
-			const Eigen::MatrixXd &aPrecision);
-	void addLocals(const Eigen::MatrixXd &aDerivatives);
+			const Eigen::MatrixBase<Precision>& aPrecision);
+
+	/// Add local derivatives to a point.
+	/**
+	 * Point needs to have a measurement.
+	 * \tparam Derivative  Derivatives matrix
+	 * \param [in] aDerivatives Local derivatives (matrix)
+	 */
+	template<typename Derivative>
+	void addLocals(const Eigen::MatrixBase<Derivative>& aDerivatives);
+	template<typename Derivative>
+
+	/// Add global derivatives to a point.
+	/**
+	 * Point needs to have a measurement.
+	 * \tparam Derivative  Derivatives matrix
+	 * \param [in] aLabels Global derivatives labels
+	 * \param [in] aDerivatives Global derivatives (matrix)
+	 */
 	void addGlobals(const std::vector<int> &aLabels,
-			const Eigen::MatrixXd &aDerivatives);
+			const Eigen::MatrixBase<Derivative>& aDerivatives);
 	//
 	unsigned int hasMeasurement() const;
 	double getMeasPrecMin() const;
@@ -136,10 +270,12 @@ private:
 	unsigned int measDim; ///< Dimension of measurement (1-5), 0 indicates absence of measurement
 	double measPrecMin; ///< Minimal measurement precision (for usage)
 	Matrix5d measProjection; ///< Projection from measurement to local system
+
 	Vector5d measResiduals; ///< Measurement residuals
 	Vector5d measPrecision; ///< Measurement precision (diagonal of inverse covariance matrix)
 	bool transFlag; ///< Transformation exists?
-	Eigen::MatrixXd measTransformation; ///< Transformation of diagonalization (of meas. precision matrix)
+	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+			Eigen::ColMajor /* default */, 5, 5> measTransformation; ///< Transformation of diagonalization (of meas. precision matrix)
 	bool scatFlag; ///< Scatterer present?
 	Eigen::Matrix2d scatTransformation; ///< Transformation of diagonalization (of scat. precision matrix)
 	Eigen::Vector2d scatResiduals; ///< Scattering residuals (initial kinks if iterating)
@@ -148,5 +284,135 @@ private:
 	std::vector<int> globalLabels; ///< Labels of global (MP-II) derivatives
 	Eigen::MatrixXd globalDerivatives; ///< Derivatives of measurement vs additional global (MP-II) parameters
 };
+
+template<typename Projection, typename Residuals, typename Precision,
+		typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type*>
+void GblPoint::addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
+		const Eigen::MatrixBase<Residuals>& aResiduals,
+		const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
+	static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Projection::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Projection) must be equal");
+	static_assert(static_cast<int>(Precision::RowsAtCompileTime) == static_cast<int>(Precision::ColsAtCompileTime), "addMeasurement: rows(Precision) and cols(Precision) must be equal");
+	static_assert(static_cast<int>(Projection::RowsAtCompileTime) == static_cast<int>(Projection::ColsAtCompileTime), "addMeasurement: rows(Projection) and cols(Projection) must be equal");
+	measDim = aResiduals.rows();
+	measPrecMin = minPrecision;
+	// arbitrary precision matrix
+	Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> measEigen {
+			aPrecision };
+	measTransformation = measEigen.eigenvectors().transpose();
+	transFlag = true;
+	measResiduals.tail(measDim) = measTransformation * aResiduals;
+	measPrecision.tail(measDim) = measEigen.eigenvalues();
+	measProjection.bottomRightCorner(measDim, measDim) = measTransformation
+			* aProjection;
+}
+
+template<typename Projection, typename Residuals, typename Precision,
+		typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type*>
+void GblPoint::addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
+		const Eigen::MatrixBase<Residuals>& aResiduals,
+		const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
+	static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Projection::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Projection) must be equal");
+	static_assert(static_cast<int>(Projection::RowsAtCompileTime) == static_cast<int>(Projection::ColsAtCompileTime), "addMeasurement: rows(Projection) and cols(Projection) must be equal");
+	measDim = aResiduals.rows();
+	measPrecMin = minPrecision;
+	// diagonal precision matrix
+	measResiduals.tail(measDim) = aResiduals;
+	measPrecision.tail(measDim) = aPrecision;
+	measProjection.bottomRightCorner(measDim, measDim) = aProjection;
+}
+
+template<typename Residuals, typename Precision, typename std::enable_if<
+		(Precision::ColsAtCompileTime != 1)>::type*>
+void GblPoint::addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
+		const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
+	static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
+	measDim = aResiduals.rows();
+	measPrecMin = minPrecision;
+	// arbitrary precision matrix
+	Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> measEigen {
+			aPrecision };
+	measTransformation = measEigen.eigenvectors().transpose();
+	transFlag = true;
+	measResiduals.tail(measDim) = measTransformation * aResiduals;
+	measPrecision.tail(measDim) = measEigen.eigenvalues();
+	measProjection.bottomRightCorner(measDim, measDim) = measTransformation;
+}
+
+template<typename Residuals, typename Precision, typename std::enable_if<
+		(Precision::ColsAtCompileTime == 1)>::type*>
+void GblPoint::addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
+		const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
+	static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
+	static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
+	measDim = aResiduals.rows();
+	measPrecMin = minPrecision;
+	// diagonal precision matrix
+	measResiduals.tail(measDim) = aResiduals;
+	measPrecision.tail(measDim) = aPrecision;
+	measProjection.setIdentity();
+}
+
+template<typename Precision, typename std::enable_if<
+		(Precision::ColsAtCompileTime == 2)>::type*>
+void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
+		const Eigen::MatrixBase<Precision>& aPrecision) {
+	static_assert(static_cast<int>(Precision::RowsAtCompileTime) == 2 or static_cast<int>(Precision::RowsAtCompileTime) == Eigen::Dynamic, "addScatterer: rows(Precision) must be 2 or dynamic");
+	scatFlag = true;
+	// arbitrary precision matrix
+	Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> scatEigen {
+			aPrecision };
+	scatTransformation = scatEigen.eigenvectors();
+	scatTransformation.transposeInPlace();
+	scatResiduals = scatTransformation * aResiduals;
+	scatPrecision = scatEigen.eigenvalues();
+}
+
+template<typename Precision, typename std::enable_if<
+		(Precision::ColsAtCompileTime == 1)>::type*>
+void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
+		const Eigen::MatrixBase<Precision>& aPrecision) {
+	static_assert(static_cast<int>(Precision::RowsAtCompileTime) == 2 or static_cast<int>(Precision::RowsAtCompileTime) == Eigen::Dynamic, "addScatterer: rows(Precision) must be 2 or dynamic");
+	scatFlag = true;
+	scatResiduals = aResiduals;
+	scatPrecision = aPrecision;
+	scatTransformation.setIdentity();
+}
+
+template<typename Derivative>
+void GblPoint::addLocals(const Eigen::MatrixBase<Derivative>& aDerivatives) {
+	if (measDim) {
+		localDerivatives.resize(aDerivatives.rows(), aDerivatives.cols());
+		if (transFlag) {
+			localDerivatives = measTransformation * aDerivatives;
+		} else {
+			localDerivatives = aDerivatives;
+		}
+	}
+}
+
+template<typename Derivative>
+void GblPoint::addGlobals(const std::vector<int> &aLabels,
+		const Eigen::MatrixBase<Derivative>& aDerivatives) {
+	if (measDim) {
+		globalLabels = aLabels;
+		globalDerivatives.resize(aDerivatives.rows(), aDerivatives.cols());
+		if (transFlag) {
+			globalDerivatives = measTransformation * aDerivatives;
+		} else {
+			globalDerivatives = aDerivatives;
+		}
+
+	}
+}
+
 }
 #endif /* GBLPOINT_H_ */

cpp/include/GblTrajectory.h

@@ -9,6 +9,8 @@
  *  GblTrajectory definition.
  *
  *  \author Claus Kleinwort, DESY, 2011 (Claus.Kleinwort@desy.de)
+ *  \author Gregor Mittag, DESY, 2017 (templates and other optimizations)
+ *
  *
  *  \copyright
  *  Copyright (c) 2011 - 2017 Deutsches Elektronen-Synchroton,
@@ -30,6 +32,7 @@
 #ifndef GBLTRAJECTORY_H_
 #define GBLTRAJECTORY_H_
 
+#include <array>
 #include "GblPoint.h"
 #include "GblData.h"
 #include "GblPoint.h"
@@ -48,29 +51,80 @@ class GblTrajectory {
 public:
 	GblTrajectory(const std::vector<GblPoint> &aPointList, bool flagCurv = true,
 			bool flagU1dir = true, bool flagU2dir = true);
+	GblTrajectory(
+			const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList);
+
+	/// Create new (simple) trajectory from list of points with external seed.
+	/**
+	 * Curved trajectory in space (default) or without curvature (q/p) or in one
+	 * plane (u-direction) only.
+	 * \tparam Seed   Seed precision matrix
+	 * \param [in] aPointList List of points
+	 * \param [in] aLabel (Signed) label of point for external seed
+	 * (<0: in front, >0: after point, slope changes at scatterer!)
+	 * \param [in] aSeed Precision matrix of external seed
+	 * \param [in] flagCurv Use q/p
+	 * \param [in] flagU1dir Use in u1 direction
+	 * \param [in] flagU2dir Use in u2 direction
+	 */
+	template<typename Seed>
 	GblTrajectory(const std::vector<GblPoint> &aPointList, unsigned int aLabel,
-			const Eigen::MatrixXd &aSeed, bool flagCurv = true, bool flagU1dir =
-					true, bool flagU2dir = true);
+			const Eigen::MatrixBase<Seed>& aSeed, bool flagCurv = true,
+			bool flagU1dir = true, bool flagU2dir = true);
+
+	/// Create new composed trajectory from list of points and transformations with arbitrary external measurements.
+	/**
+	 * Composed of curved trajectories in space. The precision matrix for the external measurements is specified as matrix.
+	 *
+	 * \tparam Derivatives  External derivatives
+	 * \tparam Measurements   Residuals vector
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aPointsAndTransList List containing pairs with list of points and transformation (at inner (first) point)
+	 * \param [in] extDerivatives Derivatives of external measurements vs external parameters
+	 * \param [in] extMeasurements External measurements (residuals)
+	 * \param [in] extPrecisions Precision of external measurements (matrix)
+	 */
+	template<typename Derivatives, typename Measurements, typename Precision,
+			typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type* =
+					nullptr>
 	GblTrajectory(
-			const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> > &aPointaAndTransList);
+			const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList,
+			const Eigen::MatrixBase<Derivatives>& extDerivatives,
+			const Eigen::MatrixBase<Measurements>& extMeasurements,
+			const Eigen::MatrixBase<Precision>& extPrecisions);
+
+	/// Create new composed trajectory from list of points and transformations with independent external measurements.
+	/**
+	 * Composed of curved trajectories in space. The (diagonal) precision matrix for the external measurements is specified as vector
+	 * (containing the diagonal).
+	 *
+	 * \tparam Derivatives  External derivatives
+	 * \tparam Measurements   Residuals vector
+	 * \tparam Precision   Precision matrix or vector (with diagonal)
+	 * \param [in] aPointsAndTransList List containing pairs with list of points and transformation (at inner (first) point)
+	 * \param [in] extDerivatives Derivatives of external measurements vs external parameters
+	 * \param [in] extMeasurements External measurements (residuals)
+	 * \param [in] extPrecisions Precision of external measurements (vector with diagonal)
+	 */
+	template<typename Derivatives, typename Measurements, typename Precision,
+			typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type* =