Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8078] pkg/trunk
|
From: <ve...@us...> - 2008-12-14 22:29:31
|
Revision: 8078
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8078&view=rev
Author: veedee
Date: 2008-12-14 22:04:35 +0000 (Sun, 14 Dec 2008)
Log Message:
-----------
first batch commit with 3 libraries (80%, 70%, 50% done) and one simple node. nothing to get excited over yet :)
Added Paths:
-----------
pkg/trunk/mapping/
pkg/trunk/mapping/bag_pcd/
pkg/trunk/mapping/bag_pcd/CMakeLists.txt
pkg/trunk/mapping/bag_pcd/Makefile
pkg/trunk/mapping/bag_pcd/manifest.xml
pkg/trunk/mapping/bag_pcd/src/
pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp
pkg/trunk/mapping/cloud_geometry/
pkg/trunk/mapping/cloud_geometry/CMakeLists.txt
pkg/trunk/mapping/cloud_geometry/Makefile
pkg/trunk/mapping/cloud_geometry/include/
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h
pkg/trunk/mapping/cloud_geometry/mainpage.dox
pkg/trunk/mapping/cloud_geometry/manifest.xml
pkg/trunk/mapping/cloud_geometry/src/
pkg/trunk/mapping/cloud_geometry/src/lapack.cpp
pkg/trunk/mapping/cloud_geometry/src/nearest.cpp
pkg/trunk/mapping/cloud_geometry/src/point.cpp
pkg/trunk/mapping/cloud_io/
pkg/trunk/mapping/cloud_io/CMakeLists.txt
pkg/trunk/mapping/cloud_io/Makefile
pkg/trunk/mapping/cloud_io/include/
pkg/trunk/mapping/cloud_io/include/cloud_io/
pkg/trunk/mapping/cloud_io/include/cloud_io/cloud_io.h
pkg/trunk/mapping/cloud_io/mainpage.dox
pkg/trunk/mapping/cloud_io/manifest.xml
pkg/trunk/mapping/cloud_io/src/
pkg/trunk/mapping/cloud_io/src/misc.cpp
pkg/trunk/mapping/cloud_io/src/read.cpp
pkg/trunk/mapping/cloud_io/src/write.cpp
pkg/trunk/mapping/sample_consensus/
pkg/trunk/mapping/sample_consensus/CMakeLists.txt
pkg/trunk/mapping/sample_consensus/Makefile
pkg/trunk/mapping/sample_consensus/include/
pkg/trunk/mapping/sample_consensus/include/sample_consensus/
pkg/trunk/mapping/sample_consensus/include/sample_consensus/lmeds.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/mlesac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/model_types.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/msac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/ransac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/rmsac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/rransac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac_model.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac_model_line.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac_model_plane.h
pkg/trunk/mapping/sample_consensus/mainpage.dox
pkg/trunk/mapping/sample_consensus/manifest.xml
pkg/trunk/mapping/sample_consensus/src/
pkg/trunk/mapping/sample_consensus/src/lmeds.cpp
pkg/trunk/mapping/sample_consensus/src/mlesac.cpp
pkg/trunk/mapping/sample_consensus/src/msac.cpp
pkg/trunk/mapping/sample_consensus/src/ransac.cpp
pkg/trunk/mapping/sample_consensus/src/rmsac.cpp
pkg/trunk/mapping/sample_consensus/src/rransac.cpp
pkg/trunk/mapping/sample_consensus/src/sac.cpp
pkg/trunk/mapping/sample_consensus/src/sac_model.cpp
pkg/trunk/mapping/sample_consensus/src/sac_model_line.cpp
pkg/trunk/mapping/sample_consensus/src/sac_model_plane.cpp
pkg/trunk/mapping/sample_consensus/test/
pkg/trunk/mapping/sample_consensus/test/test_plane_fit.cpp
Added: pkg/trunk/mapping/bag_pcd/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/bag_pcd/CMakeLists.txt (rev 0)
+++ pkg/trunk/mapping/bag_pcd/CMakeLists.txt 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.6)
+
+include(rosbuild)
+set(ROS_BUILD_TYPE Release)
+rospack(bag_pcd)
+add_definitions(-Wall)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_executable(bag_pcd src/bag_pcd.cpp)
Added: pkg/trunk/mapping/bag_pcd/Makefile
===================================================================
--- pkg/trunk/mapping/bag_pcd/Makefile (rev 0)
+++ pkg/trunk/mapping/bag_pcd/Makefile 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mapping/bag_pcd/manifest.xml
===================================================================
--- pkg/trunk/mapping/bag_pcd/manifest.xml (rev 0)
+++ pkg/trunk/mapping/bag_pcd/manifest.xml 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,16 @@
+<package>
+ <description>
+ A node which converts 3D point cloud data from BAG log files into PCD (Point Cloud Data) files.
+ </description>
+
+ <author>Radu Bogdan Rusu (ru...@cs...)</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com</url>
+ <review status="unreviewed" notes="beta"/>
+
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="tf" />
+ <depend package="cloud_io" />
+
+</package>
Added: pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp
===================================================================
--- pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp (rev 0)
+++ pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,168 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: bag_pcd.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+\author Radu Bogdan Rusu
+
+@b bag_pcd is a simple node that gets a complete point cloud and saves it into a PCD (Point Cloud Data) format.
+
+ **/
+
+// ROS core
+#include "ros/node.h"
+
+#include "tf/transform_listener.h"
+
+#include <fstream>
+
+#include "std_msgs/PointStamped.h"
+#include "std_msgs/PointCloud.h"
+
+#include "cloud_io/cloud_io.h"
+
+using namespace std_msgs;
+
+class BagToPcd: public ros::node
+{
+ public:
+
+ // ROS messages
+ PointCloud cloud_;
+
+ // Save data to disk ?
+ char fn_[80];
+ bool dump_to_disk_;
+
+ tf::TransformListener tf_;
+
+
+ ////////////////////////////////////////////////////////////////////////////////
+ BagToPcd () : ros::node ("bag_pcd"), dump_to_disk_(false), tf_(*this)
+ {
+ subscribe ("tilt_laser_cloud", cloud_, &BagToPcd::cloud_cb, 1);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ virtual ~BagToPcd () { }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Dump a point cloud to disk
+ * \param fn the name of the output file
+ * \param cloud the point cloud data message
+ * \param vx the X coordinate of the viewpoint
+ * \param vy the Y coordinate of the viewpoint
+ * \param vz the Z coordinate of the viewpoint
+ */
+ void
+ saveCloud (char *fn, PointCloud cloud, double vx, double vy, double vz)
+ {
+ std::ofstream fs;
+ fs.precision (5);
+ fs.open (fn);
+
+ int nr_pts = cloud.get_pts_size ();
+ int dim = cloud.get_chan_size ();
+ fs << "# [MetaInfo] Viewpoint " << vx << "," << vy << "," << vz << std::endl;
+ fs << "COLUMNS x y z";
+ for (int d = 0; d < dim; d++)
+ fs << " " << cloud.chan[d].name;
+ fs << std::endl;
+ fs << "POINTS " << nr_pts << std::endl;
+ fs << "DATA ascii" << std::endl;
+
+ for (int i = 0; i < nr_pts; i++)
+ {
+ fs << cloud.pts[i].x << " " << cloud.pts[i].y << " " << cloud.pts[i].z;
+ for (int d = 0; d < dim; d++)
+ fs << " " << cloud.chan[d].vals[i];
+ fs << std::endl;
+ }
+ fs.close ();
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ // Callback
+ void cloud_cb ()
+ {
+ PointStamped pin, pout;
+ pin.header.frame_id = "laser_tilt_mount_link";
+ pin.point.x = pin.point.y = pin.point.z = 0.0;
+
+ tf_.transformPoint ("base_link", pin, pout);
+
+ fprintf (stderr, "Received %d data points. Viewpoint is <%.3f, %.3f, %.3f>\n", cloud_.pts.size (), pout.point.x, pout.point.y, pout.point.z);
+
+ double c_time = cloud_.header.stamp.sec * 1e3 + cloud_.header.stamp.nsec;
+ sprintf (fn_, "%.0f.pcd", c_time);
+ if (dump_to_disk_)
+ {
+ {
+ // Add information about the viewpoint - rudimentary stuff
+ cloud_.chan.resize (cloud_.chan.size () + 3);
+ cloud_.chan[cloud_.chan.size () - 3].name = "vx";
+ cloud_.chan[cloud_.chan.size () - 2].name = "vy";
+ cloud_.chan[cloud_.chan.size () - 1].name = "vz";
+ cloud_.chan[cloud_.chan.size () - 3].vals.resize (cloud_.pts.size ());
+ cloud_.chan[cloud_.chan.size () - 2].vals.resize (cloud_.pts.size ());
+ cloud_.chan[cloud_.chan.size () - 1].vals.resize (cloud_.pts.size ());
+ for (unsigned int i = 0; i < cloud_.pts.size (); i++)
+ {
+ cloud_.chan[cloud_.chan.size () - 3].vals[i] = pout.point.x;
+ cloud_.chan[cloud_.chan.size () - 2].vals[i] = pout.point.y;
+ cloud_.chan[cloud_.chan.size () - 1].vals[i] = pout.point.z;
+ }
+ cloud_io::savePCDFile (fn_, cloud_, 5);
+ }
+ //saveCloud (fn_, cloud_, pout.point.x, pout.point.y, pout.point.z);
+ fprintf (stderr, "Data saved to %s.\n", fn_);
+ }
+ }
+};
+
+/* ---[ */
+int
+ main (int argc, char** argv)
+{
+ ros::init (argc, argv);
+
+ BagToPcd b;
+ b.dump_to_disk_ = true;
+ b.spin ();
+
+ ros::fini ();
+
+ return (0);
+}
+/* ]--- */
+
Added: pkg/trunk/mapping/cloud_geometry/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/cloud_geometry/CMakeLists.txt (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/CMakeLists.txt 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.6)
+
+include(rosbuild)
+set(ROS_BUILD_TYPE Release)
+rospack(cloud_geometry)
+add_definitions(-Wall)
+
+rospack_add_library(cloud_geometry src/lapack.cpp src/nearest.cpp src/point.cpp)
Added: pkg/trunk/mapping/cloud_geometry/Makefile
===================================================================
--- pkg/trunk/mapping/cloud_geometry/Makefile (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/Makefile 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: lapack.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_LAPACK_H_
+#define _CLOUD_GEOMETRY_LAPACK_H_
+
+#include "Eigen/Core"
+
+extern "C" void dsyev_ (char *jobz, char *uplo, int *n, double *a, int *lda, double *w, double *work, int *lwork, int *info);
+
+namespace cloud_geometry
+{
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the 3 eigen values and eigenvectors for a 3x3 covariance matrix
+ * \param covariance_matrix a 3x3 covariance matrix in Eigen2::Matrix3d format
+ * \param eigen_values the resulted eigenvalues in Eigen2::Vector3d
+ * \param eigen_vectors a 3x3 matrix in Eigen2::Matrix3d format, containing each eigenvector on a new line */
+ bool eigen_cov (Eigen::Matrix3d covariance_matrix, Eigen::Vector3d &eigen_values, Eigen::Matrix3d &eigen_vectors);
+
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: nearest.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_NEAREST_H_
+#define _CLOUD_GEOMETRY_NEAREST_H_
+
+#include "std_msgs/PointCloud.h"
+#include "std_msgs/Point32.h"
+
+#include "Eigen/Core"
+#include "cloud_geometry/lapack.h"
+
+namespace cloud_geometry
+{
+
+ namespace nearest
+ {
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the centroid of a set of points and return it as a Point32 message.
+ * \param points the input point cloud
+ * \param centroid the output centroid
+ */
+ inline void
+ computeCentroid (std_msgs::PointCloud points, std_msgs::Point32 ¢roid)
+ {
+ // For each point in the cloud
+ for (unsigned int i = 0; i < points.get_pts_size (); i++)
+ {
+ centroid.x += points.pts.at (i).x;
+ centroid.y += points.pts.at (i).y;
+ centroid.z += points.pts.at (i).z;
+ }
+
+ centroid.x /= points.get_pts_size ();
+ centroid.y /= points.get_pts_size ();
+ centroid.z /= points.get_pts_size ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the centroid of a set of points using their indices and return it as a Point32 message.
+ * \param points the input point cloud
+ * \param indices the point cloud indices that need to be used
+ * \param centroid the output centroid
+ */
+ inline void
+ computeCentroid (std_msgs::PointCloud points, std::vector<int> indices, std_msgs::Point32 ¢roid)
+ {
+ // For each point in the cloud
+ for (unsigned int i = 0; i < indices.size (); i++)
+ {
+ centroid.x += points.pts.at (indices.at (i)).x;
+ centroid.y += points.pts.at (indices.at (i)).y;
+ centroid.z += points.pts.at (indices.at (i)).z;
+ }
+
+ centroid.x /= indices.size ();
+ centroid.y /= indices.size ();
+ centroid.z /= indices.size ();
+ }
+
+ void computeCentroid (std_msgs::PointCloud points, std_msgs::PointCloud ¢roid);
+ void computeCentroid (std_msgs::PointCloud points, std::vector<int> indices, std_msgs::PointCloud ¢roid);
+
+ void computeCovarianceMatrix (std_msgs::PointCloud points, Eigen::Matrix3d &covariance_matrix);
+ void computeCovarianceMatrix (std_msgs::PointCloud points, Eigen::Matrix3d &covariance_matrix, std_msgs::Point32 ¢roid);
+ void computeCovarianceMatrix (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Matrix3d &covariance_matrix);
+ void computeCovarianceMatrix (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Matrix3d &covariance_matrix, std_msgs::Point32 ¢roid);
+
+ void computeSurfaceNormalCurvature (std_msgs::PointCloud points, Eigen::Vector4d &plane_parameters, double &curvature);
+ void computeSurfaceNormalCurvature (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Vector4d &plane_parameters, double &curvature);
+
+ }
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,268 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: norms.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_NORMS_H_
+#define _CLOUD_GEOMETRY_NORMS_H_
+
+namespace cloud_geometry
+{
+
+ namespace norms
+ {
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the L1 norm between two nD points (aka aka Manhattan norm,
+ * rectilinear distance, Minkowski's L1 distance, taxi cab metric, or city
+ * block distance)
+ * L1 = Sum (|x_i|), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ L1_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += fabs (A[i] - B[i]);
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the squared L2 norm between two nD points (aka Euclidean metric)
+ * L2_SQR = Sum (|x_i|^2), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ L2_Norm_SQR (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (A[i] - B[i]) * (A[i] - B[i]);
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the L2 norm between two nD points (aka Euclidean metric)
+ * L2 = SQRT (Sum (|x_i|^2)), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ L2_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (A[i] - B[i]) * (A[i] - B[i]);
+
+ return sqrt (norm);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Linf norm between two nD points (aka Minkowski distance,
+ * Chebyshev norm, or supremum norm)
+ * Linf = max(|xi|), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ Linf_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm = (fabs (A[i] - B[i]) > norm) ? fabs (A[i] - B[i]) : norm;
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Jeffries-Matusita (JM) distance between two nD points (aka
+ * Hellinger distance)
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ JM_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (sqrt (A[i]) - sqrt (B[i])) * (sqrt (A[i]) - sqrt (B[i]));
+
+ return sqrt (norm);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Bhattacharyya (B) distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ B_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0, result;
+
+ for (int i = 0; i < dim; i++)
+ norm += sqrt (A[i] * B[i]);
+
+ if (norm > 0)
+ result = -log (norm);
+ else
+ result = 0;
+
+ return result;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Sublinear kernel distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ Sublinear_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += sqrt (fabs (A[i] - B[i]));
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Chi-Square (CS) distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ CS_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ if ((A[i] + B[i]) != 0)
+ norm += (A[i] - B[i]) * (A[i] - B[i]) / (A[i] + B[i]);
+ else
+ norm += 0;
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Divergence distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ Div_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ if ((A[i] / B[i]) > 0)
+ norm += (A[i] - B[i]) * log (A[i] / B[i]);
+ else
+ norm += 0;
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Patrick-Fisher distance between two nD points (Same as L2 ! - when P1 = P2)
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ * \param P1 P1
+ * \param P2 P2
+ */
+ inline double
+ PF_Norm (float *A, float *B, int dim, double P1, double P2)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (P1 * A[i] - P2 * B[i]) * (P1 * A[i] - P2 * B[i]);
+ return sqrt (norm);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Kolmogorov distance between two nD points (Same as L1 ! - when P1 = P2)
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ * \param P1 P1
+ * \param P2 P2
+ */
+ inline double
+ K_Norm (float *A, float *B, int dim, double P1, double P2)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += fabs (P1 * A[i] - P2 * B[i]);
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Kullback-Leibler distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ KL_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ if ( (B[i] != 0) && ((A[i] / B[i]) > 0) )
+ norm += A[i] * log (A[i] / B[i]);
+ else
+ norm += 0;
+ return norm;
+ }
+ }
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: point.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_POINT_H_
+#define _CLOUD_GEOMETRY_POINT_H_
+
+// ROS includes
+#include "std_msgs/PointCloud.h"
+#include "std_msgs/Point32.h"
+
+namespace cloud_geometry
+{
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Create a quick copy of a point and its associated channels and return the data as a float vector.
+ * \param points the point cloud data message
+ * \param index the index of the point to return
+ * \param array the vector containing the result
+ */
+ inline void
+ getPointAsFloatArray (std_msgs::PointCloud points, int index, std::vector<float> &array)
+ {
+ // Resize for XYZ (3) + NR_CHANNELS
+ array.resize (3 + points.get_chan_size ());
+ array[0] = points.pts[index].x;
+ array[1] = points.pts[index].y;
+ array[2] = points.pts[index].z;
+
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ array[3 + d] = points.chan[d].vals[index];
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Create a quick copy of a point and its associated channels and return the data as a float vector.
+ * \param points the point cloud data message
+ * \param index the index of the point to return
+ * \param array the vector containing the result
+ * \param nr_channels the number of channels to copy (starting with the first channel)
+ */
+ inline void
+ getPointAsFloatArray (std_msgs::PointCloud points, int index, std::vector<float> &array, int nr_channels)
+ {
+ // Resize for XYZ (3) + NR_CHANNELS
+ array.resize (3 + nr_channels);
+ array[0] = points.pts[index].x;
+ array[1] = points.pts[index].y;
+ array[2] = points.pts[index].z;
+
+ for (int d = 0; d < nr_channels; d++)
+ array[3 + d] = points.chan[d].vals[index];
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Create a quick copy of a point and its associated channels and return the data as a float vector.
+ * \param points the point cloud data message
+ * \param index the index of the point to return
+ * \param array the vector containing the result
+ * \param start_channel the first channel to start copying data from
+ * \param end_channel the last channel to stop copying data at
+ */
+ inline void
+ getPointAsFloatArray (std_msgs::PointCloud points, int index, std::vector<float> &array, int start_channel, int end_channel)
+ {
+ // Resize for XYZ (3) + NR_CHANNELS
+ array.resize (3 + end_channel - start_channel);
+ array[0] = points.pts[index].x;
+ array[1] = points.pts[index].y;
+ array[2] = points.pts[index].z;
+
+ for (int d = start_channel; d < end_channel; d++)
+ array[3 + d - start_channel] = points.chan[d].vals[index];
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Create a quick copy of a point and its associated channels and return the data as a float vector.
+ * \param points the point cloud data message
+ * \param index the index of the point to return
+ * \param array the vector containing the result
+ * \param start_channel the first channel to start copying data from
+ * \param end_channel the last channel to stop copying data at
+ */
+ inline void
+ getPointAsFloatArray (std_msgs::PointCloud points, int index, std::vector<float> &array, std::vector<int> channels)
+ {
+ if (channels.size () > points.get_chan_size ())
+ return;
+ // Resize for XYZ (3) + NR_CHANNELS
+ array.resize (3 + channels.size ());
+ array[0] = points.pts[index].x;
+ array[1] = points.pts[index].y;
+ array[2] = points.pts[index].z;
+
+ for (unsigned int d = 0; d < channels.size (); d++)
+ array[3 + d] = points.chan[channels.at (d)].vals[index];
+ }
+
+ void getMinMax (std_msgs::PointCloud points, std_msgs::Point32 &min_pt, std_msgs::Point32 &max_pt);
+ void getMinMax (std_msgs::PointCloud points, std::vector<int> indices, std_msgs::Point32 &min_pt, std_msgs::Point32 &max_pt);
+
+ std_msgs::Point32 computeMedian (std_msgs::PointCloud points);
+ std_msgs::Point32 computeMedian (std_msgs::PointCloud points, std::vector<int> indices);
+
+ double computeMedianAbsoluteDeviation (std_msgs::PointCloud points, double sigma);
+ double computeMedianAbsoluteDeviation (std_msgs::PointCloud points, std::vector<int> indices, double sigma);
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the cross product between two points (vectors).
+ * \param p1 the first point/vector
+ * \param p2 the second point/vector
+ */
+ inline std_msgs::Point32
+ cross (std_msgs::Point32 p1, std_msgs::Point32 p2)
+ {
+ std_msgs::Point32 r;
+ r.x = p1.y*p2.z - p1.z*p2.y;
+ r.y = p1.z*p2.x - p1.x*p2.z;
+ r.z = p1.x*p2.y - p1.y*p2.x;
+ return (r);
+ }
+
+
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/mainpage.dox
===================================================================
--- pkg/trunk/mapping/cloud_geometry/mainpage.dox (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/mainpage.dox 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,11 @@
+
+/** \mainpage
+
+@htmlinclude manifest.html
+
+@b cloud_geometry is a general library for 3D geometric methods
+operating on point clouds.
+
+@todo write documentation and gtests.
+
+*/
Added: pkg/trunk/mapping/cloud_geometry/manifest.xml
===================================================================
--- pkg/trunk/mapping/cloud_geometry/manifest.xml (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/manifest.xml 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,20 @@
+<package>
+ <description>
+ A library for point cloud geometry.
+ </description>
+
+ <author>Radu Bogdan Rusu (ru...@cs...)</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com</url>
+ <review status="unreviewed" notes="beta"/>
+
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="eigen" />
+
+ <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcloud_geometry"/>
+ </export>
+</package>
Added: pkg/trunk/mapping/cloud_geometry/src/lapack.cpp
===================================================================
--- pkg/trunk/mapping/cloud_geometry/src/lapack.cpp (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/src/lapack.cpp 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: lapack.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#include "cloud_geometry/lapack.h"
+
+namespace cloud_geometry
+{
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief compute the 3 eigen values and eigenvectors for a 3x3 covariance matrix
+ * \param covariance_matrix a 3x3 covariance matrix in eigen2::matrix3d format
+ * \param eigen_values the resulted eigenvalues in eigen2::vector3d
+ * \param eigen_vectors a 3x3 matrix in eigen2::matrix3d format, containing each eigenvector on a new line
+ */
+ bool
+ eigen_cov (Eigen::Matrix3d covariance_matrix, Eigen::Vector3d &eigen_values, Eigen::Matrix3d &eigen_vectors)
+ {
+ char jobz = 'V'; // 'V': Compute eigenvalues and eigenvectors
+ char uplo = 'U'; // 'U': Upper triangle of A is stored
+
+ int n = 3, lda = 3, info = -1;
+ int lwork = 3 * n - 1;
+
+ double *work = new double[lwork];
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ eigen_vectors (i, j) = covariance_matrix (i, j);
+
+ dsyev_ (&jobz, &uplo, &n, eigen_vectors.data (), &lda, eigen_values.data (), work, &lwork, &info);
+
+ delete work;
+
+ return (info == 0);
+ }
+}
+
Added: pkg/trunk/mapping/cloud_geometry/src/nearest.cpp
===================================================================
--- pkg/trunk/mapping/cloud_geometry/src/nearest.cpp (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/src/nearest.cpp 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: nearest.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#include "cloud_geometry/nearest.h"
+
+namespace cloud_geometry
+{
+
+ namespace nearest
+ {
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the centroid of a set of points and return it as a PointCloud message with 1 value.
+ * \param points the input point cloud
+ * \param centroid the output centroid
+ */
+ void
+ computeCentroid (std_msgs::PointCloud points, std_msgs::PointCloud ¢roid)
+ {
+ // Prepare the data output
+ centroid.pts.resize (1);
+ centroid.chan.resize (points.get_chan_size ());
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ {
+ centroid.chan[d].name = points.chan[d].name;
+ centroid.chan[d].vals.resize (1);
+ }
+
+ // For each point in the cloud
+ for (unsigned int i = 0; i < points.get_pts_size (); i++)
+ {
+ centroid.pts[0].x += points.pts[i].x;
+ centroid.pts[0].y += points.pts[i].y;
+ centroid.pts[0].z += points.pts[i].z;
+
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ centroid.chan[d].vals[0] += points.chan[d].vals[i];
+ }
+
+ centroid.pts[0].x /= points.get_pts_size ();
+ centroid.pts[0].y /= points.get_pts_size ();
+ centroid.pts[0].z /= points.get_pts_size ();
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ centroid.chan[d].vals[0] /= points.get_pts_size ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the centroid of a set of points using their indices and return it as a PointCloud message with 1 value.
+ * \param points the input point cloud
+ * \param indices the point cloud indices that need to be used
+ * \param centroid the output centroid
+ */
+ void
+ computeCentroid (std_msgs::PointCloud points, std::vector<int> indices, std_msgs::PointCloud ¢roid)
+ {
+ // Prepare the data output
+ centroid.pts.resize (1);
+ centroid.chan.resize (points.get_chan_size ());
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ {
+ centroid.chan[d].name = points.chan[d].name;
+ centroid.chan[d].vals.resize (1);
+ }
+
+ // For each point in the cloud
+ for (unsigned int i = 0; i < indices.size (); i++)
+ {
+ centroid.pts[0].x += points.pts.at (indices.at (i)).x;
+ centroid.pts[0].y += points.pts.at (indices.at (i)).y;
+ centroid.pts[0].z += points.pts.at (indices.at (i)).z;
+
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ centroid.chan[d].vals[0] += points.chan[d].vals[i];
+ }
+
+ centroid.pts[0].x /= indices.size ();
+ centroid.pts[0].y /= indices.size ();
+ centroid.pts[0].z /= indices.size ();
+ for (unsigned int d = 0; d < points.get_chan_size (); d++)
+ centroid.chan[d].vals[0] /= indices.size ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the 3x3 covariance matrix of a given set of points. The result is returned as a Eigen::Matrix3d.
+ * \note The (x-y-z) centroid is also returned as a Point32 message.
+ * \param points the input point cloud
+ * \param covariance_matrix the 3x3 covariance matrix
+ * \param centroid the computed centroid
+ */
+ void
+ computeCovarianceMatrix (std_msgs::PointCloud points, Eigen::Matrix3d &covariance_matrix, std_msgs::Point32 ¢roid)
+ {
+ computeCentroid (points, centroid);
+
+ // Initialize to 0
+ covariance_matrix = Eigen::Matrix3d::Zero ();
+
+ // Sum of outer products
+ // covariance_matrix (k, i) += points_c (j, k) * points_c (j, i);
+ for (unsigned int j = 0; j < points.pts.size (); j++)
+ {
+ covariance_matrix (0, 0) += (points.pts[j].x - centroid.x) * (points.pts[j].x - centroid.x);
+ covariance_matrix (0, 1) += (points.pts[j].x - centroid.x) * (points.pts[j].y - centroid.y);
+ covariance_matrix (0, 2) += (points.pts[j].x - centroid.x) * (points.pts[j].z - centroid.z);
+
+ covariance_matrix (1, 0) += (points.pts[j].y - centroid.y) * (points.pts[j].x - centroid.x);
+ covariance_matrix (1, 1) += (points.pts[j].y - centroid.y) * (points.pts[j].y - centroid.y);
+ covariance_matrix (1, 2) += (points.pts[j].y - centroid.y) * (points.pts[j].z - centroid.z);
+
+ covariance_matrix (2, 0) += (points.pts[j].z - centroid.z) * (points.pts[j].x - centroid.x);
+ covariance_matrix (2, 1) += (points.pts[j].z - centroid.z) * (points.pts[j].y - centroid.y);
+ covariance_matrix (2, 2) += (points.pts[j].z - centroid.z) * (points.pts[j].z - centroid.z);
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the 3x3 covariance matrix of a given set of points. The result is returned as a Eigen::Matrix3d.
+ * \param points the input point cloud
+ * \param covariance_matrix the 3x3 covariance matrix
+ */
+ void
+ computeCovarianceMatrix (std_msgs::PointCloud points, Eigen::Matrix3d &covariance_matrix)
+ {
+ std_msgs::Point32 centroid;
+ computeCovarianceMatrix (points, covariance_matrix, centroid);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
+ * The result is returned as a Eigen::Matrix3d.
+ * \note The (x-y-z) centroid is also returned as a Point32 message.
+ * \param points the input point cloud
+ * \param indices the point cloud indices that need to be used
+ * \param covariance_matrix the 3x3 covariance matrix
+ * \param centroid the computed centroid
+ */
+ void
+ computeCovarianceMatrix (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Matrix3d &covariance_matrix, std_msgs::Point32 ¢roid)
+ {
+ computeCentroid (points, indices, centroid);
+
+ // Initialize to 0
+ covariance_matrix = Eigen::Matrix3d::Zero ();
+
+ for (unsigned int j = 0; j < indices.size (); j++)
+ {
+ covariance_matrix (0, 0) += (points.pts[indices.at (j)].x - centroid.x) * (points.pts[indices.at (j)].x - centroid.x);
+ covariance_matrix (0, 1) += (points.pts[indices.at (j)].x - centroid.x) * (points.pts[indices.at (j)].y - centroid.y);
+ covariance_matrix (0, 2) += (points.pts[indices.at (j)].x - centroid.x) * (points.pts[indices.at (j)].z - centroid.z);
+
+ covariance_matrix (1, 0) += (points.pts[indices.at (j)].y - centroid.y) * (points.pts[indices.at (j)].x - centroid.x);
+ covariance_matrix (1, 1) += (points.pts[indices.at (j)].y - centroid.y) * (points.pts[indices.at (j)].y - centroid.y);
+ covariance_matrix (1, 2) += (points.pts[indices.at (j)].y - centroid.y) * (points.pts[indices.at (j)].z - centroid.z);
+
+ covariance_matrix (2, 0) += (points.pts[indices.at (j)].z - centroid.z) * (points.pts[indices.at (j)].x - centroid.x);
+ covariance_matrix (2, 1) += (points.pts[indices.at (j)].z - centroid.z) * (points.pts[indices.at (j)].y - centroid.y);
+ covariance_matrix (2, 2) += (points.pts[indices.at (j)].z - centroid.z) * (points.pts[indices.at (j)].z - centroid.z);
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
+ * The result is returned as a Eigen::Matrix3d.
+ * \param points the input point cloud
+ * \param indices the point cloud indices that need to be used
+ * \param covariance_matrix the 3x3 covariance matrix
+ */
+ void
+ computeCovarianceMatrix (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Matrix3d &covariance_matrix)
+ {
+ std_msgs::Point32 centroid;
+ computeCovarianceMatrix (points, indices, covariance_matrix, centroid);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the Least-Squares plane fit for a given set of points, and return the estimated plane parameters
+ * together with the surface curvature.
+ * \param points the input point cloud
+ * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
+ * \param curvature the estimated surface curvature as a measure of
+ * \f[
+ * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
+ * \f]
+ */
+ void
+ computeSurfaceNormalCurvature (std_msgs::PointCloud points, Eigen::Vector4d &plane_parameters, double &curvature)
+ {
+ std_msgs::Point32 centroid;
+ // Compute the 3x3 covariance matrix
+ Eigen::Matrix3d covariance_matrix;
+ computeCovarianceMatrix (points, covariance_matrix, centroid);
+
+ // Extract the eigenvalues and eigenvectors
+ Eigen::Vector3d eigen_values;
+ Eigen::Matrix3d eigen_vectors;
+ eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
+
+ // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
+ double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
+ eigen_vectors (0, 1) * eigen_vectors (0, 1) +
+ eigen_vectors (0, 2) * eigen_vectors (0, 2));
+ plane_parameters (0) = eigen_vectors (0, 0) / norm;
+ plane_parameters (1) = eigen_vectors (0, 1) / norm;
+ plane_parameters (2) = eigen_vectors (0, 2) / norm;
+
+ // Hessian form (D = nc . p_plane (centroid here) + p)
+ plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters[1] * centroid.y + plane_parameters[2] * centroid.z);
+
+ // Compute the curvature surface change
+ curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
+ * and return the estimated plane parameters together with the surface curvature.
+ * \param points the input point cloud
+ * \param indices the point cloud indices that need to be used
+ * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
+ * \param curvature the estimated surface curvature as a measure of
+ * \f[
+ * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
+ * \f]
+ */
+ void
+ computeSurfaceNormalCurvature (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Vector4d &plane_parameters, double &curvature)
+ {
+ std_msgs::Point32 centroid;
+ // Compute the 3x3 covariance matrix
+ Eigen::Matrix3d covariance_matrix;
+ computeCovarianceMatrix (points, indices, covariance_matrix, centroid);
+
+ // Extract the eigenvalues and eigenvectors
+ Eigen::Vector3d eigen_values;
+ Eigen::Matrix3d eigen_vectors;
+ eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
+
+ // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
+ // Note: Remember to take care of the eigen_vectors ordering ! Check lapack.cpp
+ double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
+ eigen_vectors (1, 0) * eigen_vectors (1, 0) +
+ eigen_vectors (2, 0) * eigen_vectors (2, 0));
+ plane_parameters (0) = eigen_vectors (0, 0) / norm;
+ plane_parameters (1) = eigen_vectors (1, 0) / norm;
+ plane_parameters (2) = eigen_vectors (2, 0) / norm;
+
+ // Hessian form (D = nc . p_plane (centroid here) + p)
+ plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters[1] * centroid.y + plane_parameters[2] * centroid.z);
+
+ // Compute the curvature surface change
+ curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
+ }
+
+ }
+}
Added: pkg/trunk/mapping/cloud_geometry/src/point.cpp
===================================================================
--- pkg/trunk/mapping/cloud_geometry/src/point.cpp (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/src/point.cpp 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,237 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: point.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#include <cfloat>
+#include "cloud_geometry/point.h"
+
+namespace cloud_geometry
+{
+
+ /////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the median value of a 3D point cloud and return it as a Point32.
+ * \param points the point cloud data message
+ */
+ std_msgs::Point32
+ computeMedian (std_msgs::PointCloud points)
+ {
+ std_msgs::Point32 median;
+
+ // Copy the values to vectors for faster sorting
+ std::vector<double> x (points.pts.size ());
+ std::vector<double> y (points.pts.size ());
+ std::vector<double> z (points.pts.size ());
+ for (unsigned int i = 0; i < points.pts.size (); i++)
+ {
+ x[i] = points.pts[i].x;
+ y[i] = points.pts[i].y;
+ z[i] = points.pts[i].z;
+ }
+ sort (x.begin (), x.end ());
+ sort (y.begin (), y.end ());
+ sort (z.begin (), z.end ());
+
+ int mid = points.pts.size () / 2;
+ if (points.pts.size () % 2 == 0)
+ {
+ median.x = (x[mid-1] + x[mid]) / 2;
+ median.y = (y[mid-1] + y[mid]) / 2;
+ median.z = (z[mid-1] + z[mid]) / 2;
+ }
+ else
+ {
+ median.x = x[mid];
+ median.y = y[mid];
+ median.z = z[mid];
+ }
+ return (median);
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the median value of a 3D point cloud using a given set point
+ * indices and return it as a Point32.
+ * \param points the point cloud data message
+ * \param indices the point indices
+ */
+ std_msgs::Point32
+ computeMedian (std_msgs::PointCloud points, std::vector<int> indices)
+ {
+ std_msgs::Point32 median;
+
+ // Copy the values to vectors for faster sorting
+ std::vector<double> x (indices.size ());
+ std::vector<double> y (indices.size ());
+ std::vector<double> z (indices.size ());
+ for (unsigned int i = 0; i < indices.size (); i++)
+ {
+ x[i] = points.pts.at (indices.at (i)).x;
+ y[i] = points.pts.at (indices.at (i)).y;
+ z[i] = points.pts.at (indices.at (i)).z;
+ }
+ sort (x.begin (), x.end ());
+ sort (y.begin (), y.end ());
+ sort (z.begin (), z.end ());
+
+ int mid = indices.size () / 2;
+ if (indices.size () % 2 == 0)
+ {
+ median.x = (x[mid-1] + x[mid]) / 2;
+ median.y = (y[mid-1] + y[mid]) / 2;
+ median.z = (z[mid-1] + z[mid]) / 2;
+ }
+ else
+ {
+ median.x = x[mid];
+ median.y = y[mid];
+ median.z = z[mid];
+ }
+ return (median);
+ }
+
+
+ /////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the median absolute deviation:
+ * \f[
+ * MAD = \sigma * median_i (| Xi - median_j(Xj) |)
+ * \f]
+ * \note Sigma needs to be chosen carefully (a good starting sigma value is 1.4826)
+ * \param points the point cloud data message
+ * \param sigma the sigma value
+ */
+ double
+ computeMedianAbsoluteDeviation (std_msgs::PointCloud points, double sigma)
+ {
+ // median (dist (x - median (x)))
+ std_msgs::Point32 median = cloud_geometry::computeMedian (points);
+
+ std::vector<double> distances (points.pts.size ());
+
+ for (unsigned int i = 0; i < points.pts.size (); i++)
+ distances[i] = (points.pts[i].x - median.x) * (points.pts[i].x - median.x) +
+ (points.pts[i].y - median.y) * (points.pts[i].y - median.y) +
+ (points.pts[i].z - median.z) * (points.pts[i].z - median.z);
+
+ sort (distances.begin (), distances.end ());
+
+ double result;
+ int mid = points.pts.size () / 2;
+ // Do we have a "middle" point or should we "estimate" one ?
+ if (points.pts.size () % 2 == 0)
+ result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
+ else
+ result = sqrt (distances[mid]);
+ return (sigma * result);
+ }
+
+
+ /////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the median absolute deviation:
+ * \f[
+ * MAD = \sigma * median_i (| Xi - median_j(Xj) |)
+ * \f]
+ * \note Sigma needs to be chosen carefully (a good starting sigma value is 1.4826)
+ * \param points the point cloud data message
+ * \param sigma the sigma value
+ ...
[truncated message content] |
