diff --git a/libpointmatcher/pointmatcher/ICP.cpp b/libpointmatcher/pointmatcher/ICP.cpp index b242df4..0995b89 100755 --- a/libpointmatcher/pointmatcher/ICP.cpp +++ b/libpointmatcher/pointmatcher/ICP.cpp @@ -1306,7 +1306,7 @@ typename PointMatcher::TransformationParameters PointMatcher::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; } diff --git a/libpointmatcher/pointmatcher/PointMatcher.h b/libpointmatcher/pointmatcher/PointMatcher.h index 4087b12..5d31313 100755 --- a/libpointmatcher/pointmatcher/PointMatcher.h +++ b/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; } diff --git a/open3d_slam_rsl/open3d_slam/open3d_slam/include/open3d_slam/Mapper.hpp b/open3d_slam_rsl/open3d_slam/open3d_slam/include/open3d_slam/Mapper.hpp index 9af16c0..8428906 100644 --- a/open3d_slam_rsl/open3d_slam/open3d_slam/include/open3d_slam/Mapper.hpp +++ b/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& croppedCloud, + const pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess); + + pointmatcher_ros::PmTfParameters scan2mapRegistrationLargeBasin( + const std::shared_ptr& 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 scan2MapReg_; diff --git a/open3d_slam_rsl/open3d_slam/open3d_slam/src/Mapper.cpp b/open3d_slam_rsl/open3d_slam/open3d_slam/src/Mapper.cpp index a95beff..1f3eb02 100644 --- a/open3d_slam_rsl/open3d_slam/open3d_slam/src/Mapper.cpp +++ b/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 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(); + 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(); + } else { + // correctedTransform_o3d.matrix() = transformReadingToReferenceInitialGuess.matrix().cast(); + 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& 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& croppedCloud, + const pointmatcher_ros::PmTfParameters& transformReadingToReferenceInitialGuess) { + pointmatcher_ros::PmTfParameters correctedTransform; + + { + std::lock_guard 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 */ diff --git a/open3d_slam_rsl/open3d_slam/open3d_slam/src/Odometry.cpp b/open3d_slam_rsl/open3d_slam/open3d_slam/src/Odometry.cpp index 1347884..ac48070 100644 --- a/open3d_slam_rsl/open3d_slam/open3d_slam/src/Odometry.cpp +++ b/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; } diff --git a/open3d_slam_rsl/ros/open3d_slam_ros/include/open3d_slam_ros/SlamWrapperRos.hpp b/open3d_slam_rsl/ros/open3d_slam_ros/include/open3d_slam_ros/SlamWrapperRos.hpp index 8e93f5c..510eb30 100644 --- a/open3d_slam_rsl/ros/open3d_slam_ros/include/open3d_slam_ros/SlamWrapperRos.hpp +++ b/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 tfBroadcaster_; + std::shared_ptr tfStaticBroadcaster_; ros::Publisher odometryInputPub_, mappingInputPub_, submapOriginsPub_, assembledMapPub_, denseMapPub_, submapsPub_, bestGuessPathPub_, trackedPathPub_; ros::Publisher differenceLinePub_; diff --git a/open3d_slam_rsl/ros/open3d_slam_ros/param/icp.yaml b/open3d_slam_rsl/ros/open3d_slam_ros/param/icp.yaml index 0225035..957a9b9 100755 --- a/open3d_slam_rsl/ros/open3d_slam_ros/param/icp.yaml +++ b/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: diff --git a/open3d_slam_rsl/ros/open3d_slam_ros/param/param_velodyne_puck16.lua b/open3d_slam_rsl/ros/open3d_slam_ros/param/param_velodyne_puck16.lua index f6aadbf..4edbf49 100644 --- a/open3d_slam_rsl/ros/open3d_slam_ros/param/param_velodyne_puck16.lua +++ b/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 diff --git a/open3d_slam_rsl/ros/open3d_slam_ros/src/SlamMapInitializer.cpp b/open3d_slam_rsl/ros/open3d_slam_ros/src/SlamMapInitializer.cpp index c926b39..de8e794 100644 --- a/open3d_slam_rsl/ros/open3d_slam_ros/src/SlamMapInitializer.cpp +++ b/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) { diff --git a/open3d_slam_rsl/ros/open3d_slam_ros/src/SlamWrapperRos.cpp b/open3d_slam_rsl/ros/open3d_slam_ros/src/SlamWrapperRos.cpp index 098271e..073916f 100644 --- a/open3d_slam_rsl/ros/open3d_slam_ros/src/SlamWrapperRos.cpp +++ b/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(); });