adapt for TReX

ethzasl_icp_mapper/launch/trex/dyn_map_post_filters.yaml

@@ -0,0 +1,9 @@
+#NOTE: reducing density lower than the node is computing poses some problems
+- SurfaceNormalDataPointsFilter:
+    knn: 20
+    epsilon: 3.16
+    keepNormals: 1
+    keepDensities: 1
+- MaxPointCountDataPointsFilter:
+    maxCount: 600000
+    prob: 0.7

ethzasl_icp_mapper/launch/trex/icp.yaml

@@ -0,0 +1,43 @@
+readingDataPointsFilters:
+  - VoxelGridDataPointsFilter:
+      vSizeX: 0.2
+      vSizeY: 0.2
+      vSizeZ: 0.2
+
+matcher:
+  KDTreeMatcher:
+    maxDist: 4.0
+    knn: 1
+    epsilon: 1.0
+
+outlierFilters:
+  - TrimmedDistOutlierFilter:
+     ratio: 0.95
+  - SurfaceNormalOutlierFilter:
+     maxAngle: 0.42
+#  - GenericDescriptorOutlierFilter:
+#     source: reference
+#     descName: probabilityStatic
+#     useSoftThreshold: 0
+#     threshold: 0.40
+
+errorMinimizer:
+  PointToPlaneErrorMinimizer
+
+transformationCheckers:
+  - DifferentialTransformationChecker:
+      minDiffRotErr: 0.001
+      minDiffTransErr: 0.01
+      smoothLength: 4
+  - CounterTransformationChecker:
+      maxIterationCount: 40
+  - BoundTransformationChecker:
+      maxRotationNorm: 0.80
+      maxTranslationNorm: 15.00
+
+inspector:
+# VTKFileInspector
+ NullInspector
+
+logger:
+  FileLogger

ethzasl_icp_mapper/launch/trex/input_filters.yaml

@@ -0,0 +1,13 @@
+- VoxelGridDataPointsFilter:
+    vSizeX: 0.05
+    vSizeY: 0.05
+    vSizeZ: 0.05
+- SimpleSensorNoiseDataPointsFilter
+- SurfaceNormalDataPointsFilter:
+    knn: 20
+    keepNormals: 1
+    keepDensities: 1
+#- MaxDensityDataPointsFilter:
+#    maxDensity: 50000
+#- ObservationDirectionDataPointsFilter
+- OrientNormalsDataPointsFilter 

ethzasl_icp_mapper/launch/trex/map_post_filters.yaml

@@ -0,0 +1,10 @@
+- SurfaceNormalDataPointsFilter:
+    knn: 20
+    epsilon: 3.16
+    keepNormals: 1
+    keepDensities: 1
+- MaxDensityDataPointsFilter:
+    maxDensity: 10
+- MaxPointCountDataPointsFilter:
+    maxCount: 600000
+    prob: 0.7

ethzasl_icp_mapper/launch/trex/trex_dynamic_mapper.launch

@@ -0,0 +1,32 @@
+<launch>
+	<node name="mapper" type="dynamic_mapper" pkg="ethzasl_icp_mapper" output="screen">
+		<remap from="cloud_in" to="/cloud" />
+		<param name="subscribe_scan" value="false" />
+		<param name="icpConfig" value="$(find ethzasl_icp_mapper)/launch/trex/icp.yaml" />
+		<param name="inputFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/trex/input_filters.yaml" />
+		<param name="mapPostFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/trex/dyn_map_post_filters.yaml" />
+		<param name="odom_frame" value="/odom" />
+		<param name="map_frame" value="/map" />
+		<param name="useROSLogger" value="true" />
+		<param name="minOverlap" value="0.1" /> 
+		<param name="maxOverlapToMerge" value="0.99" /> 
+		<param name="minReadingPointCount" value="1000" /> 
+		<param name="minMapPointCount" value="1000" /> 
+		<param name="localizing" value="true" /> 
+		<param name="mapping" value="true" /> 
+
+    <!-- Parameters for dynamic elements -->
+		<param name="priorStatic" value="0.45"/>
+		<param name="priorDyn" value="0.55"/>
+		<param name="maxAngle" value="0.1"/>
+		<param name="eps_a" value="0.020"/><!--1 deg = 0.017rad-->
+		<param name="eps_d" value="0.1"/>
+		<param name="alpha" value="0.99"/>
+		<param name="beta" value="0.90"/>
+		<param name="maxDyn" value="0.90"/>
+		<param name="maxDistNewPoint" value="0.15"/>
+		<param name="sensorMaxRange" value="40.0"/>
+
+
+	</node>
+</launch>

ethzasl_icp_mapper/launch/trex/trex_map_intereact.launch

@@ -0,0 +1,8 @@
+<launch>
+	<node name="interact_mapper" type="interact_mapper" pkg="ethzasl_icp_mapper" output="screen">
+		<param name="map_path" value="/home/frank/.ros/3D_maps" /> 
+		<param name="base_frame" value="/vo_base" /> 
+		<param name="odom_frame" value="/vo_odom" /> 
+		<param name="map_frame" value="/map" /> 
+	</node>
+</launch>

ethzasl_icp_mapper/src/dynamic_mapper.cpp

@@ -566,6 +566,9 @@ void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scann
 		// Apply ICP
 		PM::TransformationParameters Ticp;
 		Ticp = icp(*newPointCloud, TscannerToMap);
+    //TODO TEST finish here....
+    //Ticp = PM::TransformationParameters::Identity(4,4); 
+    //END TEST
     Ticp = transformation->correctParameters(Ticp);
 
     // extract corrections
@@ -979,7 +982,7 @@ Mapper::DP* Mapper::updateMap(DP* newMap, const PM::TransformationParameters Tic
             const float probDyn = viewOnProbabilityDynamic(0, localMapId);
             const float probStatic = viewOnProbabilityStatic(0, localMapId);
 
-            mapLocalROI.setColFrom(localMapId, *newMap, i);
+            mapLocalROI.setColFrom(localMapId, *newMap, i); //TODO: check if descriptor follow...
             viewDebug(0, localMapId) = 2;  
             viewOnProbabilityDynamic(0, localMapId) = probDyn;
             viewOnProbabilityStatic(0, localMapId) = probStatic;
@@ -1056,7 +1059,7 @@ void Mapper::waitForMapBuildingCompleted()
 
 void Mapper::publishLoop(double publishPeriod)
 {
-	if(publishPeriod == 0)
+	if(publishPeriod == 0) //FIXME: just don't start the thread if 0
 		return;
 	ros::Rate r(1.0 / publishPeriod);
 	while(ros::ok())
@@ -1068,7 +1071,10 @@ void Mapper::publishLoop(double publishPeriod)
 
 void Mapper::publishTransform()
 {
-	if(processingNewCloud == false && publishMapTf == true)
+
+  // TODO: test for TREX video, put that back
+	//if(processingNewCloud == false && publishMapTf == true)
+	if(publishMapTf == true)
 	{
 		publishLock.lock();
 		// Note: we use now as timestamp to refresh the tf and avoid other buffer to be empty

libpointmatcher_ros/include/pointmatcher_ros/point_cloud.h

@@ -20,7 +20,7 @@ namespace PointMatcher_ros
 	typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::PointCloud2& rosMsg);
 
 	template<typename T>
-	typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener = 0, const std::string& fixed_frame = "/world", const bool force3D = false, const bool addTimestamps=false);
+	typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener = 0, const std::string& fixed_frame = "/world", const bool force3D = false, const bool addTimestamps=false, const bool addObservationDirection=false);
 
 	template<typename T>
 	sensor_msgs::PointCloud2 pointMatcherCloudToRosMsg(const typename PointMatcher<T>::DataPoints& pmCloud, const std::string& frame_id, const ros::Time& stamp);

libpointmatcher_ros/src/point_cloud.cpp

@@ -161,7 +161,7 @@ namespace PointMatcher_ros
 
 
 	template<typename T>
-	typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps)
+	typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps, const bool addObservationDirection)
 	{
 		typedef PointMatcher<T> PM;
 		typedef typename PM::DataPoints DataPoints;
@@ -184,11 +184,18 @@ namespace PointMatcher_ros
 			descLabels.push_back(Label("intensity", 1));
 			assert(rosMsg.intensities.size() == rosMsg.ranges.size());
 		}
+
+    int id_obs = 0;
+    if(addObservationDirection)
+    {
+			descLabels.push_back(Label("observationDirections", 3));
+    }
+
+    //TODO: change that once the time_support branch is merge in libpointmatcher
     if(addTimestamps)
     {
 			descLabels.push_back(Label("timestamp", 3));
     }
-
 		// filter points based on range
     std::vector<size_t> ids(rosMsg.ranges.size());
     std::vector<double> ranges(rosMsg.ranges.size());
@@ -219,6 +226,11 @@ namespace PointMatcher_ros
 
 		DataPoints cloud(featLabels, descLabels, goodCount);
 		cloud.getFeatureViewByName("pad").setConstant(1);
+
+    if(addObservationDirection)
+    {
+      id_obs = cloud.getDescriptorStartingRow("observationDirections");
+    }
 
 		// fill features
 		const ros::Time& startTime(rosMsg.header.stamp);
@@ -231,16 +243,18 @@ namespace PointMatcher_ros
       const T x = cos(angle) * range;
       const T y = sin(angle) * range;
 
+      // the turn ratio correct for the fact that not all sensor scan
+      // continuously during 360 deg 
+      const float turnRatio = (rosMsg.angle_max - rosMsg.angle_min)/(2*M_PI);
+      // dt_point should be more precise than rosMsg.time_increment
+      const float dt_point = (rosMsg.scan_time*turnRatio)/rosMsg.ranges.size();
+
       if (listener)
       {
-        /* Note:
-          We do an approximation, as some filters like
-          ObservationDirectionDataPointsFilter should be applied per 
-          point *before* correcting them using the tf transform, but
-          as we expect the scan to be fast with respect to the speed 
-          of the robot, we consider this approximation as being ok.
-        */
-        const ros::Time curTime(rosMsg.header.stamp + ros::Duration(ids[i] * rosMsg.time_increment));
+
+        const ros::Time curTime(rosMsg.header.stamp + ros::Duration(ids[i] * dt_point));
+        //TODO: correct curTime to handle the fact that do not cover 360 deg
+
         // wait for transform
         listener->waitForTransform(
           rosMsg.header.frame_id,
@@ -249,13 +263,21 @@ namespace PointMatcher_ros
           ros::Duration(0.1)
         );
 
-        // transform data
-        geometry_msgs::PointStamped pin, pout;
+        // transform point
+        geometry_msgs::PointStamped pin, p_out;
         pin.header.stamp = curTime;
         pin.header.frame_id = rosMsg.header.frame_id;
         pin.point.x = x;
         pin.point.y = y;
         pin.point.z = 0;
+
+        // transform sensor center
+        geometry_msgs::PointStamped s_in, s_out;
+        s_in.header.stamp = curTime;
+        s_in.header.frame_id = rosMsg.header.frame_id;
+        s_in.point.x = 0;
+        s_in.point.y = 0;
+        s_in.point.z = 0;
 
         try
         {
@@ -265,8 +287,19 @@ namespace PointMatcher_ros
             curTime,
             pin,
             fixedFrame,
-            pout
+            p_out
           );
+
+          if(addObservationDirection)
+          {
+            listener->transformPoint(
+                fixedFrame,
+                curTime,
+                s_in,
+                fixedFrame,
+                s_out
+                );
+          }
         }
         catch (const tf::ExtrapolationException& e)
         {
@@ -275,14 +308,20 @@ namespace PointMatcher_ros
         }
 
         //cout << "pin: " << pin.point.x << ", " << pin.point.y << ", " << pin.point.z << endl;
-        //cout << "pout: " << pout.point.x << ", " << pout.point.y << ", " << pout.point.z << endl;
+        //cout << "p_out: " << p_out.point.x << ", " << p_out.point.y << ", " << p_out.point.z << endl;
 
         // write back
-        cloud.features(0,i) = pout.point.x;
-        cloud.features(1,i) = pout.point.y;
+        cloud.features(0,i) = p_out.point.x;
+        cloud.features(1,i) = p_out.point.y;
         if(force3D)
-          cloud.features(2,i) = pout.point.z;
+          cloud.features(2,i) = p_out.point.z;
 
+        if(addObservationDirection)
+        {
+          cloud.descriptors(id_obs  , i) = s_out.point.x - p_out.point.x;
+          cloud.descriptors(id_obs+1, i) = s_out.point.y - p_out.point.y;
+          cloud.descriptors(id_obs+2, i) = s_out.point.z - p_out.point.z;
+        }
 			}
 		}
 
@@ -320,9 +359,9 @@ namespace PointMatcher_ros
 	}
 
 	template
-	PointMatcher<float>::DataPoints rosMsgToPointMatcherCloud<float>(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps);
+	PointMatcher<float>::DataPoints rosMsgToPointMatcherCloud<float>(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps, const bool addObservationDirection);
 	template
-	PointMatcher<double>::DataPoints rosMsgToPointMatcherCloud<double>(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps);
+	PointMatcher<double>::DataPoints rosMsgToPointMatcherCloud<double>(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps, const bool addObservationDirection);
 
 
 	template<typename T>