diff --git a/open3d_slam/open3d_slam/include/open3d_slam/croppers.hpp b/open3d_slam/open3d_slam/include/open3d_slam/croppers.hpp index d0b7e6b..226c511 100644 --- a/open3d_slam/open3d_slam/include/open3d_slam/croppers.hpp +++ b/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; }; diff --git a/open3d_slam/open3d_slam/include/open3d_slam/helpers.hpp b/open3d_slam/open3d_slam/include/open3d_slam/helpers.hpp index 2f87dfb..a36784d 100644 --- a/open3d_slam/open3d_slam/include/open3d_slam/helpers.hpp +++ b/open3d_slam/open3d_slam/include/open3d_slam/helpers.hpp @@ -22,7 +22,7 @@ std::shared_ptr transform(const Eigen::Matrix4d& T std::shared_ptr 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 cloudPtr); void estimateNormals(int numNearestNeighbours, open3d::geometry::PointCloud* pcl); diff --git a/open3d_slam/open3d_slam/src/Odometry.cpp b/open3d_slam/open3d_slam/src/Odometry.cpp index b4344a4..676bf46 100644 --- a/open3d_slam/open3d_slam/src/Odometry.cpp +++ b/open3d_slam/open3d_slam/src/Odometry.cpp @@ -18,64 +18,64 @@ namespace o3d_slam { LidarOdometry::LidarOdometry() { - cropper_ = std::make_shared(); + // cropper_ = std::make_shared(); 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 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_); diff --git a/open3d_slam/open3d_slam/src/ScanToMapRegistration.cpp b/open3d_slam/open3d_slam/src/ScanToMapRegistration.cpp index 2508397..deb5424 100644 --- a/open3d_slam/open3d_slam/src/ScanToMapRegistration.cpp +++ b/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_); } diff --git a/open3d_slam/open3d_slam/src/SlamWrapper.cpp b/open3d_slam/open3d_slam/src/SlamWrapper.cpp index d2843e1..a535ed5 100644 --- a/open3d_slam/open3d_slam/src/SlamWrapper.cpp +++ b/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 diff --git a/open3d_slam/open3d_slam/src/Submap.cpp b/open3d_slam/open3d_slam/src/Submap.cpp index d01a459..a789d50 100644 --- a/open3d_slam/open3d_slam/src/Submap.cpp +++ b/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 lck(mapPointCloudMutex_); mapCloud_ = preProcessedScan; - voxelize(params_.mapBuilder_.mapVoxelSize_, &mapCloud_); + std::shared_ptr mapCloudPtr(&mapCloud_); + voxelize(params_.mapBuilder_.mapVoxelSize_, mapCloudPtr); return true; } diff --git a/open3d_slam/open3d_slam/src/croppers.cpp b/open3d_slam/open3d_slam/src/croppers.cpp index 6781627..4f7a89c 100644 --- a/open3d_slam/open3d_slam/src/croppers.cpp +++ b/open3d_slam/open3d_slam/src/croppers.cpp @@ -46,12 +46,8 @@ std::unique_ptr 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 { diff --git a/open3d_slam/open3d_slam/src/helpers.cpp b/open3d_slam/open3d_slam/src/helpers.cpp index aa19e64..82ce20f 100644 --- a/open3d_slam/open3d_slam/src/helpers.cpp +++ b/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 cloudPtr) { if (voxelSize <= 0) { return; } - auto voxelized = pcl->VoxelDownSample(voxelSize); - *pcl = std::move(*voxelized); + std::shared_ptr voxelizedCloudPtr = cloudPtr->VoxelDownSample(voxelSize); + *cloudPtr = *voxelizedCloudPtr; } std::shared_ptr voxelizeWithinCroppingVolume(double voxel_size, const CroppingVolume& croppingVolume, diff --git a/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp b/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp index 01f44fb..8a63415 100644 --- a/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp +++ b/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 getSlamPtr(); diff --git a/ros/open3d_slam_ros/param/default/parameter_structure_definitions.lua b/ros/open3d_slam_ros/param/default/parameter_structure_definitions.lua index 854ec17..e8ab9f7 100644 --- a/ros/open3d_slam_ros/param/default/parameter_structure_definitions.lua +++ b/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 diff --git a/ros/open3d_slam_ros/param/param_realsense.lua b/ros/open3d_slam_ros/param/param_realsense.lua index d47990e..e14d5fe 100644 --- a/ros/open3d_slam_ros/param/param_realsense.lua +++ b/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 \ No newline at end of file diff --git a/ros/open3d_slam_ros/param/param_realsense_close.lua b/ros/open3d_slam_ros/param/param_realsense_close.lua new file mode 100644 index 0000000..9fdceff --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ros/open3d_slam_ros/param/param_robosense_rs16.lua b/ros/open3d_slam_ros/param/param_robosense_rs16.lua index 6f72a6a..14bc434 100644 --- a/ros/open3d_slam_ros/param/param_robosense_rs16.lua +++ b/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 \ No newline at end of file diff --git a/ros/open3d_slam_ros/rviz/default_realsense.rviz b/ros/open3d_slam_ros/rviz/default_realsense.rviz new file mode 100644 index 0000000..8a1e0f0 --- /dev/null +++ b/ros/open3d_slam_ros/rviz/default_realsense.rviz @@ -0,0 +1,399 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + Splitter Ratio: 0.6278260946273804 + Tree Height: 2320 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: RawCloud + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + camera/left_camera: + Value: false + camera0_accel_frame: + Value: false + camera0_accel_optical_frame: + Value: false + camera0_aligned_depth_to_color_frame: + Value: false + camera0_color_frame: + Value: false + camera0_color_optical_frame: + Value: false + camera0_depth_frame: + Value: false + camera0_depth_optical_frame: + Value: false + camera0_gyro_frame: + Value: false + camera0_gyro_optical_frame: + Value: false + camera0_link: + Value: false + camera2_color_frame: + Value: false + camera2_color_optical_frame: + Value: false + camera2_link: + Value: false + camera_confidence_frame: + Value: false + camera_confidence_optical_frame: + Value: false + camera_infra_frame: + Value: false + camera_infra_optical_frame: + Value: false + camera_left: + Value: false + map: + Value: false + map_o3d: + Value: true + odom_o3d: + Value: false + range_sensor_o3d: + Value: true + raw_odom_o3d: + Value: false + raw_rs_o3d: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + camera0_link: + camera0_accel_frame: + camera0_accel_optical_frame: + {} + camera0_aligned_depth_to_color_frame: + camera0_color_optical_frame: + {} + camera0_color_frame: + {} + camera0_depth_frame: + camera0_depth_optical_frame: + {} + camera0_gyro_frame: + camera0_gyro_optical_frame: + {} + camera_confidence_frame: + camera_confidence_optical_frame: + {} + camera_infra_frame: + camera_infra_optical_frame: + {} + camera2_link: + camera2_color_frame: + camera2_color_optical_frame: + {} + map: + camera_left: + camera/left_camera: + {} + map_o3d: + odom_o3d: + range_sensor_o3d: + {} + raw_odom_o3d: + {} + raw_rs_o3d: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.160181045532227 + Min Value: -1.9088754653930664 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: RawCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /mapping_node/odom_input + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 7.070262908935547 + Min Value: -0.9526716470718384 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 32; 74; 135 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: MappingInputCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /mapping_node/mapping_input + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.4691959023475647 + Min Value: -0.003396666143089533 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: AssembledMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.20000000298023224 + Style: Points + Topic: /mapping_node/assembled_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.4000000059604645 + Min Value: 0 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: DenseMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.20000000298023224 + Style: Points + Topic: /mapping_node/dense_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 8.856701850891113 + Min Value: -1.5836607217788696 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: SubmapsCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.10000000149011612 + Style: Points + Topic: /mapping_node/submaps + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /mapping_node/submap_origins + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: jsk_rviz_plugin/TFTrajectory + Enabled: false + Name: TFTrajectoryOdom + Value: false + color: 173; 127; 168 + duration: 500 + frame: raw_odom_o3d + line_width: 0.15000000596046448 + - Class: jsk_rviz_plugin/TFTrajectory + Enabled: false + Name: TFTrajectoryMap + Value: false + color: 239; 41; 41 + duration: 500 + frame: raw_rs_o3d + line_width: 0.15000000596046448 + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Update Topic: /initialization_pose/update + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map_o3d + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.8306710720062256 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -0.006322930566966534 + Y: 0.0015961230965331197 + Z: 0.2764285206794739 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.3597966432571411 + Target Frame: + Yaw: 4.868602275848389 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2832 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000024100000a08fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e00000a080000018200fffffffb0000000a005600690065007700730000000936000001400000013200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000010c0000028b0000000000000000000000010000015f00000a08fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e00000a080000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000140000000060fc0100000002fb0000000800540069006d0065010000000000001400000006dc00fffffffb0000000800540069006d006501000000000000045000000000000000000000104800000a0800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 5120 + X: 0 + Y: 0 diff --git a/ros/open3d_slam_ros/src/DataProcessorRos.cpp b/ros/open3d_slam_ros/src/DataProcessorRos.cpp index e4b9fc8..8f4b360 100644 --- a/ros/open3d_slam_ros/src/DataProcessorRos.cpp +++ b/ros/open3d_slam_ros/src/DataProcessorRos.cpp @@ -23,10 +23,6 @@ void DataProcessorRos::initCommonRosStuff() { std::cout << "Num accumulated range data: " << numAccumulatedRangeDataDesired_ << std::endl; } -void DataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp) { - std::cout << "Warning you have not implemented processMeasurement!!! \n"; -} - std::shared_ptr DataProcessorRos::getSlamPtr() { return slam_; } @@ -48,7 +44,7 @@ void DataProcessorRos::accumulateAndProcessRangeData(const PointCloud& cloud, co } if (accumulatedCloud_.IsEmpty()) { - std::cout << "Trying to insert and empyt cloud!!! Skipping the measurement \n"; + std::cout << "Trying to insert and empty cloud!!! Skipping the measurement \n"; return; } diff --git a/ros/open3d_slam_ros/src/SlamWrapperRos.cpp b/ros/open3d_slam_ros/src/SlamWrapperRos.cpp index 89cfef3..eca53ca 100644 --- a/ros/open3d_slam_ros/src/SlamWrapperRos.cpp +++ b/ros/open3d_slam_ros/src/SlamWrapperRos.cpp @@ -226,17 +226,17 @@ void SlamWrapperRos::publishMaps(const Time& time) { const ros::Time timestamp = toRos(time); { - PointCloud map = mapper_->getAssembledMapPointCloud(); - voxelize(params_.visualization_.assembledMapVoxelSize_, &map); - o3d_slam::publishCloud(map, o3d_slam::frames::mapFrame, timestamp, assembledMapPub_); + std::shared_ptr mapPtr = std::make_shared(mapper_->getAssembledMapPointCloud()); + voxelize(params_.visualization_.assembledMapVoxelSize_, mapPtr); + o3d_slam::publishCloud(*mapPtr, o3d_slam::frames::mapFrame, timestamp, assembledMapPub_); } o3d_slam::publishCloud(mapper_->getPreprocessedScan(), o3d_slam::frames::rangeSensorFrame, timestamp, mappingInputPub_); o3d_slam::publishSubmapCoordinateAxes(mapper_->getSubmaps(), o3d_slam::frames::mapFrame, timestamp, submapOriginsPub_); if (submapsPub_.getNumSubscribers() > 0) { - open3d::geometry::PointCloud cloud; - o3d_slam::assembleColoredPointCloud(mapper_->getSubmaps(), &cloud); - voxelize(params_.visualization_.submapVoxelSize_, &cloud); - o3d_slam::publishCloud(cloud, o3d_slam::frames::mapFrame, timestamp, submapsPub_); + std::shared_ptr cloudPtr; + o3d_slam::assembleColoredPointCloud(mapper_->getSubmaps(), cloudPtr.get()); + voxelize(params_.visualization_.submapVoxelSize_, cloudPtr); + o3d_slam::publishCloud(*cloudPtr, o3d_slam::frames::mapFrame, timestamp, submapsPub_); } visualizationUpdateTimer_.reset();