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 );
}
}
Eigen::Vector3d C2rph(Eigen::MatrixXd const & C)
{
SM_ASSERT_EQ_DBG(std::runtime_error,C.rows(),3,"The input matrix must be 3x3");
SM_ASSERT_EQ_DBG(std::runtime_error,C.cols(),3,"The input matrix must be 3x3");
Eigen::Vector3d rph;
rph[1] = asin(C(2,0));
rph[2] = atan2(-C(1,0),C(0,0));
rph[0] = atan2(-C(2,1),C(2,2));
return rph;
}
/// \brief Update the design variable.
void MatrixTransformation::updateImplementation(const double * dp, int size)
{
static_cast<void>(size); // used depending on NDEBUG
SM_ASSERT_EQ_DBG(aslam::Exception, size, _UpdateDimension , "Incorrect update size");
_A_a = _A;
Eigen::Matrix3d dA;
int j=0;
for (int i=0; i<9; i++){
_A(i%3,floor(static_cast<double>(i/3))) += _UpdatePattern(i%3,floor(static_cast<double>(i/3)))*dp[j];
if (_UpdatePattern(i%3,floor(static_cast<double>(i/3)))==1){j++; }
}
}
Eigen::VectorXd operator()(const Eigen::VectorXd& dr) {
int offset = 0;
for (size_t i = 0; i < _et.numDesignVariables(); i++) {
DesignVariable* d = _et.designVariable(i);
SM_ASSERT_LE_DBG(aslam::Exception, offset + d->minimalDimensions(), dr.size(), "The offset is out of bounds.");
d->update(&dr[offset], d->minimalDimensions());
offset += d->minimalDimensions();
}
SM_ASSERT_EQ_DBG(aslam::Exception, offset, dr.size(), "The input vector is too large. It wasn't covered by the design variables.");
_et.evaluateError();
value_t e = _et.error();
for (size_t i = 0; i < _et.numDesignVariables(); i++) {
DesignVariable* d = _et.designVariable(i);
d->revertUpdate();
}
return e;
}
Eigen::Matrix3d rph2C(Eigen::MatrixXd const & A, unsigned column)
{
SM_ASSERT_EQ_DBG(std::runtime_error,A.rows(),3,"The input matrix must have 3 rows");
SM_ASSERT_LT_DBG(std::runtime_error,column,A.cols(),"The requested column is out of bounds");
return rph2C(A(0,column),A(1,column),A(2,column));
}
Eigen::Matrix3d rph2C(Eigen::VectorXd const & x)
{
SM_ASSERT_EQ_DBG(std::runtime_error,x.size(),3,"The input vector must have 3 components");
return rph2C(x[0],x[1],x[2]);
}
