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]); }