changed format to original formatting

ObitoLee · Aug 16, 2018 · 30da22b · 30da22b
1 parent 62d8065
commit 30da22b
Showing 1 changed file with 26 additions and 28 deletions.
diff --git a/aslam_optimizer/aslam_backend_expressions/src/MatrixTransformation.cpp b/aslam_optimizer/aslam_backend_expressions/src/MatrixTransformation.cpp
@@ -1,4 +1,3 @@
-
 #include <aslam/backend/MatrixTransformation.hpp>
 #include <sm/kinematics/rotations.hpp>
 #include <aslam/Exceptions.hpp>
@@ -8,19 +7,19 @@ namespace aslam {
   namespace backend {
 
     MatrixTransformation::MatrixTransformation(const Eigen::Matrix3d & A) : _A(A), _A_a(A){
-      _UpdatePattern << 1,1,1,1,1,1,1,1,1;      // ## general case (if no pattern is defined: every entry of the matrix will be updated
-      _UpdateDimension = 9;               // ## number of unknowns: 9 (3x3 Matrix)
+    	_UpdatePattern << 1,1,1,1,1,1,1,1,1;			// ## general case (if no pattern is defined: every entry of the matrix will be updated
+    	_UpdateDimension = 9;						    // ## number of unknowns: 9 (3x3 Matrix)
 
     }
 
     MatrixTransformation::MatrixTransformation(const Eigen::Matrix3d & A, const Eigen::Matrix3d & UpdatePattern) : _A(A), _A_a(A){
-      _UpdatePattern << UpdatePattern;              // Pattern of the Matrix;  1: design variables; 0: constants
-      _UpdateDimension = 0;
-      for (int i=0; i<9; i++){
-        if (_UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))==1){
-          _UpdateDimension ++;          // Count, how many design variables are in the matrix
-        }
-      }
+    	_UpdatePattern << UpdatePattern;              // Pattern of the Matrix;  1: design variables; 0: constants
+    	_UpdateDimension = 0;
+    	for (int i=0; i<9; i++){
+    		if (_UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))==1){
+    			_UpdateDimension ++;					// Count, how many design variables are in the matrix
+    		}
+    	}
     }
 
     MatrixTransformation::~MatrixTransformation(){}
@@ -41,8 +40,8 @@ namespace aslam {
       Eigen::Matrix3d dA;
       int j=0;
       for (int i=0; i<9; i++){
-        _A(i%3,int(floor(static_cast<double>(i/3)))) += _UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))*dp[j];
-        if (int(_UpdatePattern(i%3,int(floor(static_cast<double>(i/3)))))==1){j++; }
+    	  _A(i%3,int(floor(static_cast<double>(i/3)))) += _UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))*dp[j];
+    	  if (_UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))==1){j++; }
       }
     }
 
@@ -67,17 +66,17 @@ namespace aslam {
     void MatrixTransformation::evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const
     {
 
-      //## get the Jacobian out of the the general case for the specific Matrix-Pattern (type)
-      Eigen::MatrixXd finalJacobian(3,_UpdateDimension);
-    int j=0;
-    for (int i=0; i<9; i++){
-      if (_UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))==1){
-        SM_ASSERT_GT_DBG(aslam::Exception, _UpdateDimension, j , "Incorrect update dimension! It doesn't match the pattern");
-        finalJacobian.col(j) = applyChainRule.col(i);   // took out only the rows for the values, which are design variables
-        j++;
-      }
-    }
-    outJacobians.add( const_cast<MatrixTransformation*>(this), finalJacobian );
+    	//## get the Jacobian out of the the general case for the specific Matrix-Pattern (type)
+    	Eigen::MatrixXd finalJacobian(3,_UpdateDimension);
+		int j=0;
+		for (int i=0; i<9; i++){
+			if (_UpdatePattern(i%3,int(floor(static_cast<double>(i/3))))==1){
+				SM_ASSERT_GT_DBG(aslam::Exception, _UpdateDimension, j , "Incorrect update dimension! It doesn't match the pattern");
+				finalJacobian.col(j) = applyChainRule.col(i);		// took out only the rows for the values, which are design variables
+				j++;
+			}
+		}
+		outJacobians.add( const_cast<MatrixTransformation*>(this), finalJacobian );
     }
 
     MatrixExpression MatrixTransformation::toExpression()
@@ -103,16 +102,15 @@ namespace aslam {
 
     void MatrixTransformation::minimalDifferenceImplementation(const Eigen::MatrixXd& xHat, Eigen::VectorXd& outDifference) const
     {
-      SM_ASSERT_TRUE(aslam::InvalidArgumentException, (xHat.rows() == 3)&&(xHat.cols() == 3), "xHat has incompatible dimensions");
-        outDifference = sm::kinematics::R2AxisAngle(_A*xHat.transpose());
+    	SM_ASSERT_TRUE(aslam::InvalidArgumentException, (xHat.rows() == 3)&&(xHat.cols() == 3), "xHat has incompatible dimensions");
+    		outDifference = sm::kinematics::R2AxisAngle(_A*xHat.transpose());
     }
 
   void MatrixTransformation::minimalDifferenceAndJacobianImplementation(const Eigen::MatrixXd& xHat, Eigen::VectorXd& outDifference, Eigen::MatrixXd& /* outJacobian */) const
     {
-      minimalDifferenceImplementation(xHat, outDifference);
-      // outJacobian = ???
+    	minimalDifferenceImplementation(xHat, outDifference);
+    	// outJacobian = ???
     }
 
   } // namespace backend
 } // namespace aslam
-