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];
}
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;
}
virtual void linearize(const BeliefT& b // state
, const ControlT& u // control
, BeliefGradT* output_A // df/dx
, BeliefControlGradT* output_B // df/du
, BeliefT* output_c // df/dm
) const {
if (output_A) {
boost::function<BeliefT (const BeliefT& )> f_b;
f_b = boost::bind(&BeliefFunc::operator(), this, _1, u, nullptr, nullptr, nullptr);
num_diff(f_b, b, belief_dim, this->epsilon, output_A);
}
if (output_B) {
boost::function<BeliefT (const ControlT& )> f_u;
f_u = boost::bind(&BeliefFunc::operator(), this, b, _1, nullptr, nullptr, nullptr);
num_diff(f_u, u, belief_dim, this->epsilon, output_B);
}
if (output_c) {
*output_c = this->call(b, u);
}
}
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;
}
/*!*****************************************************************************
*******************************************************************************
\note read_traj_file
\date June 1999
\remarks
parse a script which describes the traj task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] flag : true= use desired data, false use actual data
******************************************************************************/
static int read_traj_file() {
int j,i,r,k,rc;
static char string[100];
static char fname[100] = "traj_strike.txt";
FILE * fp = NULL;
int found = FALSE;
Matrix buff;
int aux;
int ans = 0;
double data = 0.0;
/* open the file, and parse the parameters */
while (TRUE) {
if (!get_string("Name of the Traj File\0",fname,fname))
return FALSE;
/* try to read this file */
sprintf(string, "%s/%s", "saveData", fname);
/* try to count the number of lines */
traj_len = count_traj_file(string);
get_int("What percentage of trajectory do you want to keep?", 100, &ans);
traj_len = (int)(traj_len * (double)ans / 100.0);
printf("Keeping the first %d indices...\n", traj_len);
//printf("%d\n", traj_len);
if (traj_len == -1) { // problem
return FALSE;
}
fp = fopen(string,"r");
if (fp != NULL) {
found = TRUE;
break;
}
else {
printf("ERROR: Could not open file >%s<\n", string);
}
}
/* get the number of rows, columns, sampling frequency
and calc the buffer_size */
//rc = fscanf(fp, "%d %d %d %lf", &buffer_size, &N_COLS, &traj_len, &SAMP_FREQ);
/* read file into a buffer and check if the matrix size is correct */
buff = my_matrix(1,traj_len,1,N_COLS);
q0 = my_vector(1,N_DOFS);
int out = 0;
for (r = 1; r <= traj_len; r++) {
for (k = 1; k <= N_COLS; k++) {
out = fscanf(fp, "%lf", &data);
buff[r][k] = data;
}
}
fclose(fp);
// print buffs first 10 elements
//print_mat("Buffer:\n", buff);
//print_mat_size("Buffer:\n", buff, 10, N_COLS);
/* create the pos, vel, acc , uff matrices that define the trajectory */
traj_pos = my_matrix(1, traj_len, 1, N_DOFS);
traj_vel = my_matrix(1, traj_len, 1, N_DOFS);
traj_acc = my_matrix(1, traj_len, 1, N_DOFS);
traj_uff = my_matrix(1, traj_len, 1, N_DOFS);
// save q0
for (j = 1; j <= N_DOFS; j++)
q0[j] = buff[1][2*j];
for (r = 1; r <= traj_len; r++) {
for (k = 1; k <= N_DOFS; k++) {
traj_pos[r][k] = buff[r][2*k];
traj_vel[r][k] = buff[r][2*k+1];
}
}
//print_mat_size("Vel:\n", traj_vel, 10, n_dofs);
// numerically differentiate traj_vel instead!
num_diff(traj_acc, traj_vel, SAMP_FREQ);
/* free up memory by deallocating resources */
my_free_matrix(buff,1,traj_len,1,N_COLS);
return found;
}
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;
};
