Option to add offset when retracting to 3D poses

Author: lajoiepy

cpp/include/distributed_mapper/distributed_mapper.h

@@ -311,6 +311,11 @@ class DistributedMapper{
  initial_ = evaluation_utils::retractPose3Global(initial_, linearized_poses_);
  }
 
+ /** @brief retractPose3Global performs global retraction using linearizedPoses and initial */
+ void retractPose3GlobalWithOffset(const gtsam::Point3& offset){
+ initial_ = evaluation_utils::retractPose3GlobalWithOffset(initial_, linearized_poses_, offset);
+ }
+
  /** @brief linearizedRotationAt returns the current rotation estimate at sym */
  gtsam::Vector linearizedRotationAt(const gtsam::Key& key){ return linearized_rotation_.at(key); }
 

cpp/include/distributed_mapper/evaluation_utils.h

@@ -109,6 +109,11 @@ namespace distributed_mapper{
  */
  gtsam::Values retractPose3Global(const gtsam::Values& initial, const gtsam::VectorValues& delta);
 
+ /**
+ * @brief retractPose3Global adds delta to initial in global frame with offset
+ */
+ gtsam::Values retractPose3GlobalWithOffset(const gtsam::Values& initial, const gtsam::VectorValues& delta, const gtsam::Point3& offset);
+
  /**
  * @brief buildLinearOrientationGraph generates a linear orientation graph given the nonlinear factor graph and uses the between noise
  */

cpp/src/distributed_mapper/evaluation_utils.cpp

@@ -98,6 +98,20 @@ namespace evaluation_utils{
  return estimate;
  }
 
+ //*****************************************************************************
+ Values retractPose3GlobalWithOffset(const Values& initial, const VectorValues& delta, const gtsam::Point3& offset) {
+ Values estimate;
+ for (const Values::ConstKeyValuePair &key_value: initial) {
+ Key key = key_value.key;
+ Vector6 delta_pose = delta.at(key);
+ Rot3 R = initial.at<Pose3>(key).rotation().retract(delta_pose.head(3));
+ Point3 t_initial = initial.at<Pose3>(key).translation();
+ Point3 t = Point3(delta_pose.tail(3)) + offset;
+ estimate.insert(key, Pose3(R, t));
+ }
+ return estimate;
+ }
+
  //*****************************************************************************
  pair<vector<NonlinearFactorGraph>, vector<Values> >
  loadSubgraphs(const size_t& num_subgraphs, const string& data_path) {