Fix bug when load an one-node pose graph. (SteveMacenski#495)

Co-authored-by: Kehan Xue <[email protected]>
malban · May 27, 2022 · 2ae75af · 2ae75af
1 parent 3010d46
commit 2ae75af
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp
@@ -177,7 +177,10 @@ void CeresSolver::Compute()
   }
 
   // populate contraint for static initial pose
-  if (!was_constant_set_ && first_node_ != nodes_->end()) {
+  if (!was_constant_set_ && first_node_ != nodes_->end() &&
+      problem_->HasParameterBlock(&first_node_->second(0)) &&
+      problem_->HasParameterBlock(&first_node_->second(1)) &&
+      problem_->HasParameterBlock(&first_node_->second(2))) {
     RCLCPP_DEBUG(node_->get_logger(),
       "CeresSolver: Setting first node as a constant pose:"
       "%0.2f, %0.2f, %0.2f.", first_node_->second(0),