Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13239] pkg/trunk
|
From: <ve...@us...> - 2009-04-01 18:05:58
|
Revision: 13239
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13239&view=rev
Author: veedee
Date: 2009-04-01 18:05:46 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
towards a faster, non-linear, Banach space :)
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_sphere.h
pkg/trunk/mapping/point_cloud_mapping/normal_estimation.launch
pkg/trunk/mapping/point_cloud_mapping/src/planar_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_circle.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_cylinder.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_line.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_parallel_lines.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_plane.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_sphere.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_circle_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_line_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_plane_fit.cpp
pkg/trunk/mapping/semantic_point_annotator/src/sac_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/sac_inc_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
Modified: pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -591,8 +591,8 @@
coeff.resize (0);
return (false);
}
- sac->computeCoefficients (); // Compute the model coefficients
- coeff = sac->refineCoefficients (); // Refine them using least-squares
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
model->selectWithinDistance (coeff, dist_thresh, inliers);
//inliers = sac->getInliers ();
@@ -809,10 +809,11 @@
model->setAxis (axis);
model->setEpsAngle (eps_angle);
+ vector<double> coeff;
// Search for the best line
if (sac->computeModel ())
{
- sac->computeCoefficients (); // Compute the model coefficients
+ sac->computeCoefficients (coeff); // Compute the model coefficients
//line_inliers = model->selectWithinDistance (sac->refineCoefficients (), dist_thresh);
line_inliers = sac->getInliers ();
}
@@ -855,9 +856,10 @@
model->setEpsAngle (eps_angle);
// Search for the best line
+ vector<double> coeff;
if (sac->computeModel ())
{
- sac->computeCoefficients (); // Compute the model coefficients
+ sac->computeCoefficients (coeff); // Compute the model coefficients
line_inliers = sac->getInliers ();
}
else
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -301,7 +301,9 @@
if((int) sac->getInliers().size() < sac_min_points_per_model_)
break;
inliers.push_back(sac->getInliers());
- coeff.push_back(sac->computeCoefficients());
+ vector<double> model_coefficients;
+ sac->computeCoefficients (model_coefficients);
+ coeff.push_back (model_coefficients);
//Find the edges of the line segments
cloud_geometry::statistics::getMinMax (*points, inliers.back(), minP, maxP);
Modified: pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
===================================================================
--- pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -377,7 +377,9 @@
if((int) sac->getInliers().size() < sac_min_points_per_model_)
break;
inliers.push_back(sac->getInliers());
- coeff.push_back(sac->computeCoefficients());
+ vector<double> model_coeff;
+ sac->computeCoefficients (model_coeff);
+ coeff.push_back (model_coeff);
//Find the edges of the line segments
cloud_geometry::statistics::getMinMax (*points, inliers.back(), minP, maxP);
Modified: pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
===================================================================
--- pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -227,7 +227,7 @@
// Now find the best fit parallel lines to this set of points and a corresponding set of inliers
if (sac->computeModel()) {
inliers = sac->getInliers();
- coeffs = sac->computeCoefficients();
+ sac->computeCoefficients (coeffs);
visualization(coeffs, inliers);
// Publish the result
robot_msgs::PointCloud model_cloud;
Modified: pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
===================================================================
--- pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -116,21 +116,25 @@
void
fitSACPlane (PointCloud *points, cloud_octree::Octree *octree, cloud_octree::Leaf* leaf, Polygon3D &poly)
{
+ double dist_thresh = 0.02;
vector<int> indices = leaf->getIndices ();
if ((int)indices.size () < sac_min_points_per_cell_)
return;
// Create and initialize the SAC model
sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
- sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, 0.02);
+ sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
model->setDataSet (points, indices);
+ vector<int> inliers;
+ vector<double> coeff;
// Search for the best plane
if (sac->computeModel ())
{
// Obtain the inliers and the planar model coefficients
- std::vector<int> inliers = sac->getInliers ();
- std::vector<double> coeff = sac->computeCoefficients ();
+ sac->computeCoefficients (coeff);
+ sac->refineCoefficients (coeff);
+ model->selectWithinDistance (coeff, dist_thresh, inliers);
///fprintf (stderr, "> Found a model supported by %d inliers: [%g, %g, %g, %g]\n", inliers.size (), coeff[0], coeff[1], coeff[2], coeff[3]);
// Project the inliers onto the model
Modified: pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
===================================================================
--- pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -116,21 +116,25 @@
void
fitSACPlane (PointCloud *points, cloud_octree::Octree *octree, cloud_octree::Leaf* leaf, Polygon3D &poly)
{
+ double dist_thresh = 0.05;
vector<int> indices = leaf->getIndices ();
if ((int)indices.size () < sac_min_points_per_cell_)
return;
// Create and initialize the SAC model
sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
- sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, 0.05);
+ sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
model->setDataSet (points, indices);
+ vector<int> inliers;
+ vector<double> coeff;
// Search for the best plane
if (sac->computeModel ())
{
// Obtain the inliers and the planar model coefficients
- vector<int> inliers = sac->getInliers ();
- vector<double> coeff = sac->computeCoefficients ();
+ sac->computeCoefficients (coeff);
+ sac->refineCoefficients (coeff);
+ model->selectWithinDistance (coeff, dist_thresh, inliers);
///fprintf (stderr, "> Found a model supported by %d inliers: [%g, %g, %g, %g]\n", inliers.size (), coeff[0], coeff[1], coeff[2], coeff[3]);
// Project the inliers onto the model
Modified: pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h
===================================================================
--- pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -212,7 +212,7 @@
// Find the base plane
vector<int> inliers;
vector<double> coeff;
- fitSACPlane (cloud_tr_, indices_in_bounds, inliers, coeff, &viewpoint_cloud_, sac_distance_threshold_, 100);
+ fitSACPlane (cloud_tr_, indices_in_bounds, inliers, coeff, viewpoint_cloud_, sac_distance_threshold_, 100);
// Remove points below the plane
for (unsigned int i = 0; i < indices_in_bounds.size (); i++)
@@ -387,7 +387,7 @@
*/
int
fitSACPlane (PointCloud &points, vector<int> indices, vector<int> &inliers, vector<double> &coeff,
- robot_msgs::PointStamped *viewpoint_cloud, double dist_thresh, int min_pts)
+ const robot_msgs::PointStamped &viewpoint_cloud, double dist_thresh, int min_pts)
{
if ((int)indices.size () < min_pts)
{
@@ -413,27 +413,12 @@
coeff.resize (0);
return (-1);
}
- sac->computeCoefficients (); // Compute the model coefficients
- coeff = sac->refineCoefficients (); // Refine them using least-squares
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
model->selectWithinDistance (coeff, dist_thresh, inliers);
// Flip plane normal according to the viewpoint information
- Point32 vp_m;
- vp_m.x = viewpoint_cloud->point.x - points.pts.at (inliers[0]).x;
- vp_m.y = viewpoint_cloud->point.y - points.pts.at (inliers[0]).y;
- vp_m.z = viewpoint_cloud->point.z - points.pts.at (inliers[0]).z;
-
- // Dot product between the (viewpoint - point) and the plane normal
- double cos_theta = (vp_m.x * coeff[0] + vp_m.y * coeff[1] + vp_m.z * coeff[2]);
-
- // Flip the plane normal
- if (cos_theta < 0)
- {
- for (int d = 0; d < 3; d++)
- coeff[d] *= -1;
- // Hessian form (D = nc . p_plane (centroid here) + p)
- coeff[3] = -1 * (coeff[0] * points.pts.at (inliers[0]).x + coeff[1] * points.pts.at (inliers[0]).y + coeff[2] * points.pts.at (inliers[0]).z);
- }
+ cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points.pts.at (inliers[0]), viewpoint_cloud);
//ROS_INFO ("Found a model supported by %d inliers: [%g, %g, %g, %g]\n", sac->getInliers ().size (),
// coeff[0], coeff[1], coeff[2], coeff[3]);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -101,19 +101,21 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Compute the coefficients of the model and return them. */
- virtual std::vector<double>
- computeCoefficients ()
+ virtual void
+ computeCoefficients (std::vector<double> &coefficients)
{
sac_model_->computeModelCoefficients (sac_model_->getBestModel ());
- return (sac_model_->getModelCoefficients ());
+ coefficients = sac_model_->getModelCoefficients ();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Use Least-Squares optimizations to refine the coefficients of the model, and return them. */
- virtual std::vector<double>
- refineCoefficients ()
+ /** \brief Use Least-Squares optimizations to refine the coefficients of the model, and return them.
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
+ */
+ virtual void
+ refineCoefficients (std::vector<double> &refined_coefficients)
{
- return (sac_model_->refitModel (sac_model_->getBestInliers ()));
+ sac_model_->refitModel (sac_model_->getBestInliers (), refined_coefficients);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -138,11 +140,13 @@
/** \brief Project a set of given points (using their indices) onto the model and return their projections.
* \param indices a set of indices that represent the data that we're interested in
* \param model_coefficients the coefficients of the underlying model
+ * \param projected_points the resultant projected points
*/
- virtual robot_msgs::PointCloud
- projectPointsToModel (std::vector<int> indices, std::vector<double> model_coefficients)
+ virtual void
+ projectPointsToModel (const std::vector<int> &indices, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
- return (sac_model_->projectPoints (indices, model_coefficients));
+ sac_model_->projectPoints (indices, model_coefficients, projected_points);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -61,20 +61,24 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Test whether the given model coefficients are valid given the input point cloud data. Pure virtual.
- * \param model_coefficients the model coefficients that need to be tested */
+ * \param model_coefficients the model coefficients that need to be tested
+ */
virtual bool testModelCoefficients (const std::vector<double> &model_coefficients) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Check whether the given index samples can form a valid model, compute the model coefficients from
- * these samples and store them internally in model_coefficients_. Pure virtual.
- * \param indices the point indices found as possible good candidates for creating a valid model */
+ * these samples and store them internally in model_coefficients_. Pure virtual.
+ * \param indices the point indices found as possible good candidates for creating a valid model
+ */
virtual bool computeModelCoefficients (const std::vector<int> &indices) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Recompute the model coefficients using the given inlier set and return them to the user. Pure virtual.
- * @note: these are the coefficients of the model after refinement (eg. after a least-squares optimization)
- * \param inliers the data inliers found as supporting the model */
- virtual std::vector<double> refitModel (const std::vector<int> &inliers) = 0;
+ * @note: these are the coefficients of the model after refinement (eg. after a least-squares optimization)
+ * \param inliers the data inliers found as supporting the model
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
+ */
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Compute all distances from the cloud data to a given model. Pure virtual.
@@ -95,20 +99,24 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
- * \param inliers the data inliers that we want to project on the model
- * \param model_coefficients the coefficients of a model */
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients) = 0;
+ * \param inliers the data inliers that we want to project on the model
+ * \param model_coefficients the coefficients of a model
+ * \param projected_points the resultant projected points
+ */
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Project inliers (in place) onto the given model. Pure virtual.
- * \param inliers the data inliers that we want to project on the model
- * \param model_coefficients the coefficients of a model */
+ * \param inliers the data inliers that we want to project on the model
+ * \param model_coefficients the coefficients of a model
+ */
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients) = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Verify whether a subset of indices verifies the internal model coefficients. Pure virtual.
- * \param indices the data indices that need to be tested against the model
- * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers */
+ * \param indices the data indices that need to be tested against the model
+ * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
+ */
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold) = 0;
@@ -150,7 +158,7 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Set the best set of inliers. Used by SAC methods. Do not call this except if you know what you're doing.
* \param best_inliers the set of inliers for the best model */
- void setBestInliers (std::vector<int> best_inliers) { this->best_inliers_ = best_inliers; }
+ void setBestInliers (const std::vector<int> &best_inliers) { this->best_inliers_ = best_inliers; }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return the best set of inliers found so far for this model. */
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_circle.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ * Copyright (c) 2008-2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
*
* All rights reserved.
*
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
+ static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return an unique id for this model (SACMODEL_CIRCLE2D). */
virtual int getModelType () { return (SACMODEL_CIRCLE2D); }
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_cylinder.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
+ static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return an unique id for this model (SACMODEL_CYLINDER). */
virtual int getModelType () { return (SACMODEL_CYLINDER); }
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_line.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,11 +65,11 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_parallel_lines.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -71,11 +71,11 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_plane.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
+ static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return an unique id for this model (SACMODEL_PLANE). */
virtual int getModelType () { return (SACMODEL_PLANE); }
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_sphere.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_sphere.h 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/sample_consensus/sac_model_sphere.h 2009-04-01 18:05:46 UTC (rev 13239)
@@ -65,15 +65,17 @@
virtual bool computeModelCoefficients (const std::vector<int> &indices);
- virtual std::vector<double> refitModel (const std::vector<int> &inliers);
+ virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
- virtual robot_msgs::PointCloud projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
+ virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, robot_msgs::PointCloud &projected_points);
virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
+ static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Return an unique id for this model (SACMODEL_SPHERE). */
virtual int getModelType () { return (SACMODEL_SPHERE); }
Modified: pkg/trunk/mapping/point_cloud_mapping/normal_estimation.launch
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/normal_estimation.launch 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/normal_estimation.launch 2009-04-01 18:05:46 UTC (rev 13239)
@@ -1,6 +1,8 @@
<launch>
- <param name="/normal_estimation/downsample" value="0" />
+ <param name="/normal_estimation/downsample" value="1" />
<param name="/normal_estimation/cut_distance" value="15" />
- <node pkg="point_cloud_mapping" type="normal_estimation_node" name="normal_estimation" output="screen" respawn="true" />
+ <node pkg="point_cloud_mapping" type="normal_estimation_node" name="normal_estimation" output="screen" respawn="true">
+ <remap from="tilt_laser_cloud" to="cloud_pcd" />
+ </node>
</launch>
Modified: pkg/trunk/mapping/point_cloud_mapping/src/planar_fit.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/planar_fit.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/planar_fit.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -377,8 +377,8 @@
// Search for the best plane
if (sac->computeModel (0))
{
- sac->computeCoefficients (); // Compute the model coefficients
- coeff = sac->refineCoefficients (); // Refine them using least-squares
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
model->selectWithinDistance (coeff, dist_thresh, inliers);
cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points->pts.at (inliers[0]), viewpoint_cloud);
@@ -430,8 +430,8 @@
// Search for the best plane
if (sac->computeModel (0))
{
- sac->computeCoefficients (); // Compute the model coefficients
- coeff = sac->refineCoefficients (); // Refine them using least-squares
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
model->selectWithinDistance (coeff, dist_thresh, inliers);
cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points->pts.at (inliers[0]), viewpoint_cloud);
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ * Copyright (c) 2008-2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
*
* All rights reserved.
*
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_circle.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_circle.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_circle.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -33,6 +33,9 @@
#include <point_cloud_mapping/sample_consensus/sac_model_circle.h>
#include <point_cloud_mapping/geometry/nearest.h>
+#include <boost/bind.hpp>
+#include <cminpack.h>
+
namespace sample_consensus
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -166,13 +169,15 @@
/** \brief Create a new point cloud with inliers projected onto the 2D circle model.
* \param inliers the data inliers that we want to project on the 2D circle model
* \param model_coefficients the coefficients of a 2D circle model
+ * \param projected_points the resultant projected points
* \todo implement this.
*/
- robot_msgs::PointCloud
- SACModelCircle2D::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
+ void
+ SACModelCircle2D::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
std::cerr << "[SACModelCircle2D::projecPoints] Not implemented yet." << std::endl;
- return (*cloud_);
+ projected_points = *cloud_;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -228,23 +233,82 @@
/** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the circle model after refinement (eg. after SVD)
* \param inliers the data inliers found as supporting the model
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
*/
- std::vector<double>
- SACModelCircle2D::refitModel (const std::vector<int> &inliers)
+ void
+ SACModelCircle2D::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
{
- // std::cerr << "[SACModelCircle2D::RefitModel] Cannot re-fit 0 inliers!" << std::endl;
- return (model_coefficients_);
+ ROS_ERROR ("[SACModelCircle2D::RefitModel] Cannot re-fit 0 inliers!");
+ refit_coefficients = model_coefficients_;
+ return;
}
+ if (model_coefficients_.size () == 0)
+ {
+ ROS_WARN ("[SACModelCircle2D::RefitModel] Initial model coefficients have not been estimated yet - proceeding without an initial solution!");
+ best_inliers_ = indices_;
+ }
- std::vector<double> refit (4);
-// for (int d = 0; d < 4; d++)
-// refit[d] = plane_coefficients (d);
+ int m = inliers.size ();
- return (refit);
+ double *fvec = new double[m];
+
+ int n = 3; // 3 unknowns
+ int iwa[n];
+
+ int lwa = m * n + 5 * n + m;
+ double *wa = new double[lwa];
+
+ // Set the initial solution
+ double x[3] = {0.0, 0.0, 0.0};
+ if (model_coefficients_.size () == 3)
+ for (int d = 0; d < 3; d++)
+ x[d] = model_coefficients_.at (d);
+
+ // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
+ double tol = sqrt (dpmpar (1));
+
+ // Optimize using forward-difference approximation LM
+ int info = lmdif1 (&sample_consensus::SACModelCircle2D::functionToOptimize, this, m, n, x, fvec, tol, iwa, wa, lwa);
+
+ // Compute the L2 norm of the residuals
+ ROS_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. Initial solution: %g %g %g. Final solution: %g %g %g",
+ info, enorm (m, fvec), model_coefficients_.at (0), model_coefficients_.at (1), model_coefficients_.at (2), x[0], x[1], x[2]);
+
+ refit_coefficients.resize (3);
+ for (int d = 0; d < 3; d++)
+ refit_coefficients[d] = x[d];
+
+ free (wa); free (fvec);
}
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////(
+ /** \brief Cost function to be minimized
+ * \param p a pointer to our data structure array
+ * \param m the number of functions
+ * \param n the number of variables
+ * \param x a pointer to the variables array
+ * \param fvec a pointer to the resultant functions evaluations
+ * \param iflag set to -1 inside the function to terminate execution
+ */
+ int
+ SACModelCircle2D::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
+ {
+ SACModelCircle2D *model = (SACModelCircle2D*)p;
+
+ for (int i = 0; i < n; i ++)
+ {
+ // Compute the difference between the center of the circle and the datapoint X_i
+ double xt = model->cloud_->pts[model->best_inliers_.at (i)].x - x[0];
+ double yt = model->cloud_->pts[model->best_inliers_.at (i)].y - x[1];
+
+ // g = sqrt ((x-a)^2 + (y-b)^2) - R
+ fvec[i] = sqrt (xt * xt + yt * yt) - x[2];
+ }
+ return (0);
+ }
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Verify whether a subset of indices verifies the internal circle model coefficients.
* \param indices the data indices that need to be tested against the circle model
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_cylinder.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_cylinder.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_cylinder.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -169,13 +169,15 @@
/** \brief Create a new point cloud with inliers projected onto the cylinder model.
* \param inliers the data inliers that we want to project on the cylinder model
* \param model_coefficients the coefficients of a cylinder model
+ * \param projected_points the resultant projected points
* \todo implement this.
*/
- robot_msgs::PointCloud
- SACModelCylinder::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
+ void
+ SACModelCylinder::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
std::cerr << "[SACModelCylinder::projecPoints] Not implemented yet." << std::endl;
- return (*cloud_);
+ projected_points = *cloud_;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -277,23 +279,47 @@
/** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the cylinder model after refinement (eg. after SVD)
* \param inliers the data inliers found as supporting the model
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
*/
- std::vector<double>
- SACModelCylinder::refitModel (const std::vector<int> &inliers)
+ void
+ SACModelCylinder::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
{
- // std::cerr << "[SACModelCylinder::RefitModel] Cannot re-fit 0 inliers!" << std::endl;
- return (model_coefficients_);
+ ROS_ERROR ("[SACModelCylinder::RefitModel] Cannot re-fit 0 inliers!");
+ refit_coefficients = model_coefficients_;
+ return;
}
-/*
+ refit_coefficients.resize (7);
+ for (int d = 0; d < 7; d++)
+ refit_coefficients[d] = model_coefficients_[d];
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////(
+ /** \brief Cost function to be minimized
+ * \param p a pointer to our data structure array
+ * \param m the number of functions
+ * \param n the number of variables
+ * \param x a pointer to the variables array
+ * \param fvec a pointer to the resultant functions evaluations
+ * \param iflag set to -1 inside the function to terminate execution
*/
- std::vector<double> refit (7);
- for (int d = 0; d < 4; d++)
- refit[d] = model_coefficients_[d];
+ int
+ SACModelCylinder::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
+ {
+ SACModelCylinder *model = (SACModelCylinder*)p;
- return (refit);
+ for (int i = 0; i < n; i ++)
+ {
+ // Compute the difference between the center of the circle and the datapoint X_i
+ double xt = model->cloud_->pts[model->best_inliers_.at (i)].x - x[0];
+ double yt = model->cloud_->pts[model->best_inliers_.at (i)].y - x[1];
+
+ // g = sqrt ((x-a)^2 + (y-b)^2) - R
+ fvec[i] = sqrt (xt * xt + yt * yt) - x[2];
+ }
+ return (0);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_line.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_line.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_line.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -163,21 +163,21 @@
/** \brief Create a new point cloud with inliers projected onto the line model.
* \param inliers the data inliers that we want to project on the line model
* \param model_coefficients the coefficients of a line model
+ * \param projected_points the resultant projected points
*/
- robot_msgs::PointCloud
- SACModelLine::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
+ void
+ SACModelLine::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
- robot_msgs::PointCloud projected_cloud;
// Allocate enough space
- projected_cloud.pts.resize (inliers.size ());
+ projected_points.pts.resize (inliers.size ());
+ projected_points.set_chan_size (cloud_->get_chan_size ());
- projected_cloud.set_chan_size (cloud_->get_chan_size ());
-
// Create the channels
- for (unsigned int d = 0; d < projected_cloud.get_chan_size (); d++)
+ for (unsigned int d = 0; d < projected_points.get_chan_size (); d++)
{
- projected_cloud.chan[d].name = cloud_->chan[d].name;
- projected_cloud.chan[d].vals.resize (inliers.size ());
+ projected_points.chan[d].name = cloud_->chan[d].name;
+ projected_points.chan[d].vals.resize (inliers.size ());
}
// Compute the line direction (P2 - P1)
@@ -194,14 +194,13 @@
(model_coefficients_[0] * p21.x + model_coefficients_[1] * p21.y + model_coefficients_[2] * p21.z)
) / (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z);
// Calculate the projection of the point on the line (pointProj = A + k * B)
- projected_cloud.pts[i].x = model_coefficients_.at (0) + k * p21.x;
- projected_cloud.pts[i].y = model_coefficients_.at (1) + k * p21.y;
- projected_cloud.pts[i].z = model_coefficients_.at (2) + k * p21.z;
+ projected_points.pts[i].x = model_coefficients_.at (0) + k * p21.x;
+ projected_points.pts[i].y = model_coefficients_.at (1) + k * p21.y;
+ projected_points.pts[i].z = model_coefficients_.at (2) + k * p21.z;
// Copy the other attributes
- for (unsigned int d = 0; d < projected_cloud.get_chan_size (); d++)
- projected_cloud.chan[d].vals[i] = cloud_->chan[d].vals[inliers.at (i)];
+ for (unsigned int d = 0; d < projected_points.get_chan_size (); d++)
+ projected_points.chan[d].vals[i] = cloud_->chan[d].vals[inliers.at (i)];
}
- return (projected_cloud);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -256,14 +255,19 @@
/** \brief Recompute the line coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the line model after refinement
* \param inliers the data inliers found as supporting the model
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
*/
- std::vector<double>
- SACModelLine::refitModel (const std::vector<int> &inliers)
+ void
+ SACModelLine::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
- return (model_coefficients_);
+ {
+ ROS_ERROR ("[SACModelLine::RefitModel] Cannot re-fit 0 inliers!");
+ refit_coefficients = model_coefficients_;
+ return;
+ }
- std::vector<double> refit_coefficients (6);
+ refit_coefficients.resize (6);
// Compute the centroid of the samples
robot_msgs::Point32 centroid;
@@ -284,8 +288,6 @@
refit_coefficients[3] = eigen_vectors (0, 2) + refit_coefficients[0];
refit_coefficients[4] = eigen_vectors (1, 2) + refit_coefficients[1];
refit_coefficients[5] = eigen_vectors (2, 2) + refit_coefficients[2];
-
- return (refit_coefficients);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_parallel_lines.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_parallel_lines.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_parallel_lines.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -281,20 +281,21 @@
/** \brief Create a new point cloud with inliers projected onto the line model.
* \param inliers The indices of the inliers into the data cloud.
* \param model_coefficients The coefficients of the parallel lines model.
+ * \param projected_points the resultant projected points
*/
- robot_msgs::PointCloud
- SACModelParallelLines::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
+ void
+ SACModelParallelLines::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
- robot_msgs::PointCloud projected_cloud;
// Allocate enough space
- projected_cloud.pts.resize (inliers.size ());
+ projected_points.pts.resize (inliers.size ());
// Create the channels
- projected_cloud.set_chan_size (cloud_->get_chan_size ());
- for (unsigned int d = 0; d < projected_cloud.get_chan_size (); d++)
+ projected_points.set_chan_size (cloud_->get_chan_size ());
+ for (unsigned int d = 0; d < projected_points.get_chan_size (); d++)
{
- projected_cloud.chan[d].name = cloud_->chan[d].name;
- projected_cloud.chan[d].vals.resize (inliers.size ());
+ projected_points.chan[d].name = cloud_->chan[d].name;
+ projected_points.chan[d].vals.resize (inliers.size ());
}
// Compute the closest distances from the pts to the lines.
@@ -329,15 +330,14 @@
( mx * l1.x + my * l1.y + mz * l1.z )
) / l_sqr_length;
// Calculate the projection of the point on the line (pointProj = A + k * B)
- projected_cloud.pts[i].x = mx + k * l1.x;
- projected_cloud.pts[i].y = my + k * l1.y;
- projected_cloud.pts[i].z = mz + k * l1.z;
+ projected_points.pts[i].x = mx + k * l1.x;
+ projected_points.pts[i].y = my + k * l1.y;
+ projected_points.pts[i].z = mz + k * l1.z;
// Copy the other attributes
- for (unsigned int d = 0; d < projected_cloud.get_chan_size (); d++)
- projected_cloud.chan[d].vals[i] = cloud_->chan[d].vals[inliers[i]];
+ for (unsigned int d = 0; d < projected_points.get_chan_size (); d++)
+ projected_points.chan[d].vals[i] = cloud_->chan[d].vals[inliers[i]];
}
- return (projected_cloud);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -415,19 +415,24 @@
/** \brief Recompute the parallel lines coefficients using the given inlier set and return them to the user.
* @note: These are the coefficients of the line model after refinement.
* \param inliers The data inliers found as supporting the model.
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
*/
- std::vector<double>
- SACModelParallelLines::refitModel (const std::vector<int> &inliers)
+ void
+ SACModelParallelLines::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
- return (model_coefficients_);
+ {
+ ROS_ERROR ("[SACModelParallelLines::RefitModel] Cannot re-fit 0 inliers!");
+ refit_coefficients = model_coefficients_;
+ return;
+ }
// Get the distances from the inliers to their closest line
std::vector<int> closest_line (inliers.size ());
std::vector<double> closest_dist (inliers.size ());
closestLine (inliers, model_coefficients_, &closest_line, &closest_dist);
- std::vector<double> refit_coefficients (9);
+ refit_coefficients.resize (9);
// Compute the centroids of the two sets of samples
robot_msgs::Point32 centroid1, centroid2, centroid;
@@ -487,8 +492,6 @@
refit_coefficients[3] = eigen_vectors(0, 2) + refit_coefficients[0];
refit_coefficients[4] = eigen_vectors(1, 2) + refit_coefficients[1];
refit_coefficients[5] = eigen_vectors(2, 2) + refit_coefficients[2];
-
- return (refit_coefficients);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_plane.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_plane.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_plane.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -160,21 +160,21 @@
/** \brief Create a new point cloud with inliers projected onto the plane model.
* \param inliers the data inliers that we want to project on the plane model
* \param model_coefficients the *normalized* coefficients of a plane model
+ * \param projected_points the resultant projected points
*/
- robot_msgs::PointCloud
- SACModelPlane::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
+ void
+ SACModelPlane::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
- robot_msgs::PointCloud projected_cloud;
// Allocate enough space
- projected_cloud.pts.resize (inliers.size ());
+ projected_points.pts.resize (inliers.size ());
+ projected_points.set_chan_size (cloud_->get_chan_size ());
- projected_cloud.set_chan_size (cloud_->get_chan_size ());
-
// Create the channels
- for (unsigned int d = 0; d < projected_cloud.get_chan_size (); d++)
+ for (unsigned int d = 0; d < projected_points.get_chan_size (); d++)
{
- projected_cloud.chan[d].name = cloud_->chan[d].name;
- projected_cloud.chan[d].vals.resize (inliers.size ());
+ projected_points.chan[d].name = cloud_->chan[d].name;
+ projected_points.chan[d].vals.resize (inliers.size ());
}
// Get the plane normal
@@ -196,14 +196,13 @@
model_coefficients.at (2) * cloud_->pts.at (inliers.at (i)).z +
model_coefficients.at (3) * 1;
// Calculate the projection of the point on the plane
- projected_cloud.pts[i].x = cloud_->pts.at (inliers.at (i)).x - distance_to_plane * model_coefficients.at (0);
- projected_cloud.pts[i].y = cloud_->pts.at (inliers.at (i)).y - distance_to_plane * model_coefficients.at (1);
- projected_cloud.pts[i].z = cloud_->pts.at (inliers.at (i)).z - distance_to_plane * model_coefficients.at (2);
+ projected_points.pts[i].x = cloud_->pts.at (inliers.at (i)).x - distance_to_plane * model_coefficients.at (0);
+ projected_points.pts[i].y = cloud_->pts.at (inliers.at (i)).y - distance_to_plane * model_coefficients.at (1);
+ projected_points.pts[i].z = cloud_->pts.at (inliers.at (i)).z - distance_to_plane * model_coefficients.at (2);
// Copy the other attributes
- for (unsigned int d = 0; d < projected_cloud.get_chan_size (); d++)
- projected_cloud.chan[d].vals[i] = cloud_->chan[d].vals[inliers.at (i)];
+ for (unsigned int d = 0; d < projected_points.get_chan_size (); d++)
+ projected_points.chan[d].vals[i] = cloud_->chan[d].vals[inliers.at (i)];
}
- return (projected_cloud);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -301,14 +300,16 @@
/** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the plane model after refinement (eg. after SVD)
* \param inliers the data inliers found as supporting the model
+ * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
*/
- std::vector<double>
- SACModelPlane::refitModel (const std::vector<int> &inliers)
+ void
+ SACModelPlane::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
{
- // std::cerr << "[SACModelPlanes::RefitModel] Cannot re-fit 0 inliers!" << std::endl;
- return (model_coefficients_);
+ ROS_ERROR ("[SACModelPlane::RefitModel] Cannot re-fit 0 inliers!");
+ refit_coefficients = model_coefficients_;
+ return;
}
Eigen::Vector4d plane_coefficients;
@@ -317,11 +318,9 @@
// Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
cloud_geometry::nearest::computePointNormal (*cloud_, inliers, plane_coefficients, curvature);
- std::vector<double> refit (4);
+ refit_coefficients.resize (4);
for (int d = 0; d < 4; d++)
- refit[d] = plane_coefficients (d);
-
- return (refit);
+ refit_coefficients[d] = plane_coefficients (d);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_sphere.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_sphere.cpp 2009-04-01 17:54:42 UTC (rev 13238)
+++ pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_sphere.cpp 2009-04-01 18:05:46 UTC (rev 13239)
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ * Copyright (c) 2008-2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
*
* All rights reserved.
*
@@ -186,13 +186,15 @@
/** \brief Create a new point cloud with inliers projected onto the sphere model.
* \param inliers the data inliers that we want to project on the sphere model
* \param model_coefficients the coefficients of a sphere model
+ * \param projected_points the resultant projected points
* \todo implement this.
*/
- robot_msgs::PointCloud
- SACModelSphere::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
+ void
+ SACModelSphere::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
+ robot_msgs::PointCloud &projected_points)
{
std::cerr << "[SACModelSphere::projecPoints] Not implemented yet." <...
[truncated message content] |
