Bugfixes for registration implementation. Project files added for smo…

…other importing of BRIDE projects.
blumenthal · Nov 21, 2012 · 940858d · 940858d
1 parent 9e55931
commit 940858d
Show file tree

Hide file tree

Showing 9 changed files with 197 additions and 15 deletions.
diff --git a/brics_3d_bride_depth_perception/.project b/brics_3d_bride_depth_perception/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>brics_3d_bride_depth_perception</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>
diff --git a/brics_3d_bride_example_system/.project b/brics_3d_bride_example_system/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>brics_3d_bride_example_system</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>
diff --git a/brics_3d_bride_example_system/model/filtering_example.ros b/brics_3d_bride_example_system/model/filtering_example.ros
@@ -2,4 +2,5 @@
 <ros:Architecture xmi:version="2.0" xmlns:xmi="http://www.omg.org/XMI" xmlns:ros="http://ros/1.0" name="brics_3d_example_system">
   <packages href="../../brics_3d_bride_depth_perception/model/camera_stub.ros_package#/"/>
   <packages href="../../brics_3d_bride_filtering/model/brics_3d_filter.ros_package#/"/>
+  <packages href="../../brics_3d_bride_registration/model/brics_3d_registration.ros_package#/"/>
 </ros:Architecture>
diff --git a/brics_3d_bride_example_system/model/filtering_example.ros_diagram b/brics_3d_bride_example_system/model/filtering_example.ros_diagram
@@ -12,14 +12,14 @@
           <styles xmi:type="notation:FontStyle" xmi:id="_xXKbYhhXEeKzJbRyYk3zbA" fontName="Ubuntu"/>
           <styles xmi:type="notation:LineStyle" xmi:id="_xXKbYxhXEeKzJbRyYk3zbA"/>
           <element xmi:type="ros:Publisher" href="platform:/resource/brics_3d_bride_depth_perception/model/camera_stub.ros_package#//@node.0/@publisher.0"/>
-          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXKbZBhXEeKzJbRyYk3zbA"/>
+          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXKbZBhXEeKzJbRyYk3zbA" x="239" y="63"/>
         </children>
         <element xmi:type="ros:Node" href="platform:/resource/brics_3d_bride_depth_perception/model/camera_stub.ros_package#//@node.0"/>
-        <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXImMRhXEeKzJbRyYk3zbA"/>
+        <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXImMRhXEeKzJbRyYk3zbA" x="17" y="13"/>
       </children>
     </children>
     <element xmi:type="ros:Package" href="platform:/resource/brics_3d_bride_depth_perception/model/camera_stub.ros_package#/"/>
-    <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXFi4RhXEeKzJbRyYk3zbA"/>
+    <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXFi4RhXEeKzJbRyYk3zbA" x="36" y="45" height="298"/>
   </children>
   <children xmi:type="notation:Shape" xmi:id="_xXGJ8BhXEeKzJbRyYk3zbA" type="2001" fontName="Ubuntu">
     <children xmi:type="notation:DecorationNode" xmi:id="_xXGJ8hhXEeKzJbRyYk3zbA" type="5009"/>
@@ -33,22 +33,64 @@
           <styles xmi:type="notation:FontStyle" xmi:id="_xXRJERhXEeKzJbRyYk3zbA" fontName="Ubuntu"/>
           <styles xmi:type="notation:LineStyle" xmi:id="_xXRJEhhXEeKzJbRyYk3zbA"/>
           <element xmi:type="ros:Publisher" href="platform:/resource/brics_3d_bride_filtering/model/brics_3d_filter.ros_package#//@node.0/@publisher.0"/>
-          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXRJExhXEeKzJbRyYk3zbA" x="-1" y="-41"/>
+          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXRJExhXEeKzJbRyYk3zbA" x="239" y="67"/>
         </children>
         <children xmi:type="notation:Node" xmi:id="_xXRJFRhXEeKzJbRyYk3zbA" type="3004">
           <children xmi:type="notation:DecorationNode" xmi:id="_xXRJGhhXEeKzJbRyYk3zbA" type="5003"/>
           <styles xmi:type="notation:DescriptionStyle" xmi:id="_xXRJFhhXEeKzJbRyYk3zbA"/>
           <styles xmi:type="notation:FontStyle" xmi:id="_xXRJFxhXEeKzJbRyYk3zbA" fontName="Ubuntu"/>
           <styles xmi:type="notation:LineStyle" xmi:id="_xXRJGBhXEeKzJbRyYk3zbA"/>
           <element xmi:type="ros:Subscriber" href="platform:/resource/brics_3d_bride_filtering/model/brics_3d_filter.ros_package#//@node.0/@subscriber.0"/>
-          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXRJGRhXEeKzJbRyYk3zbA" x="-107" y="1"/>
+          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXRJGRhXEeKzJbRyYk3zbA" x="-107" y="58"/>
         </children>
         <element xmi:type="ros:Node" href="platform:/resource/brics_3d_bride_filtering/model/brics_3d_filter.ros_package#//@node.0"/>
         <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXNesRhXEeKzJbRyYk3zbA"/>
       </children>
     </children>
     <element xmi:type="ros:Package" href="platform:/resource/brics_3d_bride_filtering/model/brics_3d_filter.ros_package#/"/>
-    <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXGJ8RhXEeKzJbRyYk3zbA" x="450" y="270"/>
+    <layoutConstraint xmi:type="notation:Bounds" xmi:id="_xXGJ8RhXEeKzJbRyYk3zbA" x="486" y="45" width="559" height="281"/>
+  </children>
+  <children xmi:type="notation:Shape" xmi:id="_8F7YEDMaEeKyebXB5NnPyA" type="2001" fontName="Ubuntu">
+    <children xmi:type="notation:DecorationNode" xmi:id="_8F7YEzMaEeKyebXB5NnPyA" type="5009"/>
+    <children xmi:type="notation:BasicCompartment" xmi:id="_8F7YFDMaEeKyebXB5NnPyA" type="7001">
+      <children xmi:type="notation:Shape" xmi:id="_8F9NQDMaEeKyebXB5NnPyA" type="3001" fontName="Ubuntu">
+        <children xmi:type="notation:DecorationNode" xmi:id="_8F90UTMaEeKyebXB5NnPyA" type="5008"/>
+        <children xmi:type="notation:BasicCompartment" xmi:id="_8F90UjMaEeKyebXB5NnPyA" type="7002">
+          <styles xmi:type="notation:TitleStyle" xmi:id="_8F90UzMaEeKyebXB5NnPyA" showTitle="true"/>
+          <styles xmi:type="notation:SortingStyle" xmi:id="_8F90VDMaEeKyebXB5NnPyA"/>
+          <styles xmi:type="notation:FilteringStyle" xmi:id="_8F90VTMaEeKyebXB5NnPyA"/>
+        </children>
+        <children xmi:type="notation:Node" xmi:id="_8F_CcDMaEeKyebXB5NnPyA" type="3003">
+          <children xmi:type="notation:DecorationNode" xmi:id="_8F_pgDMaEeKyebXB5NnPyA" type="5002"/>
+          <styles xmi:type="notation:DescriptionStyle" xmi:id="_8F_CcTMaEeKyebXB5NnPyA"/>
+          <styles xmi:type="notation:FontStyle" xmi:id="_8F_CcjMaEeKyebXB5NnPyA" fontName="Ubuntu"/>
+          <styles xmi:type="notation:LineStyle" xmi:id="_8F_CczMaEeKyebXB5NnPyA"/>
+          <element xmi:type="ros:Publisher" href="platform:/resource/brics_3d_bride_registration/model/brics_3d_registration.ros_package#//@node.0/@publisher.0"/>
+          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_8F_CdDMaEeKyebXB5NnPyA" x="239" y="49"/>
+        </children>
+        <children xmi:type="notation:Node" xmi:id="_8F_pgTMaEeKyebXB5NnPyA" type="3004">
+          <children xmi:type="notation:DecorationNode" xmi:id="_8F_phjMaEeKyebXB5NnPyA" type="5003"/>
+          <styles xmi:type="notation:DescriptionStyle" xmi:id="_8F_pgjMaEeKyebXB5NnPyA"/>
+          <styles xmi:type="notation:FontStyle" xmi:id="_8F_pgzMaEeKyebXB5NnPyA" fontName="Ubuntu"/>
+          <styles xmi:type="notation:LineStyle" xmi:id="_8F_phDMaEeKyebXB5NnPyA"/>
+          <element xmi:type="ros:Subscriber" href="platform:/resource/brics_3d_bride_registration/model/brics_3d_registration.ros_package#//@node.0/@subscriber.0"/>
+          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_8F_phTMaEeKyebXB5NnPyA" x="-102" y="40"/>
+        </children>
+        <children xmi:type="notation:Node" xmi:id="_8GAQkDMaEeKyebXB5NnPyA" type="3004">
+          <children xmi:type="notation:DecorationNode" xmi:id="_8GAQlTMaEeKyebXB5NnPyA" type="5003"/>
+          <styles xmi:type="notation:DescriptionStyle" xmi:id="_8GAQkTMaEeKyebXB5NnPyA"/>
+          <styles xmi:type="notation:FontStyle" xmi:id="_8GAQkjMaEeKyebXB5NnPyA" fontName="Ubuntu"/>
+          <styles xmi:type="notation:LineStyle" xmi:id="_8GAQkzMaEeKyebXB5NnPyA"/>
+          <element xmi:type="ros:Subscriber" href="platform:/resource/brics_3d_bride_registration/model/brics_3d_registration.ros_package#//@node.0/@subscriber.1"/>
+          <layoutConstraint xmi:type="notation:Bounds" xmi:id="_8GAQlDMaEeKyebXB5NnPyA" x="-112" y="112"/>
+        </children>
+        <element xmi:type="ros:Node" href="platform:/resource/brics_3d_bride_registration/model/brics_3d_registration.ros_package#//@node.0"/>
+        <layoutConstraint xmi:type="notation:Bounds" xmi:id="_8F90UDMaEeKyebXB5NnPyA"/>
+      </children>
+    </children>
+    <styles xmi:type="notation:HintedDiagramLinkStyle" xmi:id="_8F7YETMaEeKyebXB5NnPyA" hint="RosPackage"/>
+    <element xmi:type="ros:Package" href="platform:/resource/brics_3d_bride_registration/model/brics_3d_registration.ros_package#/"/>
+    <layoutConstraint xmi:type="notation:Bounds" xmi:id="_8F7YEjMaEeKyebXB5NnPyA" x="1233" y="72" width="496" height="299"/>
   </children>
   <styles xmi:type="notation:DiagramStyle" xmi:id="_eVcwoQfSEeKUmrXeTVCIDw"/>
   <element xmi:type="ros:Architecture" href="filtering_example.ros#/"/>
@@ -68,4 +110,12 @@
     <element xmi:type="ros:Topic" href="platform:/resource/brics_3d_bride_depth_perception/model/camera_stub.ros_package#//@topic.4"/>
     <bendpoints xmi:type="notation:RelativeBendpoints" xmi:id="_ndET8SHSEeK0hsm9gQ2f9Q" points="[10, 20, -114, -202]$[119, 202, -5, -20]"/>
   </edges>
+  <edges xmi:type="notation:Connector" xmi:id="_F_2qkDMbEeKyebXB5NnPyA" type="4001" source="_xXQiABhXEeKzJbRyYk3zbA" target="_8F_pgTMaEeKyebXB5NnPyA">
+    <children xmi:type="notation:DecorationNode" xmi:id="_F_3RoDMbEeKyebXB5NnPyA" type="6001">
+      <layoutConstraint xmi:type="notation:Location" xmi:id="_F_3RoTMbEeKyebXB5NnPyA" x="64" y="35"/>
+    </children>
+    <styles xmi:type="notation:FontStyle" xmi:id="_F_2qkTMbEeKyebXB5NnPyA" fontName="Ubuntu"/>
+    <element xmi:type="ros:Topic" href="platform:/resource/brics_3d_bride_filtering/model/brics_3d_filter.ros_package#//@topic.0"/>
+    <bendpoints xmi:type="notation:RelativeBendpoints" xmi:id="_F_2qkjMbEeKyebXB5NnPyA" points="[45, 10, -220, -40]$[227, 57, -38, 7]"/>
+  </edges>
 </notation:Diagram>
diff --git a/brics_3d_bride_filtering/.project b/brics_3d_bride_filtering/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>brics_3d_bride_filtering</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>
diff --git a/brics_3d_bride_filtering/model/brics_3d_filter.ros_package b/brics_3d_bride_filtering/model/brics_3d_filter.ros_package
@@ -9,4 +9,7 @@
   <depend>brics_3d</depend>
   <depend>pcl</depend>
   <depend>sensor_msgs</depend>
+  <topic name="filter_to_data" publisher="//@node.0/@publisher.0">
+    <subscriber href="../../brics_3d_bride_registration/model/brics_3d_registration.ros_package#//@node.0/@subscriber.0"/>
+  </topic>
 </ros:Package>
diff --git a/brics_3d_bride_registration/.project b/brics_3d_bride_registration/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>brics_3d_bride_registration</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>
diff --git a/brics_3d_bride_registration/common/src/Registrator_common.cpp b/brics_3d_bride_registration/common/src/Registrator_common.cpp
@@ -11,8 +11,12 @@
 /* BRICS_3D includes */
 #include <brics_3d/core/HomogeneousMatrix44.h>
 #include <brics_3d/algorithm/registration/IterativeClosestPoint.h>
+#include <brics_3d/algorithm/registration/PointCorrespondenceKDTree.h>
+#include <brics_3d/algorithm/registration/RigidTransformationEstimationSVD.h>
 #include <brics_3d/util/PCLTypecaster.h>
 
+#include <brics_3d_ros/SceneGraphTypeCasts.h>
+
 typedef pcl::PointXYZ PointType;
 /* protected region user include files end */
 
@@ -52,12 +56,18 @@ class Registrator_impl
     void configure(Registrator_config config) 
     {
         /* protected region user configure on begin */
-    	registrator = new brics_3d::IterativeClosestPoint();
+    	registrator = new brics_3d::IterativeClosestPoint(new brics_3d::PointCorrespondenceKDTree(), new brics_3d::RigidTransformationEstimationSVD());
 		/* protected region user configure end */
     }
     void update(Registrator_data &data, Registrator_config config)
     {
         /* protected region user update on begin */
+    	if(data.in_dataPointCloud.width == 0 || data.in_modelPointCloud.width == 0) {
+    		ROS_WARN_STREAM("Empty input point clouds. Skipping.");
+    		return;
+    	}
+
+    	/* data conversions */
     	pcl::PointCloud<PointType>::Ptr dataPointCloutPcl(new pcl::PointCloud<PointType>);
     	pcl::PointCloud<PointType>::Ptr modelPointCloutPcl(new pcl::PointCloud<PointType>);
     	pcl::fromROSMsg(data.in_dataPointCloud, *dataPointCloutPcl);
@@ -66,18 +76,27 @@ class Registrator_impl
     	brics_3d::PointCloud3D dataPointCloud;
     	brics_3d::PointCloud3D modelPointCloud;
 
-
     	brics_3d::PCLTypecaster caster;
     	caster.convertToBRICS3DDataType(dataPointCloutPcl, &dataPointCloud);
     	caster.convertToBRICS3DDataType(modelPointCloutPcl, &modelPointCloud);
-    	brics_3d::HomogeneousMatrix44* resultTransform = new brics_3d::HomogeneousMatrix44();
+    	brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr resultTransform(new brics_3d::HomogeneousMatrix44());
 
+    	/* The actual computation: registration of two point clouds */
     	ROS_INFO("Registering.");
-    	registrator->match(&modelPointCloud, &dataPointCloud, resultTransform);
-
-    	ROS_INFO_STREAM("resulting transform = " << std::endl << resultTransform);
-
-    	delete resultTransform;
+    	registrator->match(&modelPointCloud, &dataPointCloud, resultTransform.get());
+    	ROS_INFO_STREAM("resulting transform = " << std::endl << *resultTransform);
+
+    	/* Some gymnastics to publish a TF. In BRIDE code we cannot use the tf brodcaster... */
+    	tf::Transform tfTransform;
+    	tf::StampedTransform tfStampedTransform;
+    	geometry_msgs::TransformStamped tfGeometryMsg;
+    	brics_3d::rsg::SceneGraphTypeCasts::convertHomogeniousMatrixToTfTransform(resultTransform, tfTransform);
+    	tfStampedTransform.setData(tfTransform);
+    	tf::transformStampedTFToMsg(tfStampedTransform, tfGeometryMsg);
+
+    	tf::tfMessage message;
+    	message.transforms.push_back(tfGeometryMsg);
+    	data.out_transform = message;
 		/* protected region user update end */
     }
 

diff --git a/brics_3d_bride_registration/model/brics_3d_registration.ros_package b/brics_3d_bride_registration/model/brics_3d_registration.ros_package
@@ -8,6 +8,7 @@
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>tf</depend>
-  <depend>brics_3d</depend>
   <depend>pcl</depend>
+  <depend>brics_3d</depend>
+  <depend>brics_3d_ros</depend>
 </ros:Package>