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