diff --git a/brics_3d_bride_depth_perception/.project b/brics_3d_bride_depth_perception/.project
new file mode 100644
index 0000000..52a4cb9
--- /dev/null
+++ b/brics_3d_bride_depth_perception/.project
@@ -0,0 +1,27 @@
+
+
+ brics_3d_bride_depth_perception
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/brics_3d_bride_example_system/.project b/brics_3d_bride_example_system/.project
new file mode 100644
index 0000000..9556c3d
--- /dev/null
+++ b/brics_3d_bride_example_system/.project
@@ -0,0 +1,27 @@
+
+
+ brics_3d_bride_example_system
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/brics_3d_bride_example_system/model/filtering_example.ros b/brics_3d_bride_example_system/model/filtering_example.ros
index 26ccad9..2b148bb 100644
--- a/brics_3d_bride_example_system/model/filtering_example.ros
+++ b/brics_3d_bride_example_system/model/filtering_example.ros
@@ -2,4 +2,5 @@
+
diff --git a/brics_3d_bride_example_system/model/filtering_example.ros_diagram b/brics_3d_bride_example_system/model/filtering_example.ros_diagram
index e6de549..d336144 100644
--- 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 @@
-
+
-
+
-
+
@@ -33,7 +33,7 @@
-
+
@@ -41,14 +41,56 @@
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -68,4 +110,12 @@
+
+
+
+
+
+
+
+
diff --git a/brics_3d_bride_filtering/.project b/brics_3d_bride_filtering/.project
new file mode 100644
index 0000000..12a8032
--- /dev/null
+++ b/brics_3d_bride_filtering/.project
@@ -0,0 +1,27 @@
+
+
+ brics_3d_bride_filtering
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/brics_3d_bride_filtering/model/brics_3d_filter.ros_package b/brics_3d_bride_filtering/model/brics_3d_filter.ros_package
index 3433211..b93c323 100644
--- 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 @@
brics_3d
pcl
sensor_msgs
+
+
+
diff --git a/brics_3d_bride_registration/.project b/brics_3d_bride_registration/.project
new file mode 100644
index 0000000..850cc2f
--- /dev/null
+++ b/brics_3d_bride_registration/.project
@@ -0,0 +1,27 @@
+
+
+ brics_3d_bride_registration
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/brics_3d_bride_registration/common/src/Registrator_common.cpp b/brics_3d_bride_registration/common/src/Registrator_common.cpp
index c0d2eb0..b59a21b 100644
--- 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
#include
+#include
+#include
#include
+#include
+
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::Ptr dataPointCloutPcl(new pcl::PointCloud);
pcl::PointCloud::Ptr modelPointCloutPcl(new pcl::PointCloud);
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
index 73eb838..bd25d36 100644
--- 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 @@
geometry_msgs
sensor_msgs
tf
- brics_3d
pcl
+ brics_3d
+ brics_3d_ros