realsense updates

open3d_slam/open3d_slam/include/open3d_slam/croppers.hpp

@@ -41,7 +41,7 @@ class CroppingVolume {
   void crop(PointCloud* cloud) const;
 
  protected:
-  virtual bool isWithinVolumeImpl(const Eigen::Vector3d& p) const;
+  virtual bool isWithinVolumeImpl(const Eigen::Vector3d& p) const = 0;
   Eigen::Isometry3d pose_ = Eigen::Isometry3d::Identity();
   bool isInvertVolume_ = false;
 };

open3d_slam/open3d_slam/include/open3d_slam/helpers.hpp

@@ -22,7 +22,7 @@ std::shared_ptr<open3d::geometry::PointCloud> transform(const Eigen::Matrix4d& T
 std::shared_ptr<open3d::geometry::PointCloud> voxelizeWithinCroppingVolume(double voxel_size, const CroppingVolume& croppingVolume,
                                                                            const open3d::geometry::PointCloud& cloud);
 void randomDownSample(double downSamplingRatio, open3d::geometry::PointCloud* pcl);
-void voxelize(double voxelSize, open3d::geometry::PointCloud* pcl);
+void voxelize(double voxelSize, std::shared_ptr<PointCloud> cloudPtr);
 
 void estimateNormals(int numNearestNeighbours, open3d::geometry::PointCloud* pcl);
 

open3d_slam/open3d_slam/src/Odometry.cpp

@@ -18,64 +18,64 @@
 namespace o3d_slam {
 
 LidarOdometry::LidarOdometry() {
-  cropper_ = std::make_shared<CroppingVolume>();
+  // cropper_ = std::make_shared<CroppingVolume>();
   cloudRegistration_ = cloudRegistrationFactory(params_.scanMatcher_);
 }
 
 PointCloudPtr LidarOdometry::preprocess(const PointCloud& in) const {
-  auto croppedCloud = cropper_->crop(in);
-  o3d_slam::voxelize(params_.scanProcessing_.voxelSize_, croppedCloud.get());
-  cloudRegistration_->estimateNormalsOrCovariancesIfNeeded(croppedCloud.get());
-  return croppedCloud->RandomDownSample(params_.scanProcessing_.downSamplingRatio_);
+  auto croppedCloudPtr = cropper_->crop(in);
+  o3d_slam::voxelize(params_.scanProcessing_.voxelSize_, croppedCloudPtr);
+  cloudRegistration_->estimateNormalsOrCovariancesIfNeeded(croppedCloudPtr.get());
+  return croppedCloudPtr->RandomDownSample(params_.scanProcessing_.downSamplingRatio_);
 }
 
 bool LidarOdometry::addRangeScan(const open3d::geometry::PointCloud& cloud, const Time& timestamp) {
-  if (cloudPrev_.IsEmpty()) {
-    auto preProcessed = preprocess(cloud);
-    cloudPrev_ = *preProcessed;
-    odomToRangeSensorBuffer_.push(timestamp, odomToRangeSensorCumulative_);
-    lastMeasurementTimestamp_ = timestamp;
-    return true;
-  }
-
+  // Check
   if (timestamp < lastMeasurementTimestamp_) {
     std::cerr << "\n\n !!!!! LIDAR ODOMETRY WARNING: Measurements came out of order!!!! \n\n";
     return false;
   }
+  // Implementation
+  std::shared_ptr<PointCloud> preProcessedPtr = preprocess(cloud);
+  if (cloudPrev_.IsEmpty()) {  // First iteration
+    cloudPrev_ = *preProcessedPtr;
+    odomToRangeSensorBuffer_.push(timestamp, odomToRangeSensorCumulative_);
+    lastMeasurementTimestamp_ = timestamp;
+    return true;
+  } else {  // Later iterations
+    const o3d_slam::Timer timer;
+    const auto result = cloudRegistration_->registerClouds(cloudPrev_, *preProcessedPtr, Transform::Identity());
+
+    // todo magic
+    const bool isOdomOkay = result.fitness_ > 0.1;
+    if (!isOdomOkay) {
+      std::cout << "Odometry failed!!!!! \n";
+      std::cout << "Size of the odom buffer: " << odomToRangeSensorBuffer_.size() << std::endl;
+      std::cout << "Scan matching time elapsed: " << timer.elapsedMsec() << " msec \n";
+      std::cout << "Fitness: " << result.fitness_ << "\n";
+      std::cout << "RMSE: " << result.inlier_rmse_ << "\n";
+      std::cout << "Transform: \n" << asString(Transform(result.transformation_)) << "\n";
+      std::cout << "target size: " << cloud.points_.size() << std::endl;
+      std::cout << "reference size: " << cloudPrev_.points_.size() << std::endl;
+      std::cout << "\n \n";
+      if (!preProcessedPtr->IsEmpty()) {
+        cloudPrev_ = *preProcessedPtr;
+      }
+      return isOdomOkay;
+    }
 
-  const o3d_slam::Timer timer;
-  auto preProcessed = preprocess(cloud);
-  const auto result = cloudRegistration_->registerClouds(cloudPrev_, *preProcessed, Transform::Identity());
-
-  // todo magic
-  const bool isOdomOkay = result.fitness_ > 0.1;
-  if (!isOdomOkay) {
-    std::cout << "Odometry failed!!!!! \n";
-    std::cout << "Size of the odom buffer: " << odomToRangeSensorBuffer_.size() << std::endl;
-    std::cout << "Scan matching time elapsed: " << timer.elapsedMsec() << " msec \n";
-    std::cout << "Fitness: " << result.fitness_ << "\n";
-    std::cout << "RMSE: " << result.inlier_rmse_ << "\n";
-    std::cout << "Transform: \n" << asString(Transform(result.transformation_)) << "\n";
-    std::cout << "target size: " << cloud.points_.size() << std::endl;
-    std::cout << "reference size: " << cloudPrev_.points_.size() << std::endl;
-    std::cout << "\n \n";
-    if (!preProcessed->IsEmpty()) {
-      cloudPrev_ = std::move(*preProcessed);
+    if (isInitialTransformSet_) {
+      odomToRangeSensorCumulative_.matrix() = initialTransform_;
+      isInitialTransformSet_ = false;
+    } else {
+      odomToRangeSensorCumulative_.matrix() *= result.transformation_.inverse();
     }
-    return isOdomOkay;
-  }
 
-  if (isInitialTransformSet_) {
-    odomToRangeSensorCumulative_.matrix() = initialTransform_;
-    isInitialTransformSet_ = false;
-  } else {
-    odomToRangeSensorCumulative_.matrix() *= result.transformation_.inverse();
+    cloudPrev_ = *preProcessedPtr;
+    odomToRangeSensorBuffer_.push(timestamp, odomToRangeSensorCumulative_);
+    lastMeasurementTimestamp_ = timestamp;
+    return isOdomOkay;
   }
-
-  cloudPrev_ = std::move(*preProcessed);
-  odomToRangeSensorBuffer_.push(timestamp, odomToRangeSensorCumulative_);
-  lastMeasurementTimestamp_ = timestamp;
-  return isOdomOkay;
 }
 const Transform LidarOdometry::getOdomToRangeSensor(const Time& t) const {
   return getTransform(t, odomToRangeSensorBuffer_);

open3d_slam/open3d_slam/src/ScanToMapRegistration.cpp

@@ -32,9 +32,9 @@ void ScanToMapIcp::update(const MapperParameters& p) {
   cloudRegistration = cloudRegistrationFactory(toCloudRegistrationType(p.scanMatcher_));
 }
 
-PointCloudPtr ScanToMapIcp::preprocess(const PointCloud& in) const {
-  auto croppedCloud = mapBuilderCropper_->crop(in);
-  o3d_slam::voxelize(params_.scanProcessing_.voxelSize_, croppedCloud.get());
+PointCloudPtr ScanToMapIcp::preprocess(const PointCloud& inCloud) const {
+  auto croppedCloud = mapBuilderCropper_->crop(inCloud);
+  o3d_slam::voxelize(params_.scanProcessing_.voxelSize_, croppedCloud);
   cloudRegistration->estimateNormalsOrCovariancesIfNeeded(croppedCloud.get());
   return croppedCloud->RandomDownSample(params_.scanProcessing_.downSamplingRatio_);
 }

open3d_slam/open3d_slam/src/SlamWrapper.cpp

@@ -264,7 +264,6 @@ void SlamWrapper::odometryWorker() {
     odometryStatisticsTimer_.startStopwatch();
     const TimestampedPointCloud measurement = odometryBuffer_.pop();
     auto undistortedCloud = motionCompensationOdom_->undistortInputPointCloud(measurement.cloud_, measurement.time_);
-
     const auto isOdomOkay = odometry_->addRangeScan(*undistortedCloud, measurement.time_);
 
     // this ensures that the odom is always ahead of the mapping

open3d_slam/open3d_slam/src/Submap.cpp

@@ -47,7 +47,8 @@ bool Submap::insertScan(const PointCloud& rawScan, const PointCloud& preProcesse
   if (params_.isUseInitialMap_ && mapCloud_.IsEmpty()) {
     std::lock_guard<std::mutex> lck(mapPointCloudMutex_);
     mapCloud_ = preProcessedScan;
-    voxelize(params_.mapBuilder_.mapVoxelSize_, &mapCloud_);
+    std::shared_ptr<PointCloud> mapCloudPtr(&mapCloud_);
+    voxelize(params_.mapBuilder_.mapVoxelSize_, mapCloudPtr);
     return true;
   }
 

open3d_slam/open3d_slam/src/croppers.cpp

@@ -46,12 +46,8 @@ std::unique_ptr<CroppingVolume> croppingVolumeFactory(CroppingVolumeEnum type, c
   }
 }
 
-bool CroppingVolume::isWithinVolumeImpl(const Eigen::Vector3d& p) const {
-  return true;
-}
-
 bool CroppingVolume::isWithinVolume(const Eigen::Vector3d& p) const {
-  return isInvertVolume_ ? !isWithinVolumeImpl(p) : isWithinVolumeImpl(p);
+  return isInvertVolume_ ? !isWithinVolumeImpl(p) : isWithinVolumeImpl(p);;
 }
 
 void CroppingVolume::setIsInvertVolume(bool val) {
@@ -115,7 +111,6 @@ void CroppingVolume::setScaling(double scaling) {
   // nothing by default
 }
 
-//////////
 MinMaxRadiusCroppingVolume::MinMaxRadiusCroppingVolume(double radiusMin, double radiusMax) : radiusMin_(radiusMin), radiusMax_(radiusMax) {}
 
 bool MinMaxRadiusCroppingVolume::isWithinVolumeImpl(const Eigen::Vector3d& p) const {

open3d_slam/open3d_slam/src/helpers.cpp

@@ -104,12 +104,12 @@ void randomDownSample(double downSamplingRatio, open3d::geometry::PointCloud* pc
   auto downSampled = pcl->RandomDownSample(downSamplingRatio);
   *pcl = std::move(*downSampled);
 }
-void voxelize(double voxelSize, open3d::geometry::PointCloud* pcl) {
+void voxelize(const double voxelSize, std::shared_ptr<PointCloud> cloudPtr) {
   if (voxelSize <= 0) {
     return;
   }
-  auto voxelized = pcl->VoxelDownSample(voxelSize);
-  *pcl = std::move(*voxelized);
+  std::shared_ptr<PointCloud> voxelizedCloudPtr = cloudPtr->VoxelDownSample(voxelSize);
+  *cloudPtr = *voxelizedCloudPtr;
 }
 
 std::shared_ptr<open3d::geometry::PointCloud> voxelizeWithinCroppingVolume(double voxel_size, const CroppingVolume& croppingVolume,

ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp

@@ -21,7 +21,7 @@ class DataProcessorRos {
 
   virtual void initialize() = 0;
   virtual void startProcessing() = 0;
-  virtual void processMeasurement(const PointCloud& cloud, const Time& timestamp);
+  virtual void processMeasurement(const PointCloud& cloud, const Time& timestamp) = 0;
   void accumulateAndProcessRangeData(const PointCloud& cloud, const Time& timestamp);
   void initCommonRosStuff();
   std::shared_ptr<SlamWrapper> getSlamPtr();

ros/open3d_slam_ros/param/default/parameter_structure_definitions.lua

@@ -50,8 +50,8 @@ GLOBAL_OPTIMIZATION_PARAMETERS = {
 }
 
 SCAN_CROPPING_PARAMETERS = {
-  cropping_radius_max= 30.0,
   cropping_radius_min= 2.0,
+  cropping_radius_max= 30.0,
   min_z= -50.0,
   max_z= 50.0,
   cropper_type= "MinMaxRadius", -- options are Cylinder, MaxRadius, MinRadius, MinMaxRadius

ros/open3d_slam_ros/param/param_realsense.lua

@@ -6,8 +6,8 @@ params = deepcopy(DEFAULT_PARAMETERS)
 --ODOMETRY
 params.odometry.scan_processing.voxel_size = 0.02
 params.odometry.scan_processing.downsampling_ratio = 0.6
-params.odometry.scan_processing.scan_cropping.cropping_radius_max = 2.0
 params.odometry.scan_processing.scan_cropping.cropping_radius_min = 0.0
+params.odometry.scan_processing.scan_cropping.cropping_radius_max = 2.0
 params.odometry.scan_matching.icp.max_correspondence_dist = 0.2
 
 --MAPPER_LOCALIZER
@@ -18,8 +18,8 @@ params.mapper_localizer.is_print_timing_information = true
 params.mapper_localizer.is_attempt_loop_closures = false
 params.mapper_localizer.scan_to_map_registration.scan_processing.voxel_size = 0.03
 params.mapper_localizer.scan_to_map_registration.scan_processing.downsampling_ratio = 0.6
-params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_max = 2.0
 params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_min = 0.0
+params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_max = 2.0
 params.mapper_localizer.scan_to_map_registration.icp.max_correspondence_dist = 0.1
 
 --MAP_INITIALIZER
@@ -34,10 +34,10 @@ params.map_initializer.init_pose.yaw = 0.0
 --SUBMAP
 params.submap.submap_size = 3.0 --meters
 
-
 --MAP_BUILDER
 params.map_builder.map_voxel_size = 0.03
-params.map_builder.scan_cropping.cropping_radius_max = 25.0
+params.map_builder.scan_cropping.cropping_radius_min = 0.0
+params.map_builder.scan_cropping.cropping_radius_max = 2.0
 params.map_builder.space_carving.carve_space_every_n_scans = 1000000000
 params.map_builder.space_carving.truncation_distance = 0.45
 
@@ -64,5 +64,4 @@ params.place_recognition.consistency_check.max_drift_z = 40.0 --m
 --SAVING
 params.saving.save_map = false
 
-
 return params

ros/open3d_slam_ros/param/param_realsense_close.lua

@@ -0,0 +1,71 @@
+include "default/default_parameters.lua"
+
+
+params = deepcopy(DEFAULT_PARAMETERS)
+
+--ODOMETRY
+params.odometry.scan_processing.voxel_size = 0.01
+params.odometry.scan_processing.downsampling_ratio = 0.1
+params.odometry.scan_processing.scan_cropping.cropping_radius_min = 0.0
+params.odometry.scan_processing.scan_cropping.cropping_radius_max = 2.0
+params.odometry.scan_matching.icp.max_correspondence_dist = 0.2
+
+--MAPPER_LOCALIZER
+params.mapper_localizer.is_merge_scans_into_map = false
+params.mapper_localizer.is_build_dense_map = true
+params.mapper_localizer.is_use_map_initialization = false
+params.mapper_localizer.is_print_timing_information = true
+params.mapper_localizer.is_attempt_loop_closures = false
+params.mapper_localizer.scan_to_map_registration.scan_processing.voxel_size = 0.01
+params.mapper_localizer.scan_to_map_registration.scan_processing.downsampling_ratio = 0.1
+params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_min = 0.0
+params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_max = 2.0
+params.mapper_localizer.scan_to_map_registration.icp.max_correspondence_dist = 0.1
+
+--MAP_INITIALIZER
+params.map_initializer.pcd_file_path = ""
+params.map_initializer.init_pose.x = 0.0
+params.map_initializer.init_pose.y = 0.0
+params.map_initializer.init_pose.z = 0.0
+params.map_initializer.init_pose.roll = 0.0
+params.map_initializer.init_pose.pitch = 0.0
+params.map_initializer.init_pose.yaw = 0.0
+
+--SUBMAP
+params.submap.submap_size = 3.0 --meters
+
+--MAP_BUILDER
+params.map_builder.map_voxel_size = 0.01
+params.map_builder.scan_cropping.cropping_radius_min = 0.0
+params.map_builder.scan_cropping.cropping_radius_max = 2.0
+params.map_builder.space_carving.carve_space_every_n_scans = 1000000000
+params.map_builder.space_carving.truncation_distance = 0.45
+
+--DENSE_MAP_BUILDER
+params.dense_map_builder.map_voxel_size = 0.001
+params.dense_map_builder.scan_cropping.cropping_radius_min = 0.0
+params.dense_map_builder.scan_cropping.cropping_radius_max = 2.0
+params.dense_map_builder.space_carving.carve_space_every_n_scans = 1000000000
+params.dense_map_builder.space_carving.truncation_distance = 0.1
+
+--PLACE_RECOGNITION
+params.place_recognition.ransac_min_corresondence_set_size = 40
+params.place_recognition.max_icp_correspondence_distance = 0.3
+params.place_recognition.min_icp_refinement_fitness = 0.7
+params.place_recognition.dump_aligned_place_recognitions_to_file = false 
+params.place_recognition.min_submaps_between_loop_closures = 2
+params.place_recognition.loop_closure_search_radius = 20.0
+params.place_recognition.consistency_check.max_drift_roll = 30.0 --deg
+params.place_recognition.consistency_check.max_drift_pitch = 30.0 --deg
+params.place_recognition.consistency_check.max_drift_yaw = 30.0 --deg
+params.place_recognition.consistency_check.max_drift_x = 80.0 --m
+params.place_recognition.consistency_check.max_drift_y = 80.0 --m
+params.place_recognition.consistency_check.max_drift_z = 40.0 --m
+
+-- Visualization
+params.visualization.assembled_map_voxel_size = 0.01
+
+--SAVING
+params.saving.save_map = false
+
+return params

ros/open3d_slam_ros/param/param_robosense_rs16.lua

@@ -8,7 +8,6 @@ params.odometry.scan_processing.voxel_size = 0.05
 params.odometry.scan_processing.downsampling_ratio = 1.0
 params.odometry.scan_processing.scan_cropping.cropping_radius_max = 40.0
 
-
 --MAPPER_LOCALIZER
 params.mapper_localizer.is_merge_scans_into_map = false
 params.mapper_localizer.is_build_dense_map = false
@@ -31,7 +30,6 @@ params.map_initializer.init_pose.yaw = 0.0
 --SUBMAP
 params.submap.submap_size = 20.0 --meters
 
-
 --MAP_BUILDER
 params.map_builder.map_voxel_size = 0.1
 params.map_builder.scan_cropping.cropping_radius_max = 40.0
@@ -63,5 +61,4 @@ params.motion_compensation.is_undistort_scan = true
 --SAVING
 params.saving.save_map = false
 
-
 return params