From 3282b7b9461eab7d224543b0f8930687114377b5 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 15 Oct 2019 18:09:02 -0400
Subject: [PATCH] CoreWrapper: updated default parameters when subscribing to
 3d scan

---
 src/CoreWrapper.cpp | 57 ++++++++++++++++++++++++++++++++++-----------
 1 file changed, 44 insertions(+), 13 deletions(-)

diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 5bf12d8e..e812e1e9 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -415,21 +415,41 @@ void CoreWrapper::onInit()
 				Parameters::defaultGridRangeMax());
 		parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
 	}
+	if(subscribeScan3d && parameters_.find(Parameters::kIcpPointToPlaneRadius()) == parameters_.end())
+	{
+		NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan_cloud\" is true.",
+				Parameters::kIcpPointToPlaneRadius().c_str(),
+				Parameters::defaultIcpPointToPlaneRadius());
+		parameters_.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), "0"));
+	}
 	int regStrategy = Parameters::defaultRegStrategy();
 	Parameters::parse(parameters_, Parameters::kRegStrategy(), regStrategy);
-	if(subscribeScan2d &&
-		parameters_.find(Parameters::kRGBDProximityPathMaxNeighbors()) == parameters_.end() &&
+	if(parameters_.find(Parameters::kRGBDProximityPathMaxNeighbors()) == parameters_.end() &&
 		(regStrategy == Registration::kTypeIcp || regStrategy == Registration::kTypeVisIcp))
 	{
-		NODELET_WARN("Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
-				"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
-				"scans. To disable, set \"%s\" to 0. To suppress this warning, "
-				"add <param name=\"%s\" type=\"string\" value=\"10\"/>",
-				Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
-				Parameters::kRegStrategy().c_str(),
-				Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
-				Parameters::kRGBDProximityPathMaxNeighbors().c_str());
-		parameters_.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "10"));
+		if(subscribeScan2d)
+		{
+			NODELET_WARN("Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
+					"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
+					"scans. To disable, set \"%s\" to 0. To suppress this warning, "
+					"add <param name=\"%s\" type=\"string\" value=\"10\"/>",
+					Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+					Parameters::kRegStrategy().c_str(),
+					Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+					Parameters::kRGBDProximityPathMaxNeighbors().c_str());
+			parameters_.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "10"));
+		}
+		else if(subscribeScan3d)
+		{
+			NODELET_WARN("Setting \"%s\" parameter to 1 (default 0) as \"subscribe_scan_cloud\" is "
+					"true and \"%s\" uses ICP. To disable, set \"%s\" to 0. To suppress this warning, "
+					"add <param name=\"%s\" type=\"string\" value=\"1\"/>",
+					Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+					Parameters::kRegStrategy().c_str(),
+					Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+					Parameters::kRGBDProximityPathMaxNeighbors().c_str());
+			parameters_.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1"));
+		}
 	}
 
 	int estimationType = Parameters::defaultVisEstimationType();
@@ -641,9 +661,9 @@ void CoreWrapper::onInit()
 			NODELET_WARN("Setting %s=1 (ICP)", Parameters::kRegStrategy().c_str());
 			updateParams = true;
 
-			if(this->isSubscribedToScan2d())
+			if(modifiedParameters.find(Parameters::kRGBDProximityPathMaxNeighbors()) == modifiedParameters.end())
 			{
-				if(modifiedParameters.find(Parameters::kRGBDProximityPathMaxNeighbors()) == modifiedParameters.end())
+				if(this->isSubscribedToScan2d())
 				{
 					NODELET_WARN("Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
 							"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
@@ -655,6 +675,17 @@ void CoreWrapper::onInit()
 							Parameters::kRGBDProximityPathMaxNeighbors().c_str());
 					uInsert(parameters_, ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "10"));
 				}
+				else if(this->isSubscribedToScan3d())
+				{
+					NODELET_WARN("Setting \"%s\" parameter to 1 (default 0) as \"subscribe_scan_cloud\" is "
+							"true and \"%s\" uses ICP. To disable, set \"%s\" to 0. To suppress this warning, "
+							"add <param name=\"%s\" type=\"string\" value=\"1\"/>",
+							Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+							Parameters::kRegStrategy().c_str(),
+							Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+							Parameters::kRGBDProximityPathMaxNeighbors().c_str());
+					uInsert(parameters_, ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1"));
+				}
 			}
 		}
 		if(updateParams)