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 );
}
}
// Axis Angle rotation.
Eigen::Matrix3d axisAngle2R(double a, double ax, double ay, double az){
SM_ASSERT_LT_DBG(std::runtime_error,fabs(sqrt(ax*ax + ay*ay + az*az) - 1.0), 1e-4, "The axis is not a unit vector. ||a|| = " << (sqrt(ax*ax + ay*ay + az*az)));
if(a < 1e-12)
return Eigen::Matrix3d::Identity();
// e = [ax ay az]
// e*(e') + (eye(3) - e*(e'))*cos(a) - crossMx(e) * sin(a) =
//[ ax^2+ca*(1-ax^2), ax*ay-ca*ax*ay+sa*az, ax*az-ca*ax*az-sa*ay]
//[ ax*ay-ca*ax*ay-sa*az, ay^2+ca*(1-ay^2), ay*az-ca*ay*az+sa*ax]
//[ ax*az-ca*ax*az+sa*ay, ay*az-ca*ay*az-sa*ax, az^2+ca*(1-az^2)]
double sa = sin(a);
double ca = cos(a);
double ax2 = ax*ax;
double ay2 = ay*ay;
double az2 = az*az;
double const one = double(1);
Eigen::Matrix3d C;
C(0,0) = ax2+ca*(one-ax2); C(0,1) = ax*ay-ca*ax*ay+sa*az; C(0,2) = ax*az-ca*ax*az-sa*ay;
C(1,0) = ax*ay-ca*ax*ay-sa*az; C(1,1) = ay2+ca*(one-ay2); C(1,2) = ay*az-ca*ay*az+sa*ax;
C(2,0) = ax*az-ca*ax*az+sa*ay; C(2,1) = ay*az-ca*ay*az-sa*ax; C(2,2) = az2+ca*(one-az2);
return C;
}
/// \brief Get design variable i.
const DesignVariable* ErrorTerm::designVariable(size_t i) const
{
SM_ASSERT_LT_DBG(aslam::IndexOutOfBoundsException, i, _designVariables.size(), "index out of bounds");
return _designVariables[i];
}
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));
}
