diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph-inl.h index 264ab9430..b27da1b15 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph-inl.h @@ -80,6 +80,8 @@ void NonlinearFactorGraph::print(const std::string& str) const { // Compute the VariableIndex (column-wise index) VariableIndex variableIndex(*symbolic); + if(config.size() != variableIndex.size()) + throw std::runtime_error("orderingCOLAMD: some variables in the graph are not constrained!"); // Compute a fill-reducing ordering with COLAMD Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex));