diff --git a/aslam_optimizer/aslam_backend_expressions/src/MatrixTransformation.cpp b/aslam_optimizer/aslam_backend_expressions/src/MatrixTransformation.cpp index 3397de6be..e3a4759b5 100644 --- a/aslam_optimizer/aslam_backend_expressions/src/MatrixTransformation.cpp +++ b/aslam_optimizer/aslam_backend_expressions/src/MatrixTransformation.cpp @@ -1,4 +1,3 @@ - #include #include #include @@ -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(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(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(i/3)))) += _UpdatePattern(i%3,int(floor(static_cast(i/3))))*dp[j]; - if (int(_UpdatePattern(i%3,int(floor(static_cast(i/3)))))==1){j++; } + _A(i%3,int(floor(static_cast(i/3)))) += _UpdatePattern(i%3,int(floor(static_cast(i/3))))*dp[j]; + if (_UpdatePattern(i%3,int(floor(static_cast(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(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(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(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(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 -