void SerializedMap<T,A>::validateTableName()
{
SM_ASSERT_GE(InvalidTableNameException,_tableName.size(),1,"Table name \"" << _tableName << "\" is invalid.");
SM_ASSERT_TRUE(InvalidTableNameException,isalpha(_tableName[0]), "Table name \"" << _tableName << "\" is invalid.");
for(size_t i = 1; i < _tableName.size(); i++)
{
SM_ASSERT_TRUE(InvalidTableNameException,isalnum(_tableName[i]), "Table name \"" << _tableName << "\" is invalid. Character " << i << " is not alphanumeric.");
}
}
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 );
}
}
bool Cholmod<I>::factorize(cholmod_sparse* A, spqr_factor* L, double tol,
bool transpose) {
cholmod_sparse* At = A;
if (transpose)
At = cholmod_l_transpose(A, 1, &_cholmod) ;
SM_ASSERT_TRUE(Exception, At != NULL, "Null input");
SM_ASSERT_TRUE(Exception, L != NULL, "Null input");
_cholmod.quick_return_if_not_posdef = 1;
int status = SuiteSparseQR_numeric(tol, At, L, &_cholmod);
// TODO: check if those ones are the same for cholmod and spqr
switch (_cholmod.status) {
case CHOLMOD_NOT_INSTALLED:
std::cerr << "Cholmod failure: method not installed.";
break;
case CHOLMOD_OUT_OF_MEMORY:
std::cerr << "Cholmod failure: out of memory.";
break;
case CHOLMOD_TOO_LARGE:
std::cerr << "Cholmod failure: integer overflow occured.";
break;
case CHOLMOD_INVALID:
std::cerr << "Cholmod failure: invalid input.";
break;
case CHOLMOD_NOT_POSDEF:
// TODO(sameeragarwal): These two warnings require more
// sophisticated handling going forward. For now we will be
// strict and treat them as failures.
std::cerr << "Cholmod warning: matrix not positive definite.";
break;
case CHOLMOD_DSMALL:
std::cerr << "Cholmod warning: D for LDL' or diag(L) or "
<< "LL' has tiny absolute value.";
break;
case CHOLMOD_OK:
if (status != 0) {
break;
}
std::cerr << "Cholmod failure: cholmod_factorize returned zero "
<< "but cholmod_common::status is CHOLMOD_OK.";
break;
default:
std::cerr << "Unknown cholmod return code. ";
break;
}
if (transpose)
CholmodIndexTraits<index_t>::free_sparse(&At, &_cholmod);
if (_cholmod.status == CHOLMOD_OK && status == 1)
return true;
else
return false;
}
void MatrixArchive::validateName(std::string const & name, sm::source_file_pos const & sfp) const
{
if(name.size() > s_fixedNameSize || name.size() == 0)
{
SM_THROW(MatrixArchiveException, "The name \"" << name << "\" is an incorrect size. Names length must be between 1 and " << s_fixedNameSize);
}
SM_ASSERT_TRUE(MatrixArchiveException, isalpha(name[0]), "The name \"" << name << "\" is invalid. The first character of the name must be a letter");
for(unsigned i = 1; i < name.size(); i++)
{
SM_ASSERT_TRUE(MatrixArchiveException, isalnum(name[i]) || name[i] == '_', "The name \"" << name << "\" is invalid. The characters of the name must be alphanumeric or an underscore. (failed at character " << i << ")");
}
}
void MatrixArchive::load(boost::filesystem::path const & amaFilePath, std::set<std::string> const & validNames)
{
std::ifstream fin(amaFilePath.string().c_str(), std::ios::binary);
SM_ASSERT_TRUE(MatrixArchiveException, fin.good(), "Unable to open file " << amaFilePath << " for reading");
std::string name, valueString;
Eigen::MatrixXd matrix;
fin.peek();
while(!fin.eof())
{
BlockType blockType = readBlock(fin, name, matrix, valueString);
if(validNames.empty() || validNames.count(name) > 0)
{
validateName(name,SM_SOURCE_FILE_POS);
switch(blockType){
case MATRIX:
m_values[name] = matrix;
break;
case STRING:
m_strings[name] = valueString;
break;
}
}
fin.peek();
}
}
bool GridDetector::initCameraGeometryFromObservations(boost::shared_ptr<std::vector<cv::Mat> > images_ptr) {
std::vector<cv::Mat>& images = *images_ptr;
SM_DEFINE_EXCEPTION(Exception, std::runtime_error);
SM_ASSERT_TRUE(Exception, images.size() != 0, "Need min. one image");
std::vector<GridCalibrationTargetObservation> observations;
for(unsigned int i=0; i<images.size(); i++)
{
GridCalibrationTargetObservation obs(_target);
//detect calibration target
bool success = findTargetNoTransformation(images[i], obs);
//delete image copy (save memory)
obs.clearImage();
//append
if(success)
observations.push_back(obs);
}
//initialize the intrinsics
if(observations.size() > 0)
return _geometry->initializeIntrinsics(observations);
return false;
}
void SerializedMap<T,A>::setUpTable()
{
SM_ASSERT_TRUE(UnableToOpenDatabaseException, _db.get() != NULL, "The database is null");
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.
}
SerializedMap<T,A>::SerializedMap(boost::shared_ptr<Database> db, const std::string & tableName) : _tableName(tableName), _db(db), _iStmt(NULL), _sStmt(NULL)
{
SM_ASSERT_TRUE(UnableToOpenDatabaseException, _db.get() != NULL, "The database is null");
setUpTable();
}
void MatrixArchive::save(boost::filesystem::path const & amaFilePath, std::set<std::string> const & validNames) const
{
std::ofstream fout(amaFilePath.string().c_str(), std::ios::binary);
SM_ASSERT_TRUE(MatrixArchiveException, fout.good(), "Unable to open file " << amaFilePath.string() << " for writing");
save(fout, validNames);
fout.close();
}
void PinholeUndistorter<DISTORTION_T, MASK_T>::constructUndistortedFrame(
const cv::Mat & image, frame_t & outFrame) const {
SM_ASSERT_TRUE(std::runtime_error, _idealGeometry,
"Camera has not yet been set.")
cv::Mat undistImage;
undistortImage(image, undistImage);
outFrame.setImage(undistImage);
outFrame.setGeometry(_idealGeometry);
}
void Timer::start(){
SM_ASSERT_TRUE(TimerException,!m_timing,"The timer " + Timing::getTag(m_handle) + " is already running");
m_timing = true;
#ifdef SM_USE_HIGH_PERF_TIMER
QueryPerformanceCounter(&m_time);
#else
m_time = boost::posix_time::microsec_clock::local_time();
#endif
}
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;
}
bool Cholmod<I>::factorize(cholmod_sparse* A, cholmod_factor* L)
{
SM_ASSERT_TRUE(Exception, A != NULL, "Null input");
SM_ASSERT_TRUE(Exception, L != NULL, "Null input");
_cholmod.quick_return_if_not_posdef = 1;
int status = CholmodIndexTraits<index_t>::factorize(A, L, &_cholmod);
switch (_cholmod.status) {
case CHOLMOD_NOT_INSTALLED:
std::cerr << "Cholmod failure: method not installed.";
return false;
case CHOLMOD_OUT_OF_MEMORY:
std::cerr << "Cholmod failure: out of memory.";
return false;
case CHOLMOD_TOO_LARGE:
std::cerr << "Cholmod failure: integer overflow occured.";
return false;
case CHOLMOD_INVALID:
std::cerr << "Cholmod failure: invalid input.";
return false;
case CHOLMOD_NOT_POSDEF:
// TODO(sameeragarwal): These two warnings require more
// sophisticated handling going forward. For now we will be
// strict and treat them as failures.
std::cerr << "Cholmod warning: matrix not positive definite.";
return false;
case CHOLMOD_DSMALL:
std::cerr << "Cholmod warning: D for LDL' or diag(L) or "
<< "LL' has tiny absolute value.";
return false;
case CHOLMOD_OK:
if (status != 0) {
return true;
}
std::cerr << "Cholmod failure: cholmod_factorize returned zero "
<< "but cholmod_common::status is CHOLMOD_OK.";
return false;
default:
std::cerr << "Unknown cholmod return code. ";
return false;
}
return false;
}
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::saveStrings(std::ostream & fout, std::set<std::string> const & validNames) const
{
string_map_t::const_iterator it = m_strings.begin();
for( ; it != m_strings.end(); it++)
{
if(validNames.empty() || validNames.count(it->first) > 0)
{
writeStringBlock(fout, it->first, it->second);
SM_ASSERT_TRUE(MatrixArchiveException, fout.good(), "Error while writing string " << it->first << " to file.");
}
}
}
CovarianceReprojectionError<F>::CovarianceReprojectionError(const frame_t * frame, int keypointIndex,
HomogeneousExpression point, CameraDesignVariable<camera_geometry_t> camera, spline_t* spline, Scalar* lineDelayDv) :
_frame(frame), _keypointIndex(keypointIndex), _point(point), _camera(camera), _spline(spline), _lineDelayDv(lineDelayDv)
{
SM_ASSERT_TRUE(Exception, frame != NULL, "The frame must not be null");
// if a spline is given, estimate the covariance in each iteration
//if(!spline)
// parent_t::_invR = _frame->keypoint(_keypointIndex).invR();
JacobianContainer::set_t dvs;
point.getDesignVariables(dvs); // point dv's
camera.getDesignVariables(dvs); // camera dv's
parent_t::setDesignVariablesIterator(dvs.begin(), dvs.end());
}
void Timer::stop()
{
SM_ASSERT_TRUE(TimerException, m_timing,"The timer " + Timing::getTag(m_handle) + " is not running");
double dt;
#ifdef SM_USE_HIGH_PERF_TIMER
LARGE_INTEGER end;
QueryPerformanceCounter(&end);
dt = (double)(end.QuadPart - m_time.QuadPart)*Timing::instance().m_clockPeriod;
#else
boost::posix_time::ptime now = boost::posix_time::microsec_clock::local_time();
boost::posix_time::time_duration t = now - m_time;
dt = ((double)t.total_nanoseconds() * 1e-9);
#endif
Timing::instance().addTime(m_handle,dt);
m_timing = false;
}
std::string Timing::getTag(size_t handle){
std::string tag;
bool found = false;
// Perform a linear search for the tag
map_t::iterator i = instance().m_tagMap.begin();
for( ; i != instance().m_tagMap.end(); i++) {
if(i->second == handle){
found = true;
tag = i->first;
break;
}
}
SM_ASSERT_TRUE(TimerException,found,"Unable to find the tag associated with handle " << handle);
return tag;
}
void MatrixArchive::saveMatrices(std::ostream & fout, std::set<std::string> const & validNames) const
{
matrix_map_t::const_iterator it = m_values.begin();
for( ; it != m_values.end(); it++)
{
if(validNames.empty() || validNames.count(it->first) > 0)
{
if(isSystemLittleEndian())
{
writeMatrixBlock(fout, it->first, it->second);
}
else
{
writeMatrixBlockSwapBytes(fout, it->first, it->second);
}
SM_ASSERT_TRUE(MatrixArchiveException, fout.good(), "Error while writing matrix " << it->first << " to file.");
}
}
}
cholmod_dense* Cholmod<I>::solve(cholmod_sparse* A, spqr_factor* L,
cholmod_dense* b, double tol, bool norm, double normTol) {
cholmod_sparse* qrJ = cholmod_l_transpose(A, 1, &_cholmod);
cholmod_dense* scaling = NULL;
if (norm) {
scaling =
CholmodIndexTraits<index_t>::allocate_dense(qrJ->ncol, 1, qrJ->ncol,
CHOLMOD_REAL, &_cholmod);
double* values =
reinterpret_cast<double*>(scaling->x);
for (size_t i = 0; i < qrJ->ncol; ++i) {
const double normCol = colNorm(qrJ, i);
if (normCol < normTol)
values[i] = 0.0;
else
values[i] = 1.0 / normCol;
}
SM_ASSERT_TRUE(Exception, scale(scaling, CHOLMOD_COL, qrJ),
"Scaling failed");
}
cholmod_dense* res = NULL;
if (factorize(qrJ, L, tol)) {
cholmod_dense* qrY = SuiteSparseQR_qmult(SPQR_QTX, L, b, &_cholmod);
res = SuiteSparseQR_solve(SPQR_RETX_EQUALS_B, L, qrY, &_cholmod);
CholmodIndexTraits<index_t>::free_dense(&qrY, &_cholmod);
}
if (norm) {
const double* svalues =
reinterpret_cast<const double*>(scaling->x);
double* rvalues =
reinterpret_cast<double*>(res->x);
for (size_t i = 0; i < qrJ->ncol; ++i)
rvalues[i] = svalues[i] * rvalues[i];
CholmodIndexTraits<index_t>::free_dense(&scaling, &_cholmod);
}
CholmodIndexTraits<index_t>::free_sparse(&qrJ, &_cholmod);
return res;
}
FovDistortion::FovDistortion(double w)
: _w(w){
SM_ASSERT_TRUE(std::runtime_error, areParametersValid(w), "Invalid distortion parameter");
}
// aslam::backend compatibility
void FovDistortion::update(const double * v) {
_w += v[0];
SM_ASSERT_TRUE(std::runtime_error, areParametersValid(_w), "Invalid distortion parameter");
}
void Scalar::minimalDifferenceImplementation(const Eigen::MatrixXd& xHat, Eigen::VectorXd& outDifference) const
{
SM_ASSERT_TRUE(aslam::InvalidArgumentException, (xHat.rows() == 1)&(xHat.cols() == 1), "xHat has incompatible dimensions");
outDifference = Eigen::VectorXd(1);
outDifference(0) = _p - xHat(0,0);
}
void FovDistortion::setParameters(const Eigen::MatrixXd & S) {
_w = S(0, 0);
SM_ASSERT_TRUE(std::runtime_error, areParametersValid(_w), "Invalid distortion parameter");
}
void marginalize(
std::vector<aslam::backend::DesignVariable*>& inDesignVariables,
std::vector<aslam::backend::ErrorTerm*>& inErrorTerms,
int numberOfInputDesignVariablesToRemove,
bool useMEstimator,
boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm>& outPriorErrorTermPtr,
Eigen::MatrixXd& outCov,
std::vector<aslam::backend::DesignVariable*>& outDesignVariablesInRTop,
size_t numTopRowsInCov,
size_t /* numThreads */)
{
SM_WARN_STREAM_COND(inDesignVariables.size() == 0, "Zero input design variables in the marginalizer!");
// check for duplicates!
std::unordered_set<aslam::backend::DesignVariable*> inDvSetHT;
for(auto it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
{
auto ret = inDvSetHT.insert(*it);
SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate design variables in input list!");
}
std::unordered_set<aslam::backend::ErrorTerm*> inEtSetHT;
for(auto it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
auto ret = inEtSetHT.insert(*it);
SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate error term in input list!");
}
SM_DEBUG_STREAM("NO duplicates in input design variables or input error terms found.");
// Partition the design varibles into removed/remaining.
int dimOfDesignVariablesToRemove = 0;
std::vector<aslam::backend::DesignVariable*> remainingDesignVariables;
int k = 0;
size_t dimOfDvsInTopBlock = 0;
for(std::vector<aslam::backend::DesignVariable*>::const_iterator it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
{
if (k < numberOfInputDesignVariablesToRemove)
{
dimOfDesignVariablesToRemove += (*it)->minimalDimensions();
} else
{
remainingDesignVariables.push_back(*it);
}
if(dimOfDvsInTopBlock < numTopRowsInCov)
{
outDesignVariablesInRTop.push_back(*it);
}
dimOfDvsInTopBlock += (*it)->minimalDimensions();
k++;
}
// store original block indices to prevent side effects
std::vector<int> originalBlockIndices;
std::vector<int> originalColumnBase;
// assign block indices
int columnBase = 0;
for (size_t i = 0; i < inDesignVariables.size(); ++i) {
originalBlockIndices.push_back(inDesignVariables[i]->blockIndex());
originalColumnBase.push_back(inDesignVariables[i]->columnBase());
inDesignVariables[i]->setBlockIndex(i);
inDesignVariables[i]->setColumnBase(columnBase);
columnBase += inDesignVariables[i]->minimalDimensions();
}
int dim = 0;
std::vector<size_t> originalRowBase;
for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
originalRowBase.push_back((*it)->rowBase());
(*it)->setRowBase(dim);
dim += (*it)->dimension();
}
aslam::backend::DenseQrLinearSystemSolver qrSolver;
qrSolver.initMatrixStructure(inDesignVariables, inErrorTerms, false);
SM_INFO_STREAM("Marginalization optimization problem initialized with " << inDesignVariables.size() << " design variables and " << inErrorTerms.size() << " error terrms");
SM_INFO_STREAM("The Jacobian matrix is " << dim << " x " << columnBase);
qrSolver.evaluateError(1, useMEstimator);
qrSolver.buildSystem(1, useMEstimator);
const Eigen::MatrixXd& jacobian = qrSolver.getJacobian();
const Eigen::VectorXd& b = qrSolver.e();
// check dimension of jacobian
int jrows = jacobian.rows();
int jcols = jacobian.cols();
int dimOfRemainingDesignVariables = jcols - dimOfDesignVariablesToRemove;
//int dimOfPriorErrorTerm = jrows;
// check the rank
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(jacobian);
//lu_decomp.setThreshold(1e-20);
double threshold = lu_decomp.threshold();
int rank = lu_decomp.rank();
int fullRank = std::min(jacobian.rows(), jacobian.cols());
SM_DEBUG_STREAM("Rank of jacobian: " << rank << " (full rank: " << fullRank << ", threshold: " << threshold << ")");
bool rankDeficient = rank < fullRank;
if(rankDeficient)
{
SM_WARN("Marginalization jacobian is rank deficient!");
}
//SM_ASSERT_FALSE(aslam::Exception, rankDeficient, "Right now, we don't want the jacobian to be rank deficient - ever...");
Eigen::MatrixXd R_reduced;
Eigen::VectorXd d_reduced;
if (jrows < jcols)
{
SM_THROW(aslam::Exception, "underdetermined LSE!");
// underdetermined LSE, don't do QR
R_reduced = jacobian.block(0, dimOfDesignVariablesToRemove, jrows, jcols - dimOfDesignVariablesToRemove);
d_reduced = b;
} else {
// PTF: Do we know what will happen when the jacobian matrix is rank deficient?
// MB: yes, bad things!
// do QR decomposition
sm::timing::Timer myTimer("QR Decomposition");
Eigen::HouseholderQR<Eigen::MatrixXd> qr(jacobian);
Eigen::MatrixXd Q = qr.householderQ();
Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
Eigen::VectorXd d = Q.transpose()*b;
myTimer.stop();
if(numTopRowsInCov > 0)
{
sm::timing::Timer myTimer("Covariance computation");
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(R);
Eigen::MatrixXd Rinv = lu_decomp.inverse();
Eigen::MatrixXd covariance = Rinv * Rinv.transpose();
outCov = covariance.block(0, 0, numTopRowsInCov, numTopRowsInCov);
myTimer.stop();
}
// size_t numRowsToKeep = rank - dimOfDesignVariablesToRemove;
// SM_ASSERT_TRUE_DBG(aslam::Exception, rankDeficient || (numRowsToKeep == dimOfRemainingDesignVariables), "must be the same if full rank!");
// get the top left block
SM_ASSERT_GE(aslam::Exception, R.rows(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " rows of R because it only has " << R.rows() << " rows.");
SM_ASSERT_GE(aslam::Exception, R.cols(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " cols of R because it only has " << R.cols() << " cols.");
//outRtop = R.block(0, 0, numTopRowsInRtop, numTopRowsInRtop);
// cut off the zero rows at the bottom
R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables, dimOfRemainingDesignVariables);
//R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, numRowsToKeep, dimOfRemainingDesignVariables);
d_reduced = d.segment(dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables);
//d_reduced = d.segment(dimOfDesignVariablesToRemove, numRowsToKeep);
//dimOfPriorErrorTerm = dimOfRemainingDesignVariables;
}
// now create the new error term
boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> err(new aslam::backend::MarginalizationPriorErrorTerm(remainingDesignVariables, d_reduced, R_reduced));
outPriorErrorTermPtr.swap(err);
// restore initial block indices to prevent side effects
for (size_t i = 0; i < inDesignVariables.size(); ++i) {
inDesignVariables[i]->setBlockIndex(originalBlockIndices[i]);
inDesignVariables[i]->setColumnBase(originalColumnBase[i]);
}
int index = 0;
for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
(*it)->setRowBase(originalRowBase[index++]);
}
}
FovDistortion::FovDistortion()
: _w(1.0) {
SM_ASSERT_TRUE(std::runtime_error, areParametersValid(_w), "Invalid distortion parameter");
}
void SerializedMap<T,A>::set(::boost::uint64_t id, const ::boost::shared_ptr<T> & value)
{
SM_ASSERT_TRUE(NullValueException, value, "It is illegal to call this function with a null value");
set(id,*value);
}
void MatrixTransformation::minimalDifferenceImplementation(const Eigen::MatrixXd& xHat, Eigen::VectorXd& outDifference) const
{
SM_ASSERT_TRUE(aslam::InvalidArgumentException, (xHat.rows() == 3)&&(xHat.cols() == 3), "xHat has incompatible dimensions");
outDifference = sm::kinematics::R2AxisAngle(_A*xHat.transpose());
}
FovDistortion::FovDistortion(const sm::PropertyTree & config) {
_w = config.getDouble("w");
SM_ASSERT_TRUE(std::runtime_error, areParametersValid(_w), "Invalid distortion parameter");
}
