diff --git a/gtsam/linear/MultifrontalClique.cpp b/gtsam/linear/MultifrontalClique.cpp index 01f4a2aed7..29397a52b1 100644 --- a/gtsam/linear/MultifrontalClique.cpp +++ b/gtsam/linear/MultifrontalClique.cpp @@ -30,17 +30,7 @@ namespace gtsam { namespace { -KeyVector orderedKeysFromBlockIndex(const std::map& blockIndex) { - const size_t totalKeys = blockIndex.size(); - KeyVector orderedKeys(totalKeys); - for (const auto& entry : blockIndex) { - if (entry.second < totalKeys) { - orderedKeys[entry.second] = entry.first; - } - } - return orderedKeys; -} - +// Print keys in [start, end) using the provided formatter. void printKeyRange(std::ostream& os, const KeyVector& keys, size_t start, size_t end, const KeyFormatter& formatter) { os << "["; @@ -63,16 +53,21 @@ Vector& buildSeparatorVector(const std::vector& separatorPtrs, } #ifndef NDEBUG +bool containsKey(const KeyVector& orderedKeys, Key key) { + return std::find(orderedKeys.begin(), orderedKeys.end(), key) != + orderedKeys.end(); +} + bool validateFactorKeys(const GaussianFactorGraph& graph, const std::vector& factorIndices, - const std::map& blockIndex, + const KeyVector& orderedKeys, const std::unordered_set* fixedKeys) { for (size_t index : factorIndices) { assert(index < graph.size()); const GaussianFactor::shared_ptr& gf = graph[index]; if (!gf) continue; for (Key key : gf->keys()) { - if (blockIndex.find(key) != blockIndex.end()) continue; + if (containsKey(orderedKeys, key)) continue; if (fixedKeys && fixedKeys->count(key)) continue; return false; } @@ -86,7 +81,7 @@ bool validateFactorKeys(const GaussianFactorGraph& graph, MultifrontalClique::MultifrontalClique( std::vector factorIndices, const std::weak_ptr& parent, const KeyVector& frontals, - const KeyVector& separatorKeys, const std::map& dims, + const KeySet& separatorKeys, const KeyDimMap& dims, const GaussianFactorGraph& graph, VectorValues* solution, const std::unordered_set* fixedKeys) { factorIndices_ = std::move(factorIndices); @@ -98,31 +93,16 @@ MultifrontalClique::MultifrontalClique( "MultifrontalSolver: cluster has no frontal keys."); } - // Cache the mapping from key to Ab block index for fast fills. - blockIndex_.clear(); - size_t blockIdx = 0; - for (Key key : frontals) { - blockIndex_[key] = blockIdx; - ++blockIdx; - } - for (Key key : separatorKeys) { - blockIndex_[key] = blockIdx; - ++blockIdx; - } - - size_t dim = 0; - for (Key key : frontals) { - auto it = dims.find(key); - if (it != dims.end()) dim += it->second; - } - frontalDim = dim; + // Cache keys in block order for fast linear lookup in small cliques. + orderedKeys_.clear(); + orderedKeys_.reserve(frontals.size() + separatorKeys.size()); + orderedKeys_.insert(orderedKeys_.end(), frontals.begin(), frontals.end()); + orderedKeys_.insert(orderedKeys_.end(), separatorKeys.begin(), + separatorKeys.end()); - dim = 0; - for (Key key : separatorKeys) { - auto it = dims.find(key); - if (it != dims.end()) dim += it->second; - } - separatorDim = dim; + // Cache total frontal/separator dimensions for scheduling and sizing. + frontalDim = internal::sumDims(dims, frontals); + separatorDim = internal::sumDims(dims, separatorKeys); rhsScratch_.resize(frontalDim); separatorScratch_.resize(separatorDim); @@ -144,27 +124,29 @@ void MultifrontalClique::finalize(std::vector children) { this->children.push_back(child.clique); } - // Compute parent indices for all children. + // Compute parent indices for all children (separator blocks + RHS block). for (const auto& child : children) { if (!child.clique) continue; std::vector indices; indices.reserve(child.separatorKeys.size() + 1); for (Key key : child.separatorKeys) { - auto it = blockIndex_.find(key); - if (it == blockIndex_.end()) { - throw std::runtime_error( - "MultifrontalSolver: separator key not found in parent clique"); - } - indices.push_back(static_cast(it->second)); + indices.push_back(blockIndex(key)); } - indices.push_back(static_cast(blockIndex_.size())); + // The RHS block is always the last block in Ab/SBM. + indices.push_back(static_cast(orderedKeys_.size())); child.clique->setParentIndices(indices); } } +DenseIndex MultifrontalClique::blockIndex(Key key) const { + const auto it = std::find(orderedKeys_.begin(), orderedKeys_.end(), key); + assert(it != orderedKeys_.end()); + return static_cast(std::distance(orderedKeys_.begin(), it)); +} + void MultifrontalClique::cacheSolutionPointers(VectorValues* solution, const KeyVector& frontals, - const KeyVector& separatorKeys) { + const KeySet& separatorKeys) { frontalPtrs_.clear(); separatorPtrs_.clear(); frontalPtrs_.reserve(frontals.size()); @@ -178,9 +160,10 @@ void MultifrontalClique::cacheSolutionPointers(VectorValues* solution, } std::vector MultifrontalClique::blockDims( - const std::map& dims, const KeyVector& frontals, - const KeyVector& separatorKeys) const { + const KeyDimMap& dims, const KeyVector& frontals, + const KeySet& separatorKeys) const { std::vector blockDims; + blockDims.reserve(frontals.size() + separatorKeys.size()); for (Key k : frontals) blockDims.push_back(dims.at(k)); for (Key k : separatorKeys) blockDims.push_back(dims.at(k)); return blockDims; @@ -202,6 +185,7 @@ void MultifrontalClique::initializeMatrices( const std::vector& blockDims, size_t verticalBlockMatrixRows) { sbm_ = SymmetricBlockMatrix(blockDims, true); Ab_ = VerticalBlockMatrix(blockDims, verticalBlockMatrixRows, true); + // Ab's structure is fixed; clear it once and reuse across loads. Ab_.matrix().setZero(); } @@ -214,13 +198,15 @@ size_t MultifrontalClique::addJacobianFactor( for (auto it = jacobianFactor.begin(); it != jacobianFactor.end(); ++it) { Key k = *it; if (fixedKeys_ && fixedKeys_->count(k)) continue; - const size_t blockIdx = blockIndex_.at(k); + const size_t blockIdx = blockIndex(k); Ab_(blockIdx).middleRows(rowOffset, rows) = jacobianFactor.getA(it); } Ab_(rhsBlockIdx).middleRows(rowOffset, rows) = jacobianFactor.getb(); if (auto model = jacobianFactor.get_model()) { if (!model->isConstrained()) { + // Only whiten non-constrained rows; constrained factors are handled as + // hard constraints elsewhere. model->WhitenInPlace(Ab_.matrix().middleRows(rowOffset, rows)); } } @@ -238,7 +224,7 @@ void MultifrontalClique::addHessianFactor(const HessianFactor& hessianFactor) { ++it, ++slot) { const Key key = *it; if (fixedKeys_ && fixedKeys_->count(key)) continue; - blockIndices[slot] = static_cast(blockIndex_.at(key)); + blockIndices[slot] = blockIndex(key); } blockIndices[factorBlocks] = rhsBlock; @@ -246,8 +232,8 @@ void MultifrontalClique::addHessianFactor(const HessianFactor& hessianFactor) { } void MultifrontalClique::fillAb(const GaussianFactorGraph& graph) { - assert(validateFactorKeys(graph, factorIndices_, blockIndex_, fixedKeys_)); - sbm_.setZero(); // Easily half of the cost ! + assert(validateFactorKeys(graph, factorIndices_, orderedKeys_, fixedKeys_)); + sbm_.setZero(); // Zero the active SBM once before accumulating factors. size_t rowOffset = 0; for (size_t index : factorIndices_) { @@ -278,6 +264,7 @@ void MultifrontalClique::eliminateInPlace() { void MultifrontalClique::updateParent(MultifrontalClique& parent) const { // Expose only the separator+RHS view when contributing to the parent. + assert(sbm_.blockStart() == 0); sbm_.blockStart() = numFrontals(); assert(sbm_.nBlocks() == parentIndices_.size()); parent.sbm_.updateFromMappedBlocks(sbm_, parentIndices_); @@ -285,10 +272,10 @@ void MultifrontalClique::updateParent(MultifrontalClique& parent) const { } std::shared_ptr MultifrontalClique::conditional() const { - const KeyVector keys = orderedKeysFromBlockIndex(blockIndex_); - SymmetricBlockMatrix& sbm = sbm_; - VerticalBlockMatrix Ab = sbm.split(numFrontals()); - sbm.blockStart() = 0; // Split sets it to numFrontals(), reset to 0. + const KeyVector& keys = orderedKeys_; + assert(sbm_.blockStart() == 0); + VerticalBlockMatrix Ab = sbm_.split(numFrontals()); + sbm_.blockStart() = 0; // Split sets it to numFrontals(), reset to 0. return std::make_shared(keys, numFrontals(), std::move(Ab)); } @@ -307,7 +294,7 @@ void MultifrontalClique::updateSolution() const { // We first solve rhs = d - S * x_s rhsScratch_.noalias() = d; - if (n > nf) { + if (!separatorPtrs_.empty()) { const Vector& x_s = buildSeparatorVector(separatorPtrs_, &separatorScratch_); rhsScratch_.noalias() -= S * x_s; @@ -316,7 +303,7 @@ void MultifrontalClique::updateSolution() const { // Then solve for x_f, our solution, via R * x_f = rhs // We solve the contiguous frontal system in one triangular solve. R.solveInPlace(rhsScratch_); - auto& x_f = rhsScratch_; + const Vector& x_f = rhsScratch_; // Write solved frontal blocks back into the global solution. size_t offset = 0; @@ -330,7 +317,7 @@ void MultifrontalClique::updateSolution() const { void MultifrontalClique::print(const std::string& s, const KeyFormatter& keyFormatter) const { if (!s.empty()) std::cout << s; - const KeyVector orderedKeys = orderedKeysFromBlockIndex(blockIndex_); + const KeyVector& orderedKeys = orderedKeys_; std::cout << "Clique(frontals=["; printKeyRange(std::cout, orderedKeys, 0, std::min(numFrontals(), orderedKeys.size()), keyFormatter); @@ -364,7 +351,7 @@ void MultifrontalClique::print(const std::string& s, } std::ostream& operator<<(std::ostream& os, const MultifrontalClique& clique) { - const KeyVector orderedKeys = orderedKeysFromBlockIndex(clique.blockIndex_); + const KeyVector& orderedKeys = clique.orderedKeys_; const KeyFormatter formatter = DefaultKeyFormatter; os << "Clique(frontals="; printKeyRange(os, orderedKeys, 0, diff --git a/gtsam/linear/MultifrontalClique.h b/gtsam/linear/MultifrontalClique.h index 77d12b321a..bc5c5f527d 100644 --- a/gtsam/linear/MultifrontalClique.h +++ b/gtsam/linear/MultifrontalClique.h @@ -38,6 +38,9 @@ namespace gtsam { class GaussianConditional; +/// Map from variable key to dimension. +using KeyDimMap = std::map; + namespace internal { /// Helper class to track original factor indices. @@ -48,10 +51,19 @@ class IndexedSymbolicFactor : public SymbolicFactor { : SymbolicFactor(), index_(index) { keys_ = keys; } - IndexedSymbolicFactor(const GaussianFactor& factor, size_t index) - : SymbolicFactor(factor), index_(index) {} }; +/// Sum variable dimensions for a key range, skipping unknown keys. +template +inline size_t sumDims(const KeyDimMap& dims, const KeyRange& keys) { + size_t dim = 0; + for (Key key : keys) { + auto it = dims.find(key); + if (it != dims.end()) dim += it->second; + } + return dim; +} + } // namespace internal /** @@ -63,7 +75,7 @@ class GTSAM_EXPORT MultifrontalClique { using Children = std::vector; struct ChildInfo { shared_ptr clique; - KeyVector separatorKeys; + KeySet separatorKeys; }; std::weak_ptr parent; ///< Parent clique. @@ -83,8 +95,8 @@ class GTSAM_EXPORT MultifrontalClique { explicit MultifrontalClique(std::vector factorIndices, const std::weak_ptr& parent, const KeyVector& frontals, - const KeyVector& separatorKeys, - const std::map& dims, + const KeySet& separatorKeys, + const KeyDimMap& dims, const GaussianFactorGraph& graph, VectorValues* solution, const std::unordered_set* fixedKeys); @@ -175,12 +187,15 @@ class GTSAM_EXPORT MultifrontalClique { private: /// Cache pointers to frontal and separator update vectors. void cacheSolutionPointers(VectorValues* delta, const KeyVector& frontals, - const KeyVector& separatorKeys); + const KeySet& separatorKeys); + + /// Linear lookup for block index in small cliques. + DenseIndex blockIndex(Key key) const; /// Compute block dimensions from variable dimensions (excluding RHS). - std::vector blockDims(const std::map& dims, + std::vector blockDims(const KeyDimMap& dims, const KeyVector& frontals, - const KeyVector& separatorKeys) const; + const KeySet& separatorKeys) const; /** * Pre-allocate matrices for this clique. @@ -209,10 +224,10 @@ class GTSAM_EXPORT MultifrontalClique { separatorScratch_; ///< Cached separator stack for back-substitution. std::vector factorIndices_; - std::map blockIndex_; ///< Key->block index for fast Ab fills. + KeyVector orderedKeys_; ///< Keys ordered by block index (frontals+seps). const std::unordered_set* fixedKeys_ = nullptr; std::vector - parentIndices_; ///< Parent block indices for separators and RHS. + parentIndices_; ///< Parent block indices for separators + RHS. std::vector frontalPtrs_; ///< Pointers into solution frontals. std::vector separatorPtrs_; ///< Pointers into solution separator. diff --git a/gtsam/linear/MultifrontalSolver.cpp b/gtsam/linear/MultifrontalSolver.cpp index 9b27e60889..6cf6b2d459 100644 --- a/gtsam/linear/MultifrontalSolver.cpp +++ b/gtsam/linear/MultifrontalSolver.cpp @@ -96,6 +96,7 @@ std::unordered_set collectFixedKeys(const GaussianFactorGraph& graph) { if (!jacobianFactor) continue; auto model = jacobianFactor->get_model(); if (!model || !model->isConstrained()) continue; + // Only accept fully constrained unary factors with zero residuals. if (jacobianFactor->size() != 1) { throw std::runtime_error( "MultifrontalSolver: only unary constrained factors are supported."); @@ -151,6 +152,7 @@ SymbolicFactorGraph buildSymbolicGraph( keys.push_back(key); } } + // Skip factors that are fully constrained away. if (keys.empty()) continue; symbolicGraph.emplace_shared(keys, i); } @@ -161,12 +163,7 @@ SymbolicFactorGraph buildSymbolicGraph( size_t frontalDimForSymbolicCluster( const SymbolicJunctionTree::sharedNode& cluster, const std::map& dims) { - size_t dim = 0; - for (Key key : cluster->orderedFrontalKeys) { - auto it = dims.find(key); - if (it != dims.end()) dim += it->second; - } - return dim; + return internal::sumDims(dims, cluster->orderedFrontalKeys); } size_t separatorDimForSymbolicCluster( @@ -174,13 +171,8 @@ size_t separatorDimForSymbolicCluster( const std::map& dims, SymbolicJunctionTree::Cluster::KeySetMap* cache) { if (!cluster) return 0; - size_t dim = 0; const KeySet separatorKeys = cluster->separatorKeys(cache); - for (Key key : separatorKeys) { - auto it = dims.find(key); - if (it != dims.end()) dim += it->second; - } - return dim; + return internal::sumDims(dims, separatorKeys); } size_t totalDimForSymbolicCluster( @@ -246,13 +238,6 @@ void reportStructure(const SymbolicJunctionTree& junctionTree, stats.report(name, reportStream); } -KeyVector keyVectorFromKeySet(const KeySet& keys) { - KeyVector result; - result.reserve(keys.size()); - for (Key key : keys) result.push_back(key); - return result; -} - std::vector factorIndicesForSymbolicCluster( const SymbolicJunctionTree::sharedNode& cluster) { std::vector indices; @@ -268,7 +253,7 @@ std::vector factorIndicesForSymbolicCluster( struct BuiltClique { MultifrontalSolver::CliquePtr clique; - KeyVector separatorKeys; + KeySet separatorKeys; }; // Build cliques from a symbolic junction tree and wire parent/child metadata. @@ -277,17 +262,16 @@ struct CliqueBuilder { const GaussianFactorGraph& graph; VectorValues* solution; std::vector* cliques; - SymbolicJunctionTree::Cluster::KeySetMap separatorCache; const std::unordered_set* fixedKeys; + SymbolicJunctionTree::Cluster::KeySetMap separatorCache = {}; BuiltClique build(const SymbolicJunctionTree::sharedNode& cluster, std::weak_ptr parent) { - if (!cluster) return {nullptr, KeyVector()}; + if (!cluster) return {nullptr, KeySet()}; // Gather symbolic metadata for this clique. const KeyVector& frontals = cluster->orderedFrontalKeys; - KeyVector separatorKeys = - keyVectorFromKeySet(cluster->separatorKeys(&separatorCache)); + const KeySet& separatorKeys = cluster->separatorKeys(&separatorCache); // Create the clique node and cache static structure. auto clique = std::make_shared( @@ -319,9 +303,11 @@ MultifrontalSolver::MultifrontalSolver(const GaussianFactorGraph& graph, const Ordering& ordering, size_t mergeDimCap, std::ostream* reportStream) { - // Pre-compute variable dimensions + // Pre-compute variable dimensions and fixed keys. dims_ = computeDims(graph); fixedKeys_ = collectFixedKeys(graph); + + // Seed the cached solution with zero vectors for all variables. Ordering reducedOrdering; reducedOrdering.reserve(ordering.size()); for (Key key : ordering) { @@ -357,7 +343,7 @@ MultifrontalSolver::MultifrontalSolver(const GaussianFactorGraph& graph, } // Build the actual MultifrontalClique structure. - CliqueBuilder builder{dims_, graph, &solution_, &cliques_, {}, &fixedKeys_}; + CliqueBuilder builder{dims_, graph, &solution_, &cliques_, &fixedKeys_}; for (const auto& rootCluster : junctionTree.roots()) { if (rootCluster) { roots_.push_back( @@ -381,13 +367,10 @@ void MultifrontalSolver::load(const GaussianFactorGraph& graph) { void MultifrontalSolver::eliminateInPlace() { // Parallel elimination uses PostOrderForestParallel, which will be // multi-threaded if GTSAM was compiled with TBB. - struct EliminatePostVisitor { - void operator()(const CliquePtr& node) const { - if (node) node->eliminateInPlace(); - } - }; - EliminatePostVisitor visitorPost; TbbOpenMPMixedScope threadLimiter; + auto visitorPost = [](const CliquePtr& node) { + if (node) node->eliminateInPlace(); + }; treeTraversal::PostOrderForestParallel(*this, visitorPost, 10); } @@ -434,9 +417,22 @@ GaussianBayesTree MultifrontalSolver::computeBayesTree() const { /* ************************************************************************* */ const VectorValues& MultifrontalSolver::updateSolution() const { - for (const auto& clique : cliques_) { - clique->updateSolution(); - } + // Parallel solve uses treeTraversal::DepthFirstForestParallel (Pre-order / + // Top-Down). + TbbOpenMPMixedScope threadLimiter; + int rootData = 0; + auto visitorPre = [](const CliquePtr& node, int&) { + if (node) node->updateSolution(); + return 0; + }; + auto visitorPost = [](const CliquePtr&, int) {}; + + // Cast to non-const because treeTraversal expects a non-const reference, + // even though we are only calling const methods on the nodes. + treeTraversal::DepthFirstForestParallel( + const_cast(*this), rootData, visitorPre, visitorPost, + 10); + for (Key key : fixedKeys_) { solution_.at(key).setZero(); } diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 6c6b471e6f..2618fe3b29 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { @@ -106,6 +107,7 @@ SfmData SfmData::FromBundlerFile(const std::string &filename) { throw std::runtime_error( "Error in FromBundlerFile: can not find the file!!"); } + is.imbue(std::locale::classic()); SfmData sfmData; @@ -115,20 +117,30 @@ SfmData SfmData::FromBundlerFile(const std::string &filename) { // Get the number of camera poses and 3D points size_t nrPoses, nrPoints; - is >> nrPoses >> nrPoints; + if (!(is >> nrPoses >> nrPoints)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read header from file"); + } // Get the information for the camera poses for (size_t i = 0; i < nrPoses; i++) { // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; + double f = 0.0, k1 = 0.0, k2 = 0.0; + if (!(is >> f >> k1 >> k2)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read camera calibration"); + } Cal3Bundler K(f, k1, k2); // Get the rotation matrix - float r11, r12, r13; - float r21, r22, r23; - float r31, r32, r33; - is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; + double r11 = 0.0, r12 = 0.0, r13 = 0.0; + double r21 = 0.0, r22 = 0.0, r23 = 0.0; + double r31 = 0.0, r32 = 0.0, r33 = 0.0; + if (!(is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> + r33)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read camera rotation"); + } // Bundler-OpenGL rotation matrix Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); @@ -140,8 +152,11 @@ SfmData SfmData::FromBundlerFile(const std::string &filename) { } // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; + double tx = 0.0, ty = 0.0, tz = 0.0; + if (!(is >> tx >> ty >> tz)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read camera translation"); + } Pose3 pose = openGL2gtsam(R, tx, ty, tz); @@ -154,27 +169,43 @@ SfmData SfmData::FromBundlerFile(const std::string &filename) { SfmTrack track; // Get the 3D position - float x, y, z; - is >> x >> y >> z; + double x = 0.0, y = 0.0, z = 0.0; + if (!(is >> x >> y >> z)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read point coordinates"); + } track.p = Point3(x, y, z); // Get the color information - float r, g, b; - is >> r >> g >> b; + double r = 0.0, g = 0.0, b = 0.0; + if (!(is >> r >> g >> b)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read point color"); + } track.r = r / 255.f; track.g = g / 255.f; track.b = b / 255.f; // Now get the visibility information size_t nvisible = 0; - is >> nvisible; + if (!(is >> nvisible)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read visibility count"); + } track.measurements.reserve(nvisible); track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; - float u, v; - is >> cam_idx >> point_idx >> u >> v; + double u = 0.0, v = 0.0; + if (!(is >> cam_idx >> point_idx >> u >> v)) { + throw std::runtime_error( + "Error in FromBundlerFile: failed to read measurement data"); + } + if (cam_idx >= nrPoses) { + throw std::runtime_error( + "Error in FromBundlerFile: measurement camera index out of range"); + } track.measurements.emplace_back(cam_idx, Point2(u, -v)); track.siftIndices.emplace_back(cam_idx, point_idx); } @@ -192,39 +223,59 @@ SfmData SfmData::FromBalFile(const std::string &filename) { if (!is) { throw std::runtime_error("Error in FromBalFile: can not find the file!!"); } + is.imbue(std::locale::classic()); SfmData sfmData; // Get the number of camera poses and 3D points size_t nrPoses, nrPoints, nrObservations; - is >> nrPoses >> nrPoints >> nrObservations; + if (!(is >> nrPoses >> nrPoints >> nrObservations)) { + throw std::runtime_error( + "Error in FromBalFile: failed to read header from file"); + } sfmData.tracks.resize(nrPoints); // Get the information for the observations for (size_t k = 0; k < nrObservations; k++) { size_t i = 0, j = 0; - float u, v; - is >> i >> j >> u >> v; + double u = 0.0, v = 0.0; + if (!(is >> i >> j >> u >> v)) { + throw std::runtime_error( + "Error in FromBalFile: failed to read observation data"); + } + if (i >= nrPoses || j >= nrPoints) { + throw std::runtime_error( + "Error in FromBalFile: observation index out of range"); + } sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses for (size_t i = 0; i < nrPoses; i++) { // Get the Rodrigues vector - float wx, wy, wz; - is >> wx >> wy >> wz; + double wx = 0.0, wy = 0.0, wz = 0.0; + if (!(is >> wx >> wy >> wz)) { + throw std::runtime_error( + "Error in FromBalFile: failed to read camera rotation"); + } Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; + double tx = 0.0, ty = 0.0, tz = 0.0; + if (!(is >> tx >> ty >> tz)) { + throw std::runtime_error( + "Error in FromBalFile: failed to read camera translation"); + } Pose3 pose = openGL2gtsam(R, tx, ty, tz); // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; + double f = 0.0, k1 = 0.0, k2 = 0.0; + if (!(is >> f >> k1 >> k2)) { + throw std::runtime_error( + "Error in FromBalFile: failed to read camera calibration"); + } Cal3Bundler K(f, k1, k2); sfmData.cameras.emplace_back(pose, K); @@ -233,8 +284,11 @@ SfmData SfmData::FromBalFile(const std::string &filename) { // Get the information for the 3D points for (size_t j = 0; j < nrPoints; j++) { // Get the 3D position - float x, y, z; - is >> x >> y >> z; + double x = 0.0, y = 0.0, z = 0.0; + if (!(is >> x >> y >> z)) { + throw std::runtime_error( + "Error in FromBalFile: failed to read point coordinates"); + } SfmTrack &track = sfmData.tracks[j]; track.p = Point3(x, y, z); track.r = 0.4f; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 91317bad1e..d8731a2506 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -131,6 +132,7 @@ static void parseLines(const std::string &filename, Parser parse) { std::ifstream is(filename.c_str()); if (!is) throw std::invalid_argument("parse: can not find file " + filename); + is.imbue(std::locale::classic()); std::string tag; while (is >> tag) { parse(is, tag); // ignore return value diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 25943c2aaf..f83cca9f80 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -132,7 +132,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); // params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); - // params.setVerbosityLM("SUMMARY"); + params.setVerbosityLM("SUMMARY"); + params.setRelativeErrorTol(0.01); // 1% relative error tol if (gUseSchur) { // Create Schur-complement ordering