/** 3D rigid transformation refinement using LM
* Refine the Scale, Rotation and translation rigid transformation
* using a Levenberg-Marquardt opimization.
*
* \param[in] x1 The first 3xN matrix of euclidean points
* \param[in] x2 The second 3xN matrix of euclidean points
* \param[out] S The initial scale factor
* \param[out] t The initial 3x1 translation
* \param[out] R The initial 3x3 rotation
*
* \return none
*/
static void Refine_RTS( const Mat &x1,
const Mat &x2,
double * S,
Vec3 * t,
Mat3 * R )
{
{
lm_SRTRefine_functor functor( 7, 3 * x1.cols(), x1, x2, *S, *R, *t );
Eigen::NumericalDiff<lm_SRTRefine_functor> numDiff( functor );
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<lm_SRTRefine_functor> > lm( numDiff );
lm.parameters.maxfev = 1000;
Vec xlm = Vec::Zero( 7 ); // The deviation vector {tx,ty,tz,rotX,rotY,rotZ,S}
lm.minimize( xlm );
Vec3 transAdd = xlm.block<3, 1>( 0, 0 );
Vec3 rot = xlm.block<3, 1>( 3, 0 );
double SAdd = xlm( 6 );
//Build the rotation matrix
Mat3 Rcor =
( Eigen::AngleAxis<double>( rot( 0 ), Vec3::UnitX() )
* Eigen::AngleAxis<double>( rot( 1 ), Vec3::UnitY() )
* Eigen::AngleAxis<double>( rot( 2 ), Vec3::UnitZ() ) ).toRotationMatrix();
*R = ( *R ) * Rcor;
*t = ( *t ) + transAdd;
*S = ( *S ) + SAdd;
}
// Refine rotation
{
lm_RRefine_functor functor( 3, 3 * x1.cols(), x1, x2, *S, *R, *t );
Eigen::NumericalDiff<lm_RRefine_functor> numDiff( functor );
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<lm_RRefine_functor> > lm( numDiff );
lm.parameters.maxfev = 1000;
Vec xlm = Vec::Zero( 3 ); // The deviation vector {rotX,rotY,rotZ}
lm.minimize( xlm );
Vec3 rot = xlm.block<3, 1>( 0, 0 );
//Build the rotation matrix
Mat3 Rcor =
( Eigen::AngleAxis<double>( rot( 0 ), Vec3::UnitX() )
* Eigen::AngleAxis<double>( rot( 1 ), Vec3::UnitY() )
* Eigen::AngleAxis<double>( rot( 2 ), Vec3::UnitZ() ) ).toRotationMatrix();
*R = ( *R ) * Rcor;
}
}
transformation_t optimize_nonlinear(
const AbsoluteAdapterBase & adapter,
const Indices & indices )
{
const int n=6;
VectorXd x(n);
x.block<3,1>(0,0) = adapter.gett();
x.block<3,1>(3,0) = math::rot2cayley(adapter.getR());
OptimizeNonlinearFunctor1 functor( adapter, indices );
NumericalDiff<OptimizeNonlinearFunctor1> numDiff(functor);
LevenbergMarquardt< NumericalDiff<OptimizeNonlinearFunctor1> > lm(numDiff);
lm.resetParameters();
lm.parameters.ftol = 1.E1*NumTraits<double>::epsilon();
lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
lm.parameters.maxfev = 1000;
lm.minimize(x);
transformation_t transformation;
transformation.col(3) = x.block<3,1>(0,0);
transformation.block<3,3>(0,0) = math::cayley2rot(x.block<3,1>(3,0));
return transformation;
}
void
opengv::absolute_pose::modules::gpnp_optimize(
const Eigen::Matrix<double,12,1> & a,
const Eigen::Matrix<double,12,12> & V,
const points_t & c,
std::vector<double> & factors )
{
const int n=factors.size();
VectorXd x(n);
for(size_t i = 0; i < factors.size(); i++)
x[i] = factors[i];
GpnpOptimizationFunctor functor( a, V, c, factors.size() );
NumericalDiff<GpnpOptimizationFunctor> numDiff(functor);
LevenbergMarquardt< NumericalDiff<GpnpOptimizationFunctor> > lm(numDiff);
lm.resetParameters();
lm.parameters.ftol = 1.E10*NumTraits<double>::epsilon();
lm.parameters.xtol = 1.E10*NumTraits<double>::epsilon();
lm.parameters.maxfev = 1000;
lm.minimize(x);
for(size_t i = 0; i < factors.size(); i++)
factors[i] = x[i];
}
void test_central()
{
VectorXd x(3);
MatrixXd jac(15,3);
MatrixXd actual_jac(15,3);
my_functor functor;
x << 0.082, 1.13, 2.35;
// real one
functor.actual_df(x, actual_jac);
// using NumericalDiff
NumericalDiff<my_functor,Central> numDiff(functor);
numDiff.df(x, jac);
VERIFY_IS_APPROX(jac, actual_jac);
}
void test_forward()
{
VectorXd x(3);
MatrixXd jac(15,3);
MatrixXd actual_jac(15,3);
my_functor functor;
x << 0.082, 1.13, 2.35;
// real one
functor.actual_df(x, actual_jac);
// std::cout << actual_jac << std::endl << std::endl;
// using NumericalDiff
NumericalDiff<my_functor> numDiff(functor);
numDiff.df(x, jac);
// std::cout << jac << std::endl;
VERIFY_IS_APPROX(jac, actual_jac);
}
Eigen::MatrixXd numericalDiff(std::function<ValueType_ (const InputType_ &) > function, InputType_ const & input, double eps = sqrt(std::numeric_limits<typename NumericalDiffFunctor<ValueType_, InputType_>::scalar_t>::epsilon())){
typedef NumericalDiffFunctor<ValueType_, InputType_> Functor;
NumericalDiff<Functor> numDiff(Functor(function), eps);
return numDiff.estimateJacobian(input);
}
