TEST(SmCommonTestSuite,testAssertMacros)
{
SM_DEFINE_EXCEPTION(Exception, std::runtime_error);
{
double* val = new double;
EXPECT_NO_THROW( SM_ASSERT_TRUE(Exception, true, "") );
EXPECT_NO_THROW( SM_ASSERT_FALSE(Exception, false, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_LT(Exception, 0.0, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GT_LE(Exception, 0.1, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_LE(Exception, 0.0, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GE_LE(Exception, 1.0, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_LT(Exception, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GT(Exception, 1.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_POSITIVE(Exception, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NONNEGATIVE(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NEGATIVE(Exception, -1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NONPOSITIVE(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_ZERO(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NOTNULL(Exception, val, "") );
EXPECT_NO_THROW( SM_ASSERT_LE(Exception, 0.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_GE(Exception, 0.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NE(Exception, 0.0, 1.0, "") );
EXPECT_NO_THROW( SM_ASSERT_EQ(Exception, 0.0, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NEAR(Exception, 0.0, 1.0, 2.0, "") );
EXPECT_NO_THROW( SM_ASSERT_FINITE(Exception, 0.0, "") );
EXPECT_NO_THROW( SM_ASSERT_NOTNAN(Exception, 0.0, "") );
delete val;
}
{
double* val = NULL;
EXPECT_THROW( SM_ASSERT_TRUE(Exception, false, ""), Exception);
EXPECT_THROW( SM_ASSERT_FALSE(Exception, true, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_LT(Exception, 1.0, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GT_LE(Exception, 0.0, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_LE(Exception, -0.1, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE_LE(Exception, 1.1, 0.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_LT(Exception, 1.0, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GT(Exception, 0.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_POSITIVE(Exception, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NONNEGATIVE(Exception, -1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NEGATIVE(Exception, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NONPOSITIVE(Exception, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_ZERO(Exception, 1.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NOTNULL(Exception, val, ""), Exception );
EXPECT_THROW( SM_ASSERT_LE(Exception, 1.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_GE(Exception, -1.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NE(Exception, 0.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_EQ(Exception, 1.0, 0.0, ""), Exception );
EXPECT_THROW( SM_ASSERT_NEAR(Exception, 0.0, 1.0, 0.5, ""), Exception );
EXPECT_THROW( SM_ASSERT_FINITE(Exception, std::numeric_limits<float>::infinity(), ""), Exception );
EXPECT_THROW( SM_ASSERT_NOTNAN(Exception, std::numeric_limits<float>::signaling_NaN(), ""), Exception );
}
}
MarginalizationPriorErrorTerm::MarginalizationPriorErrorTerm(const std::vector<aslam::backend::DesignVariable*>& designVariables,
const Eigen::VectorXd& d, const Eigen::MatrixXd& R)
: aslam::backend::ErrorTermDs(R.rows()), _designVariables(designVariables), _d(d), _R(R), _dimensionDesignVariables(R.cols())
{
SM_ASSERT_GT(aslam::InvalidArgumentException, designVariables.size(), 0, "The prior error term doesn't make much sense with zero design variables.");
// PTF: Hrm...here is a big weakness of the current optimizer code. We should have
// different base classes for different uncertainty types (scalar, diagonal, matrix, none)
// to avoid big matrix multiplications during optimization.
setInvR(Eigen::MatrixXd::Identity(R.rows(), R.rows()));
//_M = Eigen::MatrixXd::Identity(R.cols(), R.cols());
// set all design variables
for(vector<aslam::backend::DesignVariable*>::iterator it = _designVariables.begin(); it != _designVariables.end(); ++it)
{
Eigen::MatrixXd values;
(*it)->getParameters(values);
_designVariableValuesAtMarginalization.push_back(values);
}
setDesignVariables(designVariables);
}
void SerializedMap<T,A>::get( ::boost::uint64_t id, T & outValue)
{
int result;
try {
// Bind the id to the select statement.
result = sqlite3_bind_int64(_sStmt, 1, id);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to bind id " << id << " to SELECT statement: " << sqlite3_errmsg(_db->db()));
// Execute the bound select statement to retrieve the row.
result = sqlite3_step(_sStmt);
// If the select was successful, this will return SQLITE_ROW. Otherwise, the frame is not in the database.
SM_ASSERT_EQ(SqlException, result, SQLITE_ROW, "Unable to execute SELECT statement: " << sqlite3_errmsg(_db->db()));
SM_ASSERT_GT(SqlException, sqlite3_column_bytes(_sStmt, 0), 0, "The SELECT statement returned zero bytes.");
//std::cout << "Loading " << sqlite3_column_bytes(_sStmt, 0) << " bytes\n";
// Deserialize the blob
// This allows us to create a stream from the blob without incurring a copy
typedef ::boost::iostreams::basic_array_source<char> Device;
::boost::iostreams::stream<Device> buffer(reinterpret_cast<const char *>(sqlite3_column_blob(_sStmt, 0)),sqlite3_column_bytes(_sStmt, 0));
iarchive_t ia(buffer);
ia >> outValue;
}
catch(const SqlException & e)
{
sqlite3_reset(_sStmt);
throw;
}
// After executing, we have to reset the statement
// http://www.sqlite.org/c3ref/step.html
result = sqlite3_reset(_sStmt);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to reset the SELECT statement: " << sqlite3_errmsg(_db->db()));
}
void showProgress(T1 done, T2 all)
{
SM_ASSERT_GT(std::runtime_error, all, 0, "#DIV0");
showProgress(static_cast<double>(done) / static_cast<double>(all));
}
static bool definitelyLessThan(const ValueType_ a, const ValueType_ b, ValueType_ epsilon = std::numeric_limits<ValueType_>::epsilon())
{
SM_ASSERT_GT(std::invalid_argument, epsilon, 0.0, "This method is only valid for an epsilon greater than 0.");
return (b - a) > internal::maxTimesEpsilon(a, b, epsilon);
}
static bool approximatelyEqual(const ValueType_ a, const ValueType_ b, ValueType_ epsilon = std::numeric_limits<ValueType_>::epsilon())
{
SM_ASSERT_GT(std::invalid_argument, epsilon, 0.0, "This method is only valid for an epsilon greater than 0.");
return std::abs(a - b) <= internal::maxTimesEpsilon(a, b, epsilon);
}
