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 Simulation::computeForces(const VectorX& x, VectorX& force)
{
VectorX gradient;
gradient.resize(m_mesh->m_system_dimension);
gradient.setZero();
// springs
for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
{
(*it)->EvaluateGradient(x, gradient);
}
// external forces
gradient -= m_external_force;
force = -gradient;
}
virtual void SetUp()
{
using namespace MPCWalkgen;
int nbSamples = 3;
Real samplingPeriod = 1.0;
bool autoCompute = true;
VectorX variable;
variable.setZero(2*nbSamples);
Real feedbackPeriod = 0.5;
vectorOfVector2 p(3);
p[0] = Vector2(1.0, 1.0);
p[1] = Vector2(-1.0, 1.0);
p[2] = Vector2(1.0, -1.0);
HumanoidFeetSupervisor<Real> feetSupervisor(nbSamples,
samplingPeriod);
feetSupervisor.setLeftFootCopConvexPolygon(ConvexPolygon<Real>(p));
feetSupervisor.setRightFootCopConvexPolygon(ConvexPolygon<Real>(p));
feetSupervisor.updateTimeline(variable, feedbackPeriod);
LIPModel<Real> lip(nbSamples, samplingPeriod, autoCompute);
lip.setFeedbackPeriod(feedbackPeriod);
HumanoidCopConstraint<Real> copCtr(lip, feetSupervisor);
VectorX x0 = VectorX::Zero(6);
function_ = copCtr.getFunction(x0);
gradient_ = copCtr.getGradient(x0.rows());
supBounds_ = copCtr.getSupBounds(x0);
infBounds_ = copCtr.getInfBounds(x0);
}