Removing parameter blocks for localization [noetic] (#405)

SteveMacenski · Jun 3, 2021 · 6fce7fa · 6fce7fa
1 parent 824fb8c
commit 6fce7fa
Showing 1 changed file with 3 additions and 3 deletions.
diff --git 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