TEST(SmCommonTestSuite,testAssertMacrosDbg)
{
SM_DEFINE_EXCEPTION(Exception, std::runtime_error);
{
double* val = new double;
EXPECT_NO_THROW( SM_ASSERT_TRUE_DBG(Exception, true, "") );
EXPECT_NO_THROW( SM_ASSERT_FALSE_DBG(Exception, false, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_LT_DBG(Exception, 0.0, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GT_LE_DBG(Exception, 0.1, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_LE_DBG(Exception, 0.0, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_LE_DBG(Exception, 1.0, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_LT_DBG(Exception, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GT_DBG(Exception, 1.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_POSITIVE_DBG(Exception, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NONNEGATIVE_DBG(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NEGATIVE_DBG(Exception, -1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NONPOSITIVE_DBG(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_ZERO_DBG(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NOTNULL_DBG(Exception, val, "") );
EXPECT_NO_THROW( SM_ASSERT_LE_DBG(Exception, 0.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_DBG(Exception, 0.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NE_DBG(Exception, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_EQ_DBG(Exception, 0.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NEAR_DBG(Exception, 0.0, 1.0, 2.0, "") );
EXPECT_NO_THROW( SM_ASSERT_FINITE_DBG(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NOTNAN_DBG(Exception, 0.0, "") );
delete val;
}
{
double* val = NULL;
EXPECT_THROW( SM_ASSERT_TRUE_DBG(Exception, false, ""), Exception);
EXPECT_THROW( SM_ASSERT_FALSE_DBG(Exception, true, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_LT_DBG(Exception, 1.0, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GT_LE_DBG(Exception, 0.0, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_LE_DBG(Exception, -0.1, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_LE_DBG(Exception, 1.1, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_LT_DBG(Exception, 1.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GT_DBG(Exception, 0.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_POSITIVE_DBG(Exception, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NONNEGATIVE_DBG(Exception, -1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NEGATIVE_DBG(Exception, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NONPOSITIVE_DBG(Exception, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_ZERO_DBG(Exception, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NOTNULL_DBG(Exception, val, ""), Exception );
EXPECT_THROW( SM_ASSERT_LE_DBG(Exception, 1.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_DBG(Exception, -1.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NE_DBG(Exception, 0.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_EQ_DBG(Exception, 1.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NEAR_DBG(Exception, 0.0, 1.0, 0.5, ""), Exception );
EXPECT_THROW( SM_ASSERT_FINITE_DBG(Exception, std::numeric_limits<float>::infinity(), ""), Exception );
EXPECT_THROW( SM_ASSERT_NOTNAN_DBG(Exception, std::numeric_limits<float>::signaling_NaN(), ""), Exception );
}
}
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,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 );
}
static inline ValueType_ maxTimesEpsilon(const ValueType_ a, const ValueType_ b, const ValueType_ epsilon)
{
SM_ASSERT_GT_DBG(std::invalid_argument, epsilon, 0.0, "This method is only valid for an epsilon greater than 0.");
return std::max(std::abs(a), std::abs(b)) * epsilon;
}