[DONT MERGE] Dev/better initialization

libpointmatcher/pointmatcher/ICP.cpp

@@ -1306,7 +1306,7 @@ typename PointMatcher<T>::TransformationParameters PointMatcher<T>::ICP::compute
         //this->errorMinimizer->appendErrorElements(this->errorMinimizer->getErrorElements());
 
         // std::cout << "scalabilityStudy: " << scalabilityStudyInMiliSeconds << " NbMatches: " << this->localizabilityDetectionParameters.numberOfPoints << std::endl;
-
+        this->residualErr_ = this->errorMinimizer->getResidualError(stepReading, reference, this->outlierWeights, this->matches);
         ++iterationCount;
     }
 

libpointmatcher/pointmatcher/PointMatcher.h

@@ -968,6 +968,9 @@ struct PointMatcher
 
 		Matrix ceresEigVect_ = Matrix::Zero(6,6);
 
+		T residualErr_ = 0.0f;
+		const T& getResidualError() const { return residualErr_;}
+
 		//! Return the latest outlier weights for the reading point cloud.
 		const OutlierWeights& getOutlierWeights() const { return outlierWeights; }
 

open3d_slam_rsl/open3d_slam/open3d_slam/include/open3d_slam/Mapper.hpp

@@ -57,6 +57,13 @@ class Mapper {
 
   void setExternalOdometryFrameToCloudFrameCalibration(const Eigen::Isometry3d& transform);
   bool isExternalOdometryFrameToCloudFrameCalibrationSet();
+  pointmatcher_ros::PmTfParameters scan2mapRegistrationWrapper(
+      const std::shared_ptr<open3d_conversions::PmStampedPointCloud>& croppedCloud,
+      const pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess);
+
+  pointmatcher_ros::PmTfParameters scan2mapRegistrationLargeBasin(
+      const std::shared_ptr<open3d_conversions::PmStampedPointCloud>& croppedCloud,
+      pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess);
 
   // This is re-initialized in the constructor as well as by a setter.
   Transform calibration_ = Transform::Identity();
@@ -66,6 +73,7 @@ class Mapper {
   nav_msgs::Path bestGuessPath_;
   bool isNewValueSetMapper_ = false;
   bool isInitialTransformSet_ = false;
+  bool attemptedLargeBasin_ = false;
 
   // The pointmatcher registration object.
   // The parameter loading dont have a slam_ API yet, thus object not private.
@@ -99,6 +107,7 @@ class Mapper {
 
   bool isIgnoreOdometryPrediction_ = false;
   bool firstRefinement_ = true;
+  float registrationFitness_ = 1.0f;
 
   std::shared_ptr<ScanToMapRegistration> scan2MapReg_;
 

open3d_slam_rsl/open3d_slam/open3d_slam/src/Mapper.cpp

@@ -154,6 +154,7 @@ void Mapper::setMapToRangeSensorInitial(const Transform& t) {
   mapToRangeSensor_ = t;
   isNewValueSetMapper_ = true;
   isInitialTransformSet_ = true;
+  attemptedLargeBasin_ = false;
 }
 
 const PointCloud& Mapper::getPreprocessedScan() const {
@@ -260,7 +261,7 @@ bool Mapper::addRangeMeasurement(const Mapper::PointCloud& rawScan, const Time&
     mapToRangeSensorEstimate = mapToRangeSensorPrev_ * odometryMotion;
 
     if (odometryMotion.translation().norm() == 0.0) {
-      std::cout << " Odometry MOTION SHOULDNT BE PERFECTLY 0. "
+      std::cout << " Odometry MOTION SHOULDNT BE PERFECTLY 0. Your Odometry system is not running. "
                 << "\033[92m" << asString(odometryMotion) << " \n"
                 << "\033[0m";
     }
@@ -364,21 +365,50 @@ bool Mapper::addRangeMeasurement(const Mapper::PointCloud& rawScan, const Time&
     // The +1000 is to prevent early triggering of the condition. Since points might decrease due to carving.
     // if(activeSubmapPm_->dataPoints_.features.cols() > croppedCloud->dataPoints_.features.cols() + 1000){
 
-    {
-      std::lock_guard<std::mutex> lck(mapManipulationMutex_);
+    if ((params_.isUseInitialMap_ && (!attemptedLargeBasin_)) && ((!firstRefinement_) || (!attemptedLargeBasin_))) {
+      correctedTransform = scan2mapRegistrationLargeBasin(croppedCloud, transformReadingToReferenceInitialGuess);
 
-      // We are explicitly setting the reference cloud above. Hence the empty placeholder.
-      open3d_conversions::PmStampedPointCloud emptyPlaceholder;
-      correctedTransform =
-          icp_.compute(croppedCloud->dataPoints_, emptyPlaceholder.dataPoints_, transformReadingToReferenceInitialGuess, false);
+    } else {
+      // std::cout << "Registering." << std::endl;
+      correctedTransform = scan2mapRegistrationWrapper(croppedCloud, transformReadingToReferenceInitialGuess);
     }
 
-    if (firstRefinement_) {
+    if ((firstRefinement_ && registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) ||
+        (firstRefinement_ && !params_.isUseInitialMap_)) {
       std::cout << "\033[92m"
-                << " Open3d SLAM is running properly."
+                << " Open3d SLAM is running properly. " << registrationFitness_ << " / " << params_.scanMatcher_.minRefinementFitness_
                 << " \n "
                 << "\033[0m";
       firstRefinement_ = false;
+      isNewValueSetMapper_ = false;
+      mapToRangeSensorEstimate.matrix() = transformReadingToReferenceInitialGuess.matrix().cast<double>();
+    }
+
+    if (isNewValueSetMapper_ && registrationFitness_ > params_.scanMatcher_.minRefinementFitness_) {
+      std::cout << "\033[31m"
+                << " Open3d SLAM Overlap: " << registrationFitness_ << " / " << params_.scanMatcher_.minRefinementFitness_ << ". "
+                << " Not sufficent. Give another initial guess please."
+                << "\033[0m"
+                << " \n ";
+
+      if (isTimeValid(timestamp)) {
+        initTime_ = timestamp;
+      }
+
+      std::cout << "\033[92m"
+                << "Setting initial value for the map to range sensor transform. ONLY expected at the start-up."
+                << "\033[0m"
+                << "\n";
+
+      mapToRangeSensorEstimate = mapToRangeSensor_;
+      mapToRangeSensorPrev_ = mapToRangeSensor_;
+      mapToRangeSensorBuffer_.clear();
+      mapToRangeSensorBuffer_.push(timestamp, mapToRangeSensor_);
+      bestGuessBuffer_.clear();
+      bestGuessBuffer_.push(timestamp, mapToRangeSensorEstimate);
+      isNewValueSetMapper_ = false;
+      isIgnoreOdometryPrediction_ = true;
+      return true;
     }
 
     const double timeElapsed = testmapperOnlyTimer_.elapsedMsecSinceStopwatchStart();
@@ -387,7 +417,8 @@ bool Mapper::addRangeMeasurement(const Mapper::PointCloud& rawScan, const Time&
     if (params_.isPrintTimingStatistics_) {
       std::cout << " Scan2Map Registration: "
                 << "\033[92m" << timeElapsed << " msec \n "
-                << "\033[0m";
+                << "\033[0m"
+                << " \n ";
     }
 
     //}else{
@@ -411,13 +442,20 @@ bool Mapper::addRangeMeasurement(const Mapper::PointCloud& rawScan, const Time&
   */
 
   // Pass the calculated transform to o3d transform.
-  Transform correctedTransform_o3d;
-  correctedTransform_o3d.matrix() = correctedTransform.matrix().cast<double>();
+  Transform correctedTransform_o3d = Transform::Identity();
+
+  if ((!params_.isUseInitialMap_) || (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) || (isNewValueSetMapper_)) {
+    // std::cout << "Registering 2." << std::endl;
+    correctedTransform_o3d.matrix() = correctedTransform.matrix().cast<double>();
+  } else {
+    // correctedTransform_o3d.matrix() = transformReadingToReferenceInitialGuess.matrix().cast<double>();
+    correctedTransform_o3d = mapToRangeSensorEstimate;
+  }
 
   // std::cout << "preeIcp: " << asString(mapToRangeSensorEstimate) << "\n";
   // std::cout << "postIcp xicp: " << asString(correctedTransform_o3d) << "\n\n";
 
-  if (isNewValueSetMapper_) {
+  /*if ( ( (isNewValueSetMapper_) && firstRefinement_))  {
     if (isTimeValid(timestamp)) {
       initTime_ = timestamp;
     }
@@ -426,13 +464,17 @@ bool Mapper::addRangeMeasurement(const Mapper::PointCloud& rawScan, const Time&
               << "Setting initial value for the map to range sensor transform. ONLY expected at the start-up."
               << "\033[0m"
               << "\n";
+
+    mapToRangeSensorEstimate = mapToRangeSensor_;
     mapToRangeSensorPrev_ = mapToRangeSensor_;
+    mapToRangeSensorBuffer_.clear();
     mapToRangeSensorBuffer_.push(timestamp, mapToRangeSensor_);
+    bestGuessBuffer_.clear();
     bestGuessBuffer_.push(timestamp, mapToRangeSensorEstimate);
     isNewValueSetMapper_ = false;
     isIgnoreOdometryPrediction_ = true;
     return true;
-  }
+  }*/
 
   // Pass to placeholders variables
   preProcessedScan_ = *processed.match_;
@@ -532,4 +574,136 @@ void Mapper::checkTransformChainingAndPrintResult(bool isCheckTransformChainingA
   }
 }
 
+pointmatcher_ros::PmTfParameters Mapper::scan2mapRegistrationLargeBasin(
+    const std::shared_ptr<open3d_conversions::PmStampedPointCloud>& croppedCloud,
+    pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess) {
+  pointmatcher_ros::PmTfParameters newTransform = transformReadingToReferenceInitialGuess;
+  if (attemptedLargeBasin_) {
+    std::cout << "Already attempted large basin. Returning." << std::endl;
+    return newTransform;
+  }
+
+  attemptedLargeBasin_ = true;
+
+  for (float anglex = 0; anglex < 2.0; anglex = anglex + 0.5) {
+    if (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) {
+      std::cout << "Exitting anglex. Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+      break;
+    }
+
+    for (float angley = 0; angley < 2.0; angley = angley + 0.5) {
+      if (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) {
+        std::cout << "Exitting angley. Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+
+        break;
+      }
+
+      for (float angle = 0; angle < 10.0; angle = angle + 5.0) {
+        // Extra angle
+        Eigen::Matrix3f rotationMatrix = Eigen::AngleAxisf(angle * M_PI / 180, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+        // An identity 4x4 matrix of floats
+        Eigen::Matrix4f homMatrix = Eigen::Matrix4f::Identity();
+
+        // Set the rotation part of the matrix to the rotationMatrix
+        // homMatrix.block<3, 3>(0, 0) = rotationMatrix;
+
+        // Set the translation part of the matrix to a random value selected between -1 and 1 meters in x and y.
+        // std::random_device rd;
+        // std::mt19937 gen(rd());
+        // std::uniform_real_distribution<float> dis(-0.5, 0.5);
+        homMatrix(0, 3) = anglex;  // x translation
+        homMatrix(1, 3) = angley;  // y translation
+
+        pointmatcher_ros::PmTfParameters augmentedGuess = transformReadingToReferenceInitialGuess.matrix() * homMatrix;
+        newTransform = scan2mapRegistrationWrapper(croppedCloud, augmentedGuess);
+        if (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) {
+          // The used angle is console output
+
+          // The augmented initial guess is:
+          // std::cout << "augmentedGuess.matrix(): " << augmentedGuess.matrix() << std::endl;
+
+          std::cout << "Successfull Delta angle: " << angle << std::endl;
+          std::cout << "Successfull Delta X: " << anglex << std::endl;
+          std::cout << "Successfull Delta Y: " << angley << std::endl;
+
+          transformReadingToReferenceInitialGuess = augmentedGuess;
+
+          // the fitness it
+          std::cout << "Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+
+          return newTransform;
+        }
+
+        // std::cout << "Failed Delta Used angle: " << angle << std::endl;
+        // std::cout << "Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+
+        // FOR NEGATIVE ANGLE
+        // Extra angle
+        Eigen::Matrix3f negrotationMatrix = Eigen::AngleAxisf(-angle * M_PI / 180, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+        // An identity 4x4 matrix of floats
+        Eigen::Matrix4f neghomMatrix = Eigen::Matrix4f::Identity();
+
+        // Set the rotation part of the matrix to the rotationMatrix
+        // neghomMatrix.block<3, 3>(0, 0) = negrotationMatrix;
+        neghomMatrix(0, 3) = -anglex;  // x translation
+        neghomMatrix(1, 3) = -angley;  // y translation
+
+        pointmatcher_ros::PmTfParameters negaugmentedGuess = transformReadingToReferenceInitialGuess.matrix() * neghomMatrix;
+        newTransform = scan2mapRegistrationWrapper(croppedCloud, negaugmentedGuess);
+
+        if (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) {
+          // std::cout << "negaugmentedGuess.matrix(): " << negaugmentedGuess.matrix() << std::endl;
+          // The used angle is console output
+          std::cout << "Successfull Delta Angle: " << -angle << std::endl;
+          std::cout << "Successfull Delta X: " << -anglex << std::endl;
+          std::cout << "Successfull Delta Y: " << -angley << std::endl;
+
+          transformReadingToReferenceInitialGuess = negaugmentedGuess;
+
+          // the fitness it
+          std::cout << "Exitting angle. Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+
+          return newTransform;
+        }
+
+        // std::cout << "Failed Used Delta angle: " << -angle << std::endl;
+        std::cout << "Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+      }
+    }
+  }
+
+  if (registrationFitness_ > params_.scanMatcher_.minRefinementFitness_) {
+    std::cout << "\033[31m"
+              << " Couldn't Find a feasible solution. Give another initial guess please."
+              << "\033[0m"
+              << " \n ";
+  }
+  return newTransform;
+}
+
+pointmatcher_ros::PmTfParameters Mapper::scan2mapRegistrationWrapper(
+    const std::shared_ptr<open3d_conversions::PmStampedPointCloud>& croppedCloud,
+    const pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess) {
+  pointmatcher_ros::PmTfParameters correctedTransform;
+
+  {
+    std::lock_guard<std::mutex> lck(mapManipulationMutex_);
+
+    // We are explicitly setting the reference cloud above. Hence the empty placeholder.
+    open3d_conversions::PmStampedPointCloud emptyPlaceholder;
+    correctedTransform =
+        icp_.compute(croppedCloud->dataPoints_, emptyPlaceholder.dataPoints_, transformReadingToReferenceInitialGuess, false);
+    registrationFitness_ = icp_.getResidualError();
+  }
+
+  // Registering fitness is
+  // std::cout << "Registration Fitness: "
+  //          << "\033[92m" << registrationFitness_ << " \n"
+  //          << "\033[0m";
+
+  return correctedTransform;
+}
+
 } /* namespace o3d_slam */

open3d_slam_rsl/open3d_slam/open3d_slam/src/Odometry.cpp

@@ -111,8 +111,8 @@ void LidarOdometry::setInitialTransform(const Eigen::Matrix4d& initialTransform)
   //  if I leave it like this it is always continuous, but starts always from the
   //  origin
   if (isInitialTransformSet_) {
-    std::cout << "\033[31m"
-              << "Open3d_slam odometry initial transform already set. Skipping. OK to see in the beginning."
+    std::cout << "\033[33m"
+              << " Open3d_slam odometry initial transform already set. Skipping. OK to see in the beginning."
               << "\033[0m" << std::endl;
     return;
   }

open3d_slam_rsl/ros/open3d_slam_ros/include/open3d_slam_ros/SlamWrapperRos.hpp

@@ -63,6 +63,7 @@ class SlamWrapperRos : public SlamWrapper {
 
   ros::NodeHandlePtr nh_;
   std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfStaticBroadcaster_;
   ros::Publisher odometryInputPub_, mappingInputPub_, submapOriginsPub_, assembledMapPub_, denseMapPub_, submapsPub_, bestGuessPathPub_,
       trackedPathPub_;
   ros::Publisher differenceLinePub_;

open3d_slam_rsl/ros/open3d_slam_ros/launch/development.launch

@@ -17,7 +17,7 @@
   <!--        /lidar/point_cloud  -->
   <!--        /point_cloud_filter/lidar/point_cloud_filtered -->
   <!--        /hesai/pandar  -->
-  <arg name="cloud_topic" default="/lidar/point_cloud"/> <!-- /point_cloud_filter/lidar_depth_camera/point_cloud_filtered  /lidar/point_cloud -->
+  <arg name="cloud_topic" default="/point_cloud_filter/lidar_depth_camera/point_cloud_filtered"/> <!-- /point_cloud_filter/lidar_depth_camera/point_cloud_filtered  /lidar/point_cloud -->
 
   <!-- The tracked frame of the External odometry. Transformation from this frame to tracked_sensor_Frame should exist. -->
   <!-- Reads as `pose of base in external odometry frame` -->
@@ -37,9 +37,9 @@
   <!--   /zed2i/zed_node/odom  -->
   <!--   /state_estimator/odometry  -->
   <!--   /state_estimator/odometry  -->
-  <arg name="odometry_topic" default="/graph_msf/est_odometry_odom_imu"/> <!-- /point_cloud_filter/lidar_depth_camera/odometry_pose -->
+  <arg name="odometry_topic" default="/graph_msf/est_odometry_odom_imu2"/> <!-- /point_cloud_filter/lidar_depth_camera/odometry_pose -->
 
-  <arg name="pose_stamped_topic" default="/point_cloud_filter/lidar_depth_camera/odometry_pose"/> <!-- /point_cloud_filter/lidar_depth_camera/odometry_pose -->
+  <arg name="pose_stamped_topic" default="/point_cloud_filter/lidar_depth_camera/odometry_pose2"/> <!-- /point_cloud_filter/lidar_depth_camera/odometry_pose -->
 
   <!-- Common Options:  -->
   <!--   /state_estimator/pose_in_odom   -->

open3d_slam_rsl/ros/open3d_slam_ros/param/icp.yaml

@@ -48,11 +48,11 @@ ceresDegeneracyAnalysis:
     regularizationWeight: 500.0 # you need to set  .0  as double
 
 degeneracyAwareness:
-  OptimizedEqualityConstraints:
-    enoughInformationThreshold: 250   #new 250  300
-    insufficientInformationThreshold: 180  #new 180 150
-    point2NormalMinimalAlignmentAngleThreshold: 80
-    point2NormalStrongAlignmentAngleThreshold: 45
+  #OptimizedEqualityConstraints:
+  #  enoughInformationThreshold: 250   #new 250  300
+  #  insufficientInformationThreshold: 180  #new 180 150
+  #  point2NormalMinimalAlignmentAngleThreshold: 80
+  #  point2NormalStrongAlignmentAngleThreshold: 45
   #EqualityConstraints:
   #  highInformationThreshold: 250   ##       RUMLANG GICIK     
   #  enoughInformationThreshold: 180  # was 150 17 sept when was 250 it crasahed. investigate
@@ -81,7 +81,7 @@ degeneracyAwareness:
   #  point2NormalStrongAlignmentAngleThreshold: 45
   #  inequalityboundmultiplier: 8
 
-  #None:
+  None:
 
 transformationCheckers:
   - DifferentialTransformationChecker: