[Patrick Pletscher] Fixed performance issue in FactorGraph::clamp and FactorGraph...