jacobian_t estimateJacobian(input_t const & x0)
{
// evaluate the function at the operating point:
value_t fx0 = functor(x0);
size_t N = x0.size();
size_t M = fx0.size();
//std::cout << "Size: " << M << ", " << N << std::endl;
jacobian_t J;
J.resize(M, N);
SM_ASSERT_EQ(std::runtime_error,x0.size(),J.cols(),"Unexpected number of columns for input size");
SM_ASSERT_EQ(std::runtime_error,fx0.size(),J.rows(),"Unexpected number of columns for output size");
for(unsigned c = 0; c < N; c++) {
// Calculate a central difference.
// This step size was stolen from cminpack: temp = eps * fabs(x[j]);
scalar_t rcEps = std::max(static_cast<scalar_t>(fabs(x0(c))) * eps,eps);
value_t fxp = functor(functor.update(x0,c,rcEps));
value_t fxm = functor(functor.update(x0,c,-rcEps));
J.block(0, c, M, 1) = (fxp - fxm).template cast<typename jacobian_t::Scalar>()/(typename jacobian_t::Scalar)(rcEps*(scalar_t)2.0);
}
return J;
}
void SerializedMap<T,A>::setUpTable()
{
validateTableName();
int result;
// Make sure the table exists.
// This table will have a 64 bit integer key and a binary blob of data.
std::string sql = "create table if not exists " + _tableName + "(id INTEGER primary_key unique, data BLOB);";
sqlite3_stmt * stmt;
// http://www.sqlite.org/c3ref/prepare.html
result = sqlite3_prepare_v2(_db->db(), sql.c_str(), -1, &stmt, NULL);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to prepare statement: " << sqlite3_errmsg(_db->db()));
result = sqlite3_step(stmt);
SM_ASSERT_EQ(SqlException, result, SQLITE_DONE, "Unable to execute statement: " << sqlite3_errmsg(_db->db()));
// Finalize is like a delete statement. Every prepared statement must be finalized.
sqlite3_finalize(stmt);
// Pre-prepare the insert statement.
std::string insert = "INSERT or REPLACE into " + _tableName + " VALUES(?,?);";
result = sqlite3_prepare(_db->db(), insert.c_str(), -1, &_iStmt, 0);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to prepare statement: " << sqlite3_errmsg(_db->db()));
// Pre-prepare the select statement.
std::string select = "SELECT data FROM " + _tableName + " WHERE id = ?";
result = sqlite3_prepare(_db->db(), select.c_str(), -1, &_sStmt, 0);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to prepare statement: " << sqlite3_errmsg(_db->db()));
// \todo: http://web.utk.edu/~jplyon/sqlite/SQLite_optimization_FAQ.html
// \todo: Set up an LRU cache.
}
MatrixArchive::BlockType MatrixArchive::readBlock(std::istream & fin, std::string & name, Eigen::MatrixXd & matrix, std::string & stringValue) const
{
char start, end;
// start character
fin.read(&start,1);
BlockType blockType;
if(start == s_magicCharStartAStringBlock){
blockType = STRING;
}
else{
SM_ASSERT_EQ(MatrixArchiveException, start, s_magicCharStartAMatrixBlock, "The block didn't start with the expected character");
blockType = MATRIX;
}
// 64 character name
char nameBuffer[s_fixedNameSize + 1];
nameBuffer[s_fixedNameSize] = '\0';
fin.read(nameBuffer,(std::streamsize)s_fixedNameSize);
name = nameBuffer;
boost::trim(name);
switch(blockType){
case MATRIX:
readMatrix(fin, matrix);
break;
case STRING:
readString(fin, stringValue);
break;
}
// end character
fin.read(&end,1);
SM_ASSERT_EQ(MatrixArchiveException, end, s_magicCharEnd, "The matrix block didn't end with the expected character");
return blockType;
}
void ErrorTermFs<C>::setInvR(const Eigen::MatrixBase<DERIVED>& invR)
{
SM_ASSERT_EQ(Exception, invR.rows(), invR.cols(), "The covariance matrix must be square");
SM_ASSERT_EQ(Exception, invR.rows(), (int)dimension(), "The covariance matrix does not match the size of the error");
// http://eigen.tuxfamily.org/dox-devel/classEigen_1_1LDLT.html#details
// LDLT seems to work on positive semidefinite matrices.
sm::eigen::computeMatrixSqrt(invR, _sqrtInvR);
}
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 );
}
}
double MarginalizationPriorErrorTerm::evaluateErrorImplementation()
{
Eigen::VectorXd diff = getDifferenceSinceMarginalization();
SM_ASSERT_EQ(aslam::Exception, diff.rows(), _R.cols(), "Dimension of R and the minimal difference vector mismatch!");
SM_ASSERT_EQ(aslam::Exception, _d.rows(), _R.rows(), "Dimension of R and the d mismatch!");
Eigen::VectorXd currentError(_dimensionErrorTerm);
currentError.setZero();
currentError = -(_d - _R*diff);
setError(currentError);
return evaluateChiSquaredError();
}
double MatrixArchive::getScalar(std::string const & scalarName) const
{
matrix_map_t::const_iterator it = m_values.find(scalarName);
if(it == m_values.end())
{
SM_THROW(MatrixArchiveException, "There is no scalar named \"" << scalarName << "\" in the archive");
}
Eigen::MatrixXd const & M = it->second;
SM_ASSERT_EQ(MatrixArchiveException, M.rows(), 1, "The stored value is not a scalar");
SM_ASSERT_EQ(MatrixArchiveException, M.cols(), 1, "The stored value is not a scalar");
return M(0,0);
}
void Cholmod<I>::getR(cholmod_sparse* A, cholmod_sparse** R) {
cholmod_sparse* qrJ = cholmod_l_transpose(A, 1, &_cholmod);
SuiteSparseQR<double>(SPQR_ORDERING_FIXED, SPQR_NO_TOL, qrJ->ncol, 0,
qrJ, NULL, NULL, NULL, NULL, R, NULL, NULL, NULL, NULL, &_cholmod);
SM_ASSERT_EQ(Exception, _cholmod.status, CHOLMOD_OK,
"QR factorization failed");
CholmodIndexTraits<index_t>::free_sparse(&qrJ, &_cholmod);
}
void ErrorTerm::setDesignVariables(const std::vector<DesignVariable*>& designVariables)
{
/// \todo Set the back link to the error term in the design variable.
SM_ASSERT_EQ(aslam::UnsupportedOperationException, _designVariables.size(), 0, "The design variable container already has objects. The design variables may only be set once");
/// \todo: set the back-link in the design variable.
for (unsigned i = 0; i < designVariables.size(); ++i) {
SM_ASSERT_TRUE(aslam::InvalidArgumentException, designVariables[i] != NULL, "Design variable " << i << " is null");
}
_designVariables = designVariables;
}
void ErrorTerm::setDesignVariablesIterator(ITERATOR_T start, ITERATOR_T end)
{
/// \todo Set the back link to the error term in the design variable.
SM_ASSERT_EQ(aslam::UnsupportedOperationException, _designVariables.size(), 0, "The design variable container already has objects. The design variables may only be set once");
/// \todo: set the back-link in the design variable.
int ii = 0;
for (ITERATOR_T i = start; i != end; ++i, ++ii) {
SM_ASSERT_TRUE(aslam::InvalidArgumentException, *i != NULL, "Design variable " << ii << " is null");
}
_designVariables.insert(_designVariables.begin(), start, end);
}
void MatrixArchive::getVector(std::string const & vectorName, Eigen::VectorXd & outVector) const
{
matrix_map_t::const_iterator it = m_values.find(vectorName);
if(it == m_values.end())
{
SM_THROW(MatrixArchiveException, "There is no vector named \"" << vectorName << "\" in the archive");
}
Eigen::MatrixXd const & M = it->second;
SM_ASSERT_EQ(MatrixArchiveException, M.cols(), 1, "The stored value is not a vector");
outVector = M.col(0);
}
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 SerializedMap<T,A>::set(::boost::uint64_t id, const T & value)
{
int result;
try {
// Step 1 is to serialize the object. There is no way to know how big the object will be
// so unfortunately, this will incur an extra copy.
// The binary flag is important here.
std::ostringstream oss(std::ios_base::binary);
oarchive_t oa(oss);
oa << value;
//std::cout << "Saving " << oss.str().size() << " bytes\n";
// Step 2 is to bind the frameId and data to the insert statement.
// http://sqlite.org/c3ref/bind_blob.html
result = sqlite3_bind_int64(_iStmt, 1, id);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to bind id " << id << " to INSERT statement: " << sqlite3_errmsg(_db->db()));
result = static_cast<int>(sqlite3_bind_blob(_iStmt, 2, oss.str().c_str(), oss.str().size(), SQLITE_TRANSIENT));
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to bind blob of size " << oss.str().size() << " to INSERT statement: " << sqlite3_errmsg(_db->db()));
// Finally, we execute the bound insert statement.
result = sqlite3_step(_iStmt);
SM_ASSERT_EQ(SqlException, result, SQLITE_DONE, "Unable to execute INSERT statement for id " << id << " and blob of size " << oss.str().size() << ": " << sqlite3_errmsg(_db->db()));
}
catch(const SqlException & e)
{
sqlite3_reset(_iStmt);
throw;
}
// After executing, we have to reset the statement
// http://www.sqlite.org/c3ref/step.html
result = sqlite3_reset(_iStmt);
SM_ASSERT_EQ(SqlException, result, SQLITE_OK, "Unable to reset the INSERT statement: " << sqlite3_errmsg(_db->db()));
}
void MarginalizationPriorErrorTerm::evaluateJacobiansImplementation(JacobianContainer & outJ) const
{
int colIndex = 0;
std::vector<Eigen::MatrixXd>::const_iterator it_marg = _designVariableValuesAtMarginalization.begin();
for(vector<aslam::backend::DesignVariable*>::const_iterator it = _designVariables.begin(); it != _designVariables.end(); ++it, ++it_marg)
{
int dimDesignVariable = (*it)->minimalDimensions();
Eigen::MatrixXd M;
Eigen::VectorXd diff;
(*it)->minimalDifferenceAndJacobian(*it_marg, diff, M);
SM_ASSERT_EQ(aslam::Exception, M.rows(), dimDesignVariable, "Minimal difference jacobian and design variable dimension mismatch!");
outJ.add(*it, _R.block(0, colIndex, _dimensionErrorTerm, dimDesignVariable)*M);
colIndex += dimDesignVariable;
}
}
cholmod_factor* Cholmod<I>::analyze(cholmod_sparse* J)
{
//std::cout << "Cholmod:" << std::endl;
//CholmodIndexTraits<index_t>::print_sparse(J, "J", &_cholmod);
//std::cout << "/Cholmod" << std::endl;
//std::cout << "Checking common\n";
//cholmod_print_common("Common", &_cholmod);
//int rval = cholmod_check_common(&_cholmod);
//std::cout << "common result: " << rval << std::endl;
//std::cout << "Checking the sparse matrix\n";
//cholmod_print_sparse(J, "J", &_cholmod);
//rval = cholmod_check_sparse(J, &_cholmod);
//std::cout << "sparse matrix result: " << rval << std::endl;
// From the cholmod header:
//
// * If you know the method that is best for your matrix, set Common->nmethods
// * to 1 and set Common->method [0] to the set of parameters for that method.
// * If you set it to 1 and do not provide a permutation, then only AMD will
// * be called.
_cholmod.nmethods = 1;
// AMD may be used with both J or J*J'
_cholmod.method[0].ordering = CHOLMOD_AMD;
// From the cholmod header:
// CHOLMOD_SIMPLICIAL always do simplicial
// CHOLMOD_AUTO select simpl/super depending on matrix
// CHOLMOD_SUPERNODAL always do supernodal
// * If Common->supernodal <= CHOLMOD_SIMPLICIAL
// * (0) then cholmod_analyze performs a
// * simplicial analysis. If >= CHOLMOD_SUPERNODAL (2), then a supernodal
// * analysis is performed. If == CHOLMOD_AUTO (1) and
// * flop/nnz(L) < Common->supernodal_switch, then a simplicial analysis
// * is done. A supernodal analysis done otherwise.
// * Default: CHOLMOD_AUTO. Default supernodal_switch = 40
_cholmod.supernodal = CHOLMOD_AUTO;
cholmod_factor* factor = NULL;
factor = CholmodIndexTraits<index_t>::analyze(J, &_cholmod);
SM_ASSERT_EQ(Exception, _cholmod.status, CHOLMOD_OK, "The symbolic cholesky factorization failed.");
SM_ASSERT_FALSE(Exception, factor == NULL, "cholmod_analyze returned a null factor");
return factor;
}
spqr_factor* Cholmod<I>::analyzeQR(cholmod_sparse* J)
{
// From the cholmod header:
//
// * If you know the method that is best for your matrix, set Common->nmethods
// * to 1 and set Common->method [0] to the set of parameters for that method.
// * If you set it to 1 and do not provide a permutation, then only AMD will
// * be called.
// _cholmod.nmethods = 1;
// same properties apply as cholmod_factor analyze
//_cholmod.method[0].ordering = CHOLMOD_AMD;
//_cholmod.supernodal = CHOLMOD_AUTO;
_cholmod.SPQR_nthreads = -1; // let tbb choose whats best
_cholmod.SPQR_grain = 12; // +/-2* number of cores
spqr_factor* factor = NULL;
cholmod_sparse* qrJ = cholmod_l_transpose(J, 1, &_cholmod) ;
factor = SuiteSparseQR_symbolic <double>(SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, qrJ, &_cholmod) ;
CholmodIndexTraits<index_t>::free_sparse(&qrJ, &_cholmod);
SM_ASSERT_EQ(Exception, _cholmod.status, CHOLMOD_OK, "The symbolic qr factorization failed.");
SM_ASSERT_FALSE(Exception, factor == NULL, "SuiteSparseQR_symbolic returned a null factor");
return factor;
}