GumbelDistributionFitter::GumbelDistributionFitResult GumbelDistributionFitter::fit(vector<DPosition<2> > & input)
{
Eigen::VectorXd x_init (2);
x_init(0) = init_param_.a;
x_init(1) = init_param_.b;
GumbelDistributionFunctor functor (2, &input);
Eigen::LevenbergMarquardt<GumbelDistributionFunctor> lmSolver (functor);
Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);
//the states are poorly documented. after checking the source, we believe that
//all states except NotStarted, Running and ImproperInputParameters are good
//termination states.
if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
{
throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-GumbelDistributionFitter", "Could not fit the gumbel distribution to the data");
}
#ifdef GUMBEL_DISTRIBUTION_FITTER_VERBOSE
// build a formula with the fitted parameters for gnuplot
stringstream formula;
formula << "f(x)=" << "(1/" << x_init(1) << ") * " << "exp(( " << x_init(0) << "- x)/" << x_init(1) << ") * exp(-exp((" << x_init(0) << " - x)/" << x_init(1) << "))";
cout << formula.str() << endl;
#endif
return GumbelDistributionFitResult (x_init(0), x_init(1));
}
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
if (model_coefficients.size () != 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
// Need at least 3 samples
if (inliers.size () <= 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ());
return;
}
tmp_inliers_ = &inliers;
OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
int info = lm.minimize (optimized_coefficients);
// 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, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2]);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
if (inliers.empty ())
{
PCL_DEBUG ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n");
return;
}
OptimizationFunctor functor (this, inliers);
Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
int info = lm.minimize (optimized_coefficients);
// Compute the L2 norm of the residuals
PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
line_dir.normalize ();
optimized_coefficients[3] = line_dir[0];
optimized_coefficients[4] = line_dir[1];
optimized_coefficients[5] = line_dir[2];
}
GaussFitter::GaussFitResult GaussFitter::fit(vector<DPosition<2> > & input) const
{
Eigen::VectorXd x_init (3);
x_init(0) = init_param_.A;
x_init(1) = init_param_.x0;
x_init(2) = init_param_.sigma;
GaussFunctor functor (3, &input);
Eigen::LevenbergMarquardt<GaussFunctor> lmSolver (functor);
Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);
// the states are poorly documented. after checking the source and
// http://www.ultimatepp.org/reference%24Eigen_demo%24en-us.html we believe that
// all states except TooManyFunctionEvaluation and ImproperInputParameters are good
// termination states.
if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters ||
status == Eigen::LevenbergMarquardtSpace::TooManyFunctionEvaluation)
{
throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-GaussFitter", "Could not fit the Gaussian to the data: Error " + String(status));
}
x_init(2) = fabs(x_init(2)); // sigma can be negative, but |sigma| would actually be the correct solution
#ifdef GAUSS_FITTER_VERBOSE
std::stringstream formula;
formula << "f(x)=" << result.A << " * exp(-(x - " << result.x0 << ") ** 2 / 2 / (" << result.sigma << ") ** 2)";
std::cout << formular.str() << std::endl;
#endif
return GaussFitResult (x_init(0), x_init(1), x_init(2));
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () != indices_tgt.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
return;
}
if (indices_src.size () < 4) // need at least 4 samples
{
PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!",
indices_src.size ());
return;
}
// If no warp function has been set, use the default (WarpPointRigid6D)
if (!warp_point_)
warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);
int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space
Eigen::VectorXd x (n_unknowns);
x.setConstant (n_unknowns, 0);
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
tmp_idx_src_ = &indices_src;
tmp_idx_tgt_ = &indices_tgt;
OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this);
Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm (num_diff);
int info = lm.minimize (x);
// Compute the norm of the residuals
PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
PCL_DEBUG ("Final solution: [%f", x[0]);
for (int i = 1; i < n_unknowns; ++i)
PCL_DEBUG (" %f", x[i]);
PCL_DEBUG ("]\n");
// Return the correct transformation
Eigen::VectorXf params = x.cast<float> ();
warp_point_->setParam (params);
transformation_matrix = warp_point_->getTransform ();
tmp_src_ = NULL;
tmp_tgt_ = NULL;
tmp_idx_src_ = tmp_idx_tgt_ = NULL;
}
template <typename PointSource, typename PointTarget, typename MatScalar> void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
// <cloud_src,cloud_src> is the source dataset
if (cloud_src.points.size () != cloud_tgt.points.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
PCL_ERROR ("Number or points in source (%lu) differs than target (%lu)!\n",
cloud_src.points.size (), cloud_tgt.points.size ());
return;
}
if (cloud_src.points.size () < 4) // need at least 4 samples
{
PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!\n",
cloud_src.points.size ());
return;
}
int n_unknowns = warp_point_->getDimension ();
VectorX x (n_unknowns);
x.setZero ();
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
OptimizationFunctor functor (static_cast<int> (cloud_src.points.size ()), this);
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
//Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
int info = lm.minimize (x);
// Compute the norm of the residuals
PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
PCL_DEBUG ("Final solution: [%f", x[0]);
for (int i = 1; i < n_unknowns; ++i)
PCL_DEBUG (" %f", x[i]);
PCL_DEBUG ("]\n");
// Return the correct transformation
warp_point_->setParam (x);
transformation_matrix = warp_point_->getTransform ();
tmp_src_ = NULL;
tmp_tgt_ = NULL;
}
void OptimizePick::optimize(std::vector<PeakShape> & peaks, Data & data)
{
if (peaks.empty())
return;
size_t global_peak_number = 0;
data.peaks.assign(peaks.begin(), peaks.end());
size_t num_dimensions = 4 * data.peaks.size();
Eigen::VectorXd x_init (num_dimensions);
x_init.setZero();
// We have to initialize the parameters for the optimization
for (size_t i = 0; i < data.peaks.size(); i++)
{
PeakShape current_peak = data.peaks[i];
double h = current_peak.height;
double wl = current_peak.left_width;
double wr = current_peak.right_width;
double p = current_peak.mz_position;
if (boost::math::isnan(wl))
{
data.peaks[i].left_width = 1;
wl = 1.;
}
if (boost::math::isnan(wr))
{
data.peaks[i].right_width = 1;
wr = 1.;
}
x_init(4 * i) = h;
x_init(4 * i + 1) = wl;
x_init(4 * i + 2) = wr;
x_init(4 * i + 3) = p;
}
data.penalties = penalties_;
unsigned num_data_points = std::max(data.positions.size() + 1, num_dimensions);
OptPeakFunctor functor (num_dimensions, num_data_points, &data);
Eigen::LevenbergMarquardt<OptPeakFunctor> lmSolver (functor);
lmSolver.parameters.maxfev = max_iteration_;
Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);
//the states are poorly documented. after checking the source, we believe that
//all states except NotStarted, Running and ImproperInputParameters are good
//termination states.
if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
{
throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-OptimizePeak:", "Could not fit the data: Error " + String(status));
}
// iterate over all peaks and store the optimized values in peaks
for (size_t current_peak = 0; current_peak < data.peaks.size(); current_peak++)
{
// Store the current parameters for this peak
peaks[global_peak_number + current_peak].height = x_init(4 * current_peak);
peaks[global_peak_number + current_peak].mz_position = x_init(4 * current_peak + 3);
peaks[global_peak_number + current_peak].left_width = x_init(4 * current_peak + 1);
peaks[global_peak_number + current_peak].right_width = x_init(4 * current_peak + 2);
// compute the area
// is it a Lorentz or a Sech - Peak?
if (peaks[global_peak_number + current_peak].type == PeakShape::LORENTZ_PEAK)
{
PeakShape p = peaks[global_peak_number + current_peak];
double x_left_endpoint = p.mz_position - 1 / p.left_width * sqrt(p.height / 1 - 1);
double x_right_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
double area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
double area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_right_endpoint));
peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
std::cout << "Lorentz " << area_left << " " << area_right
<< " " << peaks[global_peak_number + current_peak].area << std::endl;
#endif
}
else //It's a Sech - Peak
{
PeakShape p = peaks[global_peak_number + current_peak];
double x_left_endpoint = p.mz_position - 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
double x_right_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
double area_left = p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint)) / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
double area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_right_endpoint)) / cosh(p.right_width * (p.mz_position - x_right_endpoint)));
peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
std::cout << "Sech " << area_left << " " << area_right
<< " " << peaks[global_peak_number + current_peak].area << std::endl;
std::cout << p.mz_position << " " << x_left_endpoint << " " << x_right_endpoint << std::endl;
#endif
}
}
//global_peak_number += data.peaks.size();
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const std::vector<int> &indices_src_dfp,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
const std::vector<int> &indices_tgt_dfp,
float alpha_arg,
Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () != indices_tgt.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
return;
}
if (indices_src_dfp.size () != indices_tgt_dfp.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src_dfp.size (), (unsigned long)indices_tgt_dfp.size ());
return;
}
if ( (indices_src.size () + indices_tgt_dfp.size () )< 4) // need at least 4 samples
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize] ");
PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
(unsigned long)indices_src.size ());
return;
}
// If no warp function has been set, use the default (WarpPointRigid6D)
if (!warp_point_)
warp_point_.reset (new pcl::WarpPointRigid6D<PointSource, PointTarget>);
int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space
int num_p = indices_src.size ();
int num_dfp = indices_src_dfp.size ();
Eigen::VectorXd x(n_unknowns);
x.setConstant (n_unknowns, 0);
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
tmp_idx_src_ = &indices_src;
tmp_idx_tgt_ = &indices_tgt;
tmp_idx_src_dfp_ = &indices_src_dfp;
tmp_idx_tgt_dfp_ = &indices_tgt_dfp;
// TODO: CHANGE NUMBER OF VALUES TO NUM_P + NUM_DFP
OptimizationFunctor functor (n_unknowns, 1, num_p, num_dfp, this); // Initialize functor
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); // Add derivative functionality
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm (num_diff);
int info = lm.minimize (x); // Minimize cost
// Compute the norm of the residuals
// PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
// PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
// PCL_DEBUG ("Final solution: [%f", x[0]);
std::cout << "[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation]" << std::endl;
std::cout << "LM solver finished with exit code " << info <<", having a residual norm of " << lm.fvec.norm () << std::endl;
std::cout << "Final solution: " << x[0] << std::endl;
for (int i = 1; i < n_unknowns; ++i)
PCL_DEBUG (" %f", x[i]);
PCL_DEBUG ("]\n");
// Return the correct transformation
Eigen::VectorXf params = x.cast<float> ();
warp_point_->setParam (params);
transformation_matrix = warp_point_->getTransform ();
tmp_src_ = NULL;
tmp_tgt_ = NULL;
tmp_idx_src_ = tmp_idx_tgt_ = NULL;
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const std::vector<int> &handles_indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
const std::vector<int> &handles_indices_tgt,
Eigen::Matrix4f &transformation_matrix )
{
if (indices_src.size () != indices_tgt.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
return;
}
if (!((indices_src_dfp_set_)&&(indices_tgt_dfp_set_)))
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Correspondences of distinctive feature points in source and target clouds are not set");
return;
}
if (indices_src_dfp_.size () != indices_tgt_dfp_.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or distinctive feature points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src_dfp_.size (), (unsigned long)indices_tgt_dfp_.size ());
return;
}
if (weights_dfp_set_ && ((weights_dfp_.size () != indices_src_dfp_.size ())))
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or distinctive feature point pairs weights of (%lu) differs than number of distinctive feature points (%lu)!\n", (unsigned long)weights_dfp_.size (), (unsigned long)indices_tgt_dfp_.size ());
return;
}
if ( (indices_src.size () + indices_src_dfp_.size()) < 4) // need at least 4 samples
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize] ");
PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
(unsigned long)indices_src.size ());
return;
}
// If no warp function has been set, use the default (WarpPointRigid6D)
if (!warp_point_)
warp_point_.reset (new pcl::WarpPointRigid6D<PointSource, PointTarget>);
int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space
int num_p = indices_src.size ();
int num_dfp = indices_src_dfp_.size ();
int num_handle_p = handles_indices_src.size ();
Eigen::VectorXd x(n_unknowns);
x.setConstant (n_unknowns, 0);
// Set temporary pointers to clouds
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
// ... common points
tmp_idx_src_ = &indices_src;
tmp_idx_tgt_ = &indices_tgt;
// ... DF points
tmp_idx_src_dfp_ = &indices_src_dfp_;
tmp_idx_tgt_dfp_ = &indices_tgt_dfp_;
// ... handles
tmp_idx_src_handles_ = &handles_indices_src;
tmp_idx_tgt_handles_ = &handles_indices_tgt;
//std::cerr << "indices_src_dfp_ size " << indices_src_dfp_.size() << " \n ";
//std::cerr << "indices_tgt_dfp_ size " << indices_tgt_dfp_.size() << " \n ";
int info;
double lm_norm;
int iter;
if (weights_dfp_set_) {
// DF Weights are set
tmp_dfp_weights_ = &weights_dfp_;
OptimizationFunctorWithWeights functor (n_unknowns, num_handle_p+num_p+num_dfp, num_p, num_dfp, num_handle_p, this); // Initialize functor
Eigen::NumericalDiff<OptimizationFunctorWithWeights> num_diff (functor); // Add derivative functionality
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithWeights> > lm (num_diff);
/* From Eigen:: enum Status {
NotStarted = -2, Running = -1, ImproperInputParameters = 0, RelativeReductionTooSmall = 1,
RelativeErrorTooSmall = 2, RelativeErrorAndReductionTooSmall = 3, CosinusTooSmall = 4, TooManyFunctionEvaluation = 5,
FtolTooSmall = 6, XtolTooSmall = 7, GtolTooSmall = 8, UserAsked = 9
} */
info = lm.minimize (x); // Minimize cost
lm_norm = lm.fvec.norm ();
iter = lm.iter;
} else {
// DF Weights are not set
OptimizationFunctor functor (n_unknowns, num_p+num_dfp+num_handle_p, num_p, num_dfp, num_handle_p, this); // Initialize functor
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); // Add derivative functionality
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm (num_diff);
info = lm.minimize (x); // Minimize cost
lm_norm = lm.fvec.norm ();
iter = lm.iter;
}
// Compute the norm of the residuals
// PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
// PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm_norm ());
// PCL_DEBUG ("Final solution: [%f", x[0]);
// for (int i = 1; i < n_unknowns; ++i)
// PCL_DEBUG (" %f", x[i]);
// PCL_DEBUG ("]\n");
/*
std::cout << "[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation]" << std::endl;
std::cout << "LM solver finished with exit code " << info <<", having a residual norm of " << lm_norm
<< ", in iteration "<< iter << std::endl;
*/
// Return the correct transformation
Eigen::VectorXf params = x.cast<float> ();
warp_point_->setParam (params);
transformation_matrix = warp_point_->getTransform ();
//std::cout << " Obtained transform = " << std::endl << transformation_matrix << std::endl;
tmp_src_ = NULL;
tmp_tgt_ = NULL;
tmp_idx_src_ = tmp_idx_tgt_ = NULL;
tmp_idx_src_dfp_ = tmp_idx_tgt_dfp_ = NULL;
tmp_idx_src_handles_ = tmp_idx_tgt_handles_ = NULL;
tmp_weights_ = NULL;
};