[PROBABLY DON'T MERGE] dev/alma_multi_robot

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,
+      const 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,30 +365,39 @@ 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;
     }
 
+    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 ";
+    }
+
     const double timeElapsed = testmapperOnlyTimer_.elapsedMsecSinceStopwatchStart();
     testmapperOnlyTimer_.addMeasurementMsec(timeElapsed);
 
     if (params_.isPrintTimingStatistics_) {
       std::cout << " Scan2Map Registration: "
                 << "\033[92m" << timeElapsed << " msec \n "
-                << "\033[0m";
+                << "\033[0m"
+                << " \n ";
     }
 
     //}else{
@@ -411,13 +421,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_)) {
     if (isTimeValid(timestamp)) {
       initTime_ = timestamp;
     }
@@ -426,8 +443,12 @@ 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";
-    mapToRangeSensorPrev_ = mapToRangeSensor_;
+
+    mapToRangeSensorEstimate = mapToRangeSensor_;
+    // mapToRangeSensorPrev_ = mapToRangeSensor_;
+    mapToRangeSensorBuffer_.clear();
     mapToRangeSensorBuffer_.push(timestamp, mapToRangeSensor_);
+    bestGuessBuffer_.clear();
     bestGuessBuffer_.push(timestamp, mapToRangeSensorEstimate);
     isNewValueSetMapper_ = false;
     isIgnoreOdometryPrediction_ = true;
@@ -532,4 +553,97 @@ void Mapper::checkTransformChainingAndPrintResult(bool isCheckTransformChainingA
   }
 }
 
+pointmatcher_ros::PmTfParameters Mapper::scan2mapRegistrationLargeBasin(
+    const std::shared_ptr<open3d_conversions::PmStampedPointCloud>& croppedCloud,
+    const pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess) {
+  pointmatcher_ros::PmTfParameters newTransform = transformReadingToReferenceInitialGuess;
+  if (attemptedLargeBasin_) {
+    return newTransform;
+  }
+
+  attemptedLargeBasin_ = true;
+
+  for (float angle = 0; std::abs(angle) < 20; angle = angle + 1) {
+    // 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;
+
+    pointmatcher_ros::PmTfParameters augmentedGuess = transformReadingToReferenceInitialGuess.matrix() * homMatrix;
+    newTransform = scan2mapRegistrationWrapper(croppedCloud, augmentedGuess);
+    if (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) {
+      // The used angle is console output
+      std::cout << "Successfull Delta angle: " << angle << std::endl;
+
+      // the fitness it
+      std::cout << "Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+
+      break;
+    }
+
+    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;
+    pointmatcher_ros::PmTfParameters negaugmentedGuess = transformReadingToReferenceInitialGuess.matrix() * neghomMatrix;
+    newTransform = scan2mapRegistrationWrapper(croppedCloud, negaugmentedGuess);
+
+    if (registrationFitness_ < params_.scanMatcher_.minRefinementFitness_) {
+      // The used angle is console output
+      std::cout << "Successfull Delta Angle: " << -angle << std::endl;
+
+      // the fitness it
+      std::cout << "Registration Fitness (Lower better): " << registrationFitness_ << std::endl;
+
+      break;
+    }
+
+    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/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:

open3d_slam_rsl/ros/open3d_slam_ros/param/param_velodyne_puck16.lua

@@ -12,9 +12,9 @@ params.odometry.use_odometry_topic_instead_of_scan_to_scan = true --Uses Odometr
 params.odometry.use_IMU_for_attitude_initialization = false --Uses IMU msgs to initialize gravity aligned attitude.
 
 --MAPPER_LOCALIZER
-params.mapper_localizer.is_use_map_initialization = false
+params.mapper_localizer.is_use_map_initialization = true
 params.mapper_localizer.republish_the_preloaded_map = true
-params.mapper_localizer.is_merge_scans_into_map = true
+params.mapper_localizer.is_merge_scans_into_map = false
 params.mapper_localizer.is_build_dense_map = false
 params.mapper_localizer.is_attempt_loop_closures = true
 params.mapper_localizer.is_print_timing_information = false
@@ -28,11 +28,12 @@ params.mapper_localizer.scan_to_map_registration.icp.max_correspondence_dist = 2
 params.mapper_localizer.scan_to_map_registration.icp.knn = 10 --Currently only used for surface normal estimation.
 params.mapper_localizer.scan_to_map_registration.icp.max_distance_knn = 1.0 --Currently only used for surface normal estimation.
 params.mapper_localizer.scan_to_map_registration.icp.reference_cloud_seting_period = 1.5 --sec
+params.mapper_localizer.scan_to_map_registration.min_refinement_fitness = 0.1 --helps to prevent bad registrations.
 
 --MAP_INITIALIZER
 params.map_initializer.pcd_file_package = "open3d_slam_ros"
-params.map_initializer.pcd_file_path = "/data/ETH_LEE_H_with_terrace.pcd"
-params.map_initializer.is_initialize_interactively = false
+params.map_initializer.pcd_file_path = "/data/5cm_localizationMap.ply"
+params.map_initializer.is_initialize_interactively = true
 params.map_initializer.init_pose.x = 0.0
 params.map_initializer.init_pose.y = 2.0
 params.map_initializer.init_pose.z = 0.0

open3d_slam_rsl/ros/open3d_slam_ros/src/SlamMapInitializer.cpp

@@ -38,7 +38,7 @@ SlamMapInitializer::~SlamMapInitializer() {
 void SlamMapInitializer::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& msg) {
   Eigen::Isometry3d init_transform;
   tf::poseMsgToEigen(msg.pose.pose, init_transform);
-  std::cout << "Initial Pose \n" << asString(init_transform) << std::endl;
+  // std::cout << "Initial Pose \n" << asString(init_transform) << std::endl;
   slamPtr_->setInitialTransform(init_transform.matrix());
 }
 bool SlamMapInitializer::initSlamCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {

open3d_slam_rsl/ros/open3d_slam_ros/src/SlamWrapperRos.cpp

@@ -39,6 +39,7 @@ namespace {
 
 SlamWrapperRos::SlamWrapperRos(ros::NodeHandlePtr nh) : BASE(), nh_(nh) {
   tfBroadcaster_.reset(new tf2_ros::TransformBroadcaster());
+  tfStaticBroadcaster_.reset(new tf2_ros::StaticTransformBroadcaster());
 
   prevPublishedTimeScanToScan_ = fromUniversal(0);
   prevPublishedTimeScanToMap_ = fromUniversal(0);
@@ -60,7 +61,19 @@ SlamWrapperRos::~SlamWrapperRos() {
 }
 
 void SlamWrapperRos::startWorkers() {
-  tfWorker_ = std::thread([this]() { tfWorker(); });
+  // tfWorker_ = std::thread([this]() { tfWorker(); });
+  geometry_msgs::TransformStamped tfMapO3dToIntegrated;
+  tfMapO3dToIntegrated.header.stamp = ros::Time::now();
+  tfMapO3dToIntegrated.header.frame_id = "map_o3d_integrated";
+  tfMapO3dToIntegrated.child_frame_id = "map_o3d";
+  tfMapO3dToIntegrated.transform.translation.x = 0.0;
+  tfMapO3dToIntegrated.transform.translation.y = 0.0;
+  tfMapO3dToIntegrated.transform.translation.z = 0.0;
+  tfMapO3dToIntegrated.transform.rotation.x = 0.0;
+  tfMapO3dToIntegrated.transform.rotation.y = 0.0;
+  tfMapO3dToIntegrated.transform.rotation.z = 0.0;
+  tfMapO3dToIntegrated.transform.rotation.w = 1.0;
+  tfStaticBroadcaster_->sendTransform(tfMapO3dToIntegrated);
   visualizationWorker_ = std::thread([this]() { visualizationWorker(); });
   if (params_.odometry_.isPublishOdometryMsgs_) {
     odomPublisherWorker_ = std::thread([this]() { odomPublisherWorker(); });