Feature: initialize DLO with known pose

cfg/params.yaml

@@ -11,6 +11,18 @@ dlo:
 
   odomNode:
 
+    initialPose:
+      use: false
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.0
+      orientation:
+        w: 1.0
+        x: 0.0
+        y: 0.0
+        z: 0.0
+
     preprocessing:
       cropBoxFilter:
         use: true

include/dlo/odom.h

@@ -75,6 +75,7 @@ class dlo::OdomNode {
   ros::Publisher keyframe_pub;
   ros::Publisher kf_pub;
 
+  Eigen::Vector3f origin;
   std::vector<std::pair<Eigen::Vector3f, Eigen::Quaternionf>> trajectory;
   std::vector<std::pair<std::pair<Eigen::Vector3f, Eigen::Quaternionf>, pcl::PointCloud<PointType>::Ptr>> keyframes;
   std::vector<std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>> keyframe_normals;
@@ -205,6 +206,10 @@ class dlo::OdomNode {
   int submap_kcc_;
   double submap_concave_alpha_;
 
+  bool initial_pose_use_;
+  Eigen::Vector3f initial_position_;
+  Eigen::Quaternionf initial_orientation_;
+
   bool crop_use_;
   double crop_size_;
 

src/dlo/odom.cc

@@ -45,6 +45,8 @@ dlo::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) {
   this->odom.pose.pose.orientation.z = 0.;
   this->odom.pose.covariance = {0.};
 
+  this->origin = Eigen::Vector3f(0., 0., 0.);
+
   this->T = Eigen::Matrix4f::Identity();
   this->T_s2s = Eigen::Matrix4f::Identity();
   this->T_s2s_prev = Eigen::Matrix4f::Identity();
@@ -206,6 +208,20 @@ void dlo::OdomNode::getParams() {
   ros::param::param<int>("~dlo/odomNode/submap/keyframe/kcv", this->submap_kcv_, 10);
   ros::param::param<int>("~dlo/odomNode/submap/keyframe/kcc", this->submap_kcc_, 10);
 
+  // Initial Position
+  ros::param::param<bool>("~dlo/odomNode/initialPose/use", this->initial_pose_use_, false);
+
+  double px, py, pz, qx, qy, qz, qw;
+  ros::param::param<double>("~dlo/odomNode/initialPose/position/x", px, 0.0);
+  ros::param::param<double>("~dlo/odomNode/initialPose/position/y", py, 0.0);
+  ros::param::param<double>("~dlo/odomNode/initialPose/position/z", pz, 0.0);
+  ros::param::param<double>("~dlo/odomNode/initialPose/orientation/w", qw, 1.0);
+  ros::param::param<double>("~dlo/odomNode/initialPose/orientation/x", qx, 0.0);
+  ros::param::param<double>("~dlo/odomNode/initialPose/orientation/y", qy, 0.0);
+  ros::param::param<double>("~dlo/odomNode/initialPose/orientation/z", qz, 0.0);
+  this->initial_position_ = Eigen::Vector3f(px, py, pz);
+  this->initial_orientation_ = Eigen::Quaternionf(qw, qx, qy, qz);
+
   // Crop Box Filter
   ros::param::param<bool>("~dlo/odomNode/preprocessing/cropBoxFilter/use", this->crop_use_, false);
   ros::param::param<double>("~dlo/odomNode/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0);
@@ -557,11 +573,31 @@ void dlo::OdomNode::initializeDLO() {
   }
 
   // Gravity Align
-  if (this->gravity_align_ && this->imu_use_ && this->imu_calibrated) {
+  if (this->gravity_align_ && this->imu_use_ && this->imu_calibrated && !this->initial_pose_use_) {
     std::cout << "Aligning to gravity... "; std::cout.flush();
     this->gravityAlign();
   }
 
+  // Use initial known pose
+  if (this->initial_pose_use_) {
+    std::cout << "Setting known initial pose... "; std::cout.flush();
+
+    // set known position
+    this->pose = this->initial_position_;
+    this->T.block(0,3,3,1) = this->pose;
+    this->T_s2s.block(0,3,3,1) = this->pose;
+    this->T_s2s_prev.block(0,3,3,1) = this->pose;
+    this->origin = this->initial_position_;
+
+    // set known orientation
+    this->rotq = this->initial_orientation_;
+    this->T.block(0,0,3,3) = this->rotq.toRotationMatrix();
+    this->T_s2s.block(0,0,3,3) = this->rotq.toRotationMatrix();
+    this->T_s2s_prev.block(0,0,3,3) = this->rotq.toRotationMatrix();
+
+    std::cout << "done" << std::endl << std::endl;
+  }
+
   this->dlo_initialized = true;
   std::cout << "DLO initialized! Starting localization..." << std::endl;
 
@@ -1357,7 +1393,7 @@ void dlo::OdomNode::debug() {
   std::cout << "Position    [xyz]  :: " << this->pose[0] << " " << this->pose[1] << " " << this->pose[2] << std::endl;
   std::cout << "Orientation [wxyz] :: " << this->rotq.w() << " " << this->rotq.x() << " " << this->rotq.y() << " " << this->rotq.z() << std::endl;
   std::cout << "Distance Traveled  :: " << length_traversed << " meters" << std::endl;
-  std::cout << "Distance to Origin :: " << sqrt(pow(this->pose[0],2) + pow(this->pose[1],2) + pow(this->pose[2],2)) << " meters" << std::endl;
+  std::cout << "Distance to Origin :: " << sqrt(pow(this->pose[0]-this->origin[0],2) + pow(this->pose[1]-this->origin[1],2) + pow(this->pose[2]-this->origin[2],2)) << " meters" << std::endl;
 
   std::cout << std::endl << std::right << std::setprecision(2) << std::fixed;
   std::cout << "Computation Time :: " << std::setfill(' ') << std::setw(6) << this->comp_times.back()*1000. << " ms    // Avg: " << std::setw(5) << avg_comp_time*1000. << std::endl;