From 6fce7fa0cf64a21720e4a3a76c68fcb7f27604c1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 3 Jun 2021 08:42:28 -0700 Subject: [PATCH] Removing parameter blocks for localization [noetic] (#405) --- slam_toolbox/solvers/ceres_solver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/slam_toolbox/solvers/ceres_solver.cpp b/slam_toolbox/solvers/ceres_solver.cpp index db56e441d..87ea5d8ec 100644 --- a/slam_toolbox/solvers/ceres_solver.cpp +++ b/slam_toolbox/solvers/ceres_solver.cpp @@ -358,9 +358,9 @@ void CeresSolver::RemoveNode(kt_int32s id) GraphIterator nodeit = nodes_->find(id); if (nodeit != nodes_->end()) { - problem_->SetParameterBlockConstant(&first_node_->second(0)); - problem_->SetParameterBlockConstant(&first_node_->second(1)); - problem_->SetParameterBlockConstant(&first_node_->second(2)); + problem_->RemoveParameterBlock(&nodeit->second(0)); + problem_->RemoveParameterBlock(&nodeit->second(1)); + problem_->RemoveParameterBlock(&nodeit->second(2)); nodes_->erase(nodeit); } else