methods to anchor first pose

mit-acl · Feb 10, 2023 · 1b07169 · 1b07169
1 parent 0af66a3
commit 1b07169
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 0 deletions.
diff --git a/include/DPGO/PGOAgent.h b/include/DPGO/PGOAgent.h
@@ -719,6 +719,11 @@ class PGOAgent {
    * @brief Return the number of currently active robots
   */
   size_t numActiveRobots() const;
+  /**
+   * @brief Add a prior to the first pose of this robot
+  */
+  bool anchorFirstPose();
+  bool anchorFirstPose(const LiftedPose &prior);
 
  private:
   // Stores the auxiliary variables from neighbors (only used in acceleration)

diff --git a/src/PGOAgent.cpp b/src/PGOAgent.cpp
@@ -341,6 +341,14 @@ void PGOAgent::initializeInGlobalFrame(const Pose &T_world_robot) {
     initializeRobustOptimization();
   }
 
+  // For robot 0, anchor its first pose to fix the global frame
+  if (getID() == 0) {
+    LiftedPose prior(relaxation_rank(), dimension());
+    prior.rotation() = YLift.value();
+    prior.translation() = Vector::Zero(r);
+    anchorFirstPose(prior);
+  }
+
   // Initialize auxiliary variables
   if (mParams.acceleration) {
     initializeAcceleration();
@@ -1175,4 +1183,22 @@ size_t PGOAgent::numActiveRobots() const {
   return num_active;
 }
 
+bool PGOAgent::anchorFirstPose() {
+  if (num_poses() > 0) {
+    LiftedPose prior(relaxation_rank(), dimension());
+    prior.setData(X.pose(0));
+    mPoseGraph->setPrior(0, prior);
+  } else {
+    return false;
+  }
+  return true;
+}
+
+bool PGOAgent::anchorFirstPose(const LiftedPose &prior) {
+  CHECK_EQ(prior.d(), dimension());
+  CHECK_EQ(prior.r(), relaxation_rank());
+  mPoseGraph->setPrior(0, prior);
+  return true;
+}
+
 }  // namespace DPGO