template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
boost::mutex::scoped_lock lock (tmp_mutex_);
const int n_unknowns = 3; // 3 unknowns
// Needs a set of valid model coefficients
if (model_coefficients.size () != n_unknowns)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
optimized_coefficients = model_coefficients;
return;
}
// Need at least 3 samples
if (inliers.size () <= 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
tmp_inliers_ = &inliers;
int m = inliers.size ();
double *fvec = new double[m];
int iwa[n_unknowns];
int lwa = m * n_unknowns + 5 * n_unknowns + m;
double *wa = new double[lwa];
// Set the initial solution
double x[n_unknowns];
for (int d = 0; d < n_unknowns; ++d)
x[d] = model_coefficients[d]; // initial guess
// 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 (&pcl::SampleConsensusModelCircle2D<PointT>::functionToOptimize, this, m, n_unknowns, x, fvec, tol, iwa, wa, lwa);
// Compute the L2 norm of the residuals
PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n",
info, enorm (m, fvec), model_coefficients[0], model_coefficients[1], model_coefficients[2], x[0], x[1], x[2]);
optimized_coefficients = Eigen::Vector3f (x[0], x[1], x[2]);
free (wa); free (fvec);
}
/** \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
*/
void
SACModelCircle2D::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
{
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_;
}
tmp_inliers_ = &inliers;
int m = inliers.size ();
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 ((int)model_coefficients_.size () == n)
for (int d = 0; d < n; 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. \nInitial solution: %g %g %g \nFinal 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 (n);
for (int d = 0; d < n; d++)
refit_coefficients[d] = x[d];
free (wa); free (fvec);
}
int SkeletonFitting::minimizeSimple(void* p, minpack_func_mn fcn, int m, int n, double* vals, double tol)
{
double* fvec = new double[m];
// work array
int* iwa = new int[n];
int lwa = m * n + 5 * n + m;
double* wa = new double[lwa];
int info = lmdif1(fcn, p, m, n, vals, fvec, tol, iwa, wa, lwa);
delete[] fvec;
delete[] iwa;
delete[] wa;
return info;
}
