// returns the 90 deg rotation of a (around n) most similar to target b
/// a and b should be in the same plane orthogonal to N
static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b,
const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n)
{
Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized();
typename DerivedV::Scalar scorea = a.dot(b);
typename DerivedV::Scalar scorec = c.dot(b);
if (fabs(scorea)>=fabs(scorec))
return a*Sign(scorea);
else
return c*Sign(scorec);
}
inline Real evaluate_point<1>(const Triangle<3>& t, const Point& point, const Eigen::Matrix<Real,3,1>& coefficients)
{
Eigen::Matrix<Real,3,1> bary_coeff = t.getBaryCoordinates(point);
//std::cout<< "B-coord: "<<bary_coeff<<std::endl;
return(coefficients.dot(bary_coeff));
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
signed_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{
/*
This method calcultes the sign of the angle which is used in the calculation of a dihedral angle.
As such, this method is not validated for other uses. It will fail if the basis atoms for the
dihedral (atom 2 and atom 3) overlap.
*/
float sa = 180.0 ;
float angle ;
float pi = acos(-1.0) ;
float ada = coor1.dot(coor1) ;
float bdb = coor2.dot(coor2) ;
if(ada*bdb <= 0.0)
{
std::cout << "; " ;
return sa ;
}
else
{
float adb = coor1.dot(coor2) ;
float argument = adb/sqrt(ada*bdb) ;
angle = (180.0/pi) * acos(argument) ;
}
Eigen::Matrix<float,3,1> cp ; cp = coor1.cross(coor2) ;
float dp = coor3.dot(cp) ;
float sign = 0.0 ;
if(dp < 0.0)
{
sign = -1.0 ;
}
else if(dp > 0.0)
{
sign = 1.0 ;
}
sa = sign*angle ;
return sa ;
}
inline double dot_product(const Eigen::Matrix<double, R1, C1>& v1,
const Eigen::Matrix<double, R2, C2>& v2) {
stan::math::check_vector("dot_product(%1%)",v1,"v1",(double*)0);
stan::math::check_vector("dot_product(%1%)",v2,"v2",(double*)0);
stan::math::check_matching_sizes("dot_product(%1%)",v1,"v1",
v2,"v2",(double*)0);
return v1.dot(v2);
}
Scalar costFunc(
const Eigen::Matrix<Scalar, -1, 1> &beta,
const Eigen::Matrix<Scalar, -1, -1> &X,
const Eigen::Matrix<Scalar, -1, 1> &y)
{
Eigen::Matrix<Scalar, -1, 1> tmp = (X * beta - y);
return tmp.dot(tmp);
}
inline double multiply(const Eigen::Matrix<double, 1, C1>& rv,
const Eigen::Matrix<double, R2, 1>& v) {
stan::math::check_matching_sizes("multiply",
"rv", rv,
"v", v);
if (rv.size() != v.size())
throw std::domain_error("rv.size() != v.size()");
return rv.dot(v);
}
// returns the 180 deg rotation of a (around n) most similar to target b
// a and b should be in the same plane orthogonal to N
static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_line(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b)
{
typename DerivedV::Scalar scorea = a.dot(b);
if (scorea<0)
return -a;
else
return a;
}
IGL_INLINE void igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::getGeneralCoeffConstraints(const Eigen::VectorXi &isConstrained,
const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
int k,
const Eigen::VectorXi &rootsIndex,
Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> &Ck)
{
int numConstrained = isConstrained.sum();
Ck.resize(numConstrained,1);
// int n = rootsIndex.cols();
Eigen::MatrixXi allCombs;
{
Eigen::VectorXi V = Eigen::VectorXi::LinSpaced(n,0,n-1);
igl::nchoosek(V,k+1,allCombs);
}
int ind = 0;
for (int fi = 0; fi <numF; ++fi)
{
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = B1.row(fi);
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = B2.row(fi);
if(isConstrained[fi])
{
std::complex<typename DerivedV::Scalar> ck(0);
for (int j = 0; j < allCombs.rows(); ++j)
{
std::complex<typename DerivedV::Scalar> tk(1.);
//collect products
for (int i = 0; i < allCombs.cols(); ++i)
{
int index = allCombs(j,i);
int ri = rootsIndex[index];
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> w;
if (ri>0)
w = cfW.block(fi,3*(ri-1),1,3);
else
w = -cfW.block(fi,3*(-ri-1),1,3);
typename DerivedV::Scalar w0 = w.dot(b1);
typename DerivedV::Scalar w1 = w.dot(b2);
std::complex<typename DerivedV::Scalar> u(w0,w1);
tk*= u;
}
//collect sum
ck += tk;
}
Ck(ind) = ck;
ind ++;
}
}
}
Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) {
Eigen::Matrix<double, 3, 1> v = A.cross(B);
double s = v.norm();
double c = A.dot(B);
Eigen::Matrix<double, 3, 3> vx;
vx << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx;
}
IGL_INLINE void igl::per_corner_normals(
const Eigen::PlainObjectBase<DerivedV>& /*V*/,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedV>& FN,
const std::vector<std::vector<IndexType> >& VF,
const double corner_threshold,
Eigen::PlainObjectBase<DerivedV> & CN)
{
using namespace igl;
using namespace Eigen;
using namespace std;
// number of faces
const int m = F.rows();
// valence of faces
const int n = F.cols();
// initialize output to ***zero***
CN.setZero(m*n,3);
// loop over faces
for(size_t i = 0;int(i)<m;i++)
{
// Normal of this face
Eigen::Matrix<typename DerivedV::Scalar,3,1> fn = FN.row(i);
// loop over corners
for(size_t j = 0;int(j)<n;j++)
{
const std::vector<IndexType> &incident_faces = VF[F(i,j)];
// loop over faces sharing vertex of this corner
for(int k = 0;k<(int)incident_faces.size();k++)
{
Eigen::Matrix<typename DerivedV::Scalar,3,1> ifn = FN.row(incident_faces[k]);
// dot product between face's normal and other face's normal
double dp = fn.dot(ifn);
// if difference in normal is slight then add to average
if(dp > cos(corner_threshold*PI/180))
{
// add to running sum
CN.row(i*n+j) += ifn;
// else ignore
}else
{
}
}
// normalize to take average
CN.row(i*n+j).normalize();
}
}
}
float KalmanTrace::sq_distance_in_sigmas (const Observation& observation) {
update_prediction( observation.time );
Eigen::Matrix<double,Obs,1> expected_observation
= observation_matrix * prediction.position;
Eigen::Matrix<double,Obs,Obs> covar_of_observations
= observation_matrix * prediction.covariance
* observation_matrix.transpose();
Eigen::Matrix<double,Obs,Obs> covar_of_next_measurement =
covar_of_observations + observation.covariance;
Eigen::Matrix<double,Obs,1> difference
= expected_observation - observation.position;
return difference.dot(
covar_of_next_measurement.inverse() * difference );
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
calc_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{
/*
Method to calculate the angle between three atoms
*/
float angle ;
Eigen::Matrix<float,3,1> u ; u = coor1 - coor2 ;
Eigen::Matrix<float,3,1> v ; v = coor3 - coor2 ;
float c = (u.dot(v))/(u.norm()*v.norm()) ;
angle = acos(c) ;
return angle ;
}
IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F,
const Eigen::Matrix<T, Eigen::Dynamic, 1>&X,
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G )
{
G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3);
for (int i = 0; i<F.rows(); ++i)
{
// renaming indices of vertices of triangles for convenience
int i1 = F(i,0);
int i2 = F(i,1);
int i3 = F(i,2);
// #F x 3 matrices of triangle edge vectors, named after opposite vertices
Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2);
Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3);
Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1);
// area of parallelogram is twice area of triangle
// area of parallelogram is || v1 x v2 ||
Eigen::Matrix<T, 1, 3> n = v32.cross(v13);
// This does correct l2 norm of rows, so that it contains #F list of twice
// triangle areas
double dblA = std::sqrt(n.dot(n));
// now normalize normals to get unit normals
Eigen::Matrix<T, 1, 3> u = n / dblA;
// rotate each vector 90 degrees around normal
double norm21 = std::sqrt(v21.dot(v21));
double norm13 = std::sqrt(v13.dot(v13));
Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21);
eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21));
eperp21 *= norm21;
Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13);
eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13));
eperp13 *= norm13;
G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA;
};
}
double operator()(const Eigen::Matrix<double,Eigen::Dynamic,1> &x) {
return x.dot(x) - 1.0;
}
double SurfaceFit::PolynomFit()
{
if (PlaneFit::Fit() == FLOAT_MAX)
return FLOAT_MAX;
Base::Vector3d bs(this->_vBase.x,this->_vBase.y,this->_vBase.z);
Base::Vector3d ex(this->_vDirU.x,this->_vDirU.y,this->_vDirU.z);
Base::Vector3d ey(this->_vDirV.x,this->_vDirV.y,this->_vDirV.z);
Base::Vector3d ez(this->_vDirW.x,this->_vDirW.y,this->_vDirW.z);
// A*x = b
// See also www.cs.jhu.edu/~misha/Fall05/10.23.05.pdf
// z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f
// z = P * Vi with Vi=(xi^2,yi^2,xiyi,xi,yi,1) and P=(a,b,c,d,e,f)
// To get the best-fit values the sum needs to be minimized:
// S = sum[(z-zi)^2} -> min with zi=z coordinates of the given points
// <=> S = sum[z^2 - 2*z*zi + zi^2] -> min
// <=> S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2] -> min
// To get the optimum the gradient of the expression must be the null vector
// Note: grad F(P) = (P*Vi)^2 = 2*(P*Vi)*Vi
// grad G(P) = -2*(P*Vi)*zi = -2*Vi*zi
// grad H(P) = zi^2 = 0
// => grad S(P) = sum[2*(P*Vi)*Vi - 2*Vi*zi] = 0
// <=> sum[(P*Vi)*Vi] = sum[Vi*zi]
// <=> sum[(Vi*Vi^t)*P] = sum[Vi*zi] where (Vi*Vi^t) is a symmetric matrix
// <=> sum[(Vi*Vi^t)]*P = sum[Vi*zi]
Eigen::Matrix<double,6,6> A = Eigen::Matrix<double,6,6>::Zero();
Eigen::Matrix<double,6,1> b = Eigen::Matrix<double,6,1>::Zero();
Eigen::Matrix<double,6,1> x = Eigen::Matrix<double,6,1>::Zero();
std::vector<Base::Vector3d> transform;
transform.reserve(_vPoints.size());
double dW2 = 0;
for (std::list<Base::Vector3f>::const_iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) {
Base::Vector3d clPoint(it->x,it->y,it->z);
clPoint.TransformToCoordinateSystem(bs, ex, ey);
transform.push_back(clPoint);
double dU = clPoint.x;
double dV = clPoint.y;
double dW = clPoint.z;
double dU2 = dU*dU;
double dV2 = dV*dV;
double dUV = dU*dV;
dW2 += dW*dW;
A(0,0) = A(0,0) + dU2*dU2;
A(0,1) = A(0,1) + dU2*dV2;
A(0,2) = A(0,2) + dU2*dUV;
A(0,3) = A(0,3) + dU2*dU ;
A(0,4) = A(0,4) + dU2*dV ;
A(0,5) = A(0,5) + dU2 ;
b(0) = b(0) + dU2*dW ;
A(1,1) = A(1,1) + dV2*dV2;
A(1,2) = A(1,2) + dV2*dUV;
A(1,3) = A(1,3) + dV2*dU ;
A(1,4) = A(1,4) + dV2*dV ;
A(1,5) = A(1,5) + dV2 ;
b(1) = b(1) + dV2*dW ;
A(2,2) = A(2,2) + dUV*dUV;
A(2,3) = A(2,3) + dUV*dU ;
A(2,4) = A(2,4) + dUV*dV ;
A(2,5) = A(2,5) + dUV ;
b(3) = b(3) + dUV*dW ;
A(3,3) = A(3,3) + dU *dU ;
A(3,4) = A(3,4) + dU *dV ;
A(3,5) = A(3,5) + dU ;
b(3) = b(3) + dU *dW ;
A(4,4) = A(4,4) + dV *dV ;
A(4,5) = A(4,5) + dV ;
b(5) = b(5) + dV *dW ;
A(5,5) = A(5,5) + 1 ;
b(5) = b(5) + 1 *dW ;
}
// Mat is symmetric
//
A(1,0) = A(0,1);
A(2,0) = A(0,2);
A(3,0) = A(0,3);
A(4,0) = A(0,4);
A(5,0) = A(0,5);
A(2,1) = A(1,2);
A(3,1) = A(1,3);
A(4,1) = A(1,4);
A(5,1) = A(1,5);
A(3,2) = A(2,3);
A(4,2) = A(2,4);
A(5,2) = A(2,5);
A(4,3) = A(3,4);
A(5,3) = A(3,5);
A(5,4) = A(4,5);
Eigen::HouseholderQR< Eigen::Matrix<double,6,6> > qr(A);
x = qr.solve(b);
// FunctionContainer gets an implicit function F(x,y,z) = 0 of this form
// _fCoeff[0] +
// _fCoeff[1]*x + _fCoeff[2]*y + _fCoeff[3]*z +
// _fCoeff[4]*x^2 + _fCoeff[5]*y^2 + _fCoeff[6]*z^2 +
// _fCoeff[7]*x*y + _fCoeff[8]*x*z + _fCoeff[9]*y*z
//
// The bivariate polynomial surface we get here is of the form
// z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f
// Writing it as implicit surface F(x,y,z) = 0 gives this form
// F(x,y,z) = f(x,y) - z = a*x^2 + b*y^2 + c*x*y + d*x + e*y - z + f
// Thus:
// _fCoeff[0] = f
// _fCoeff[1] = d
// _fCoeff[2] = e
// _fCoeff[3] = -1
// _fCoeff[4] = a
// _fCoeff[5] = b
// _fCoeff[6] = 0
// _fCoeff[7] = c
// _fCoeff[8] = 0
// _fCoeff[9] = 0
_fCoeff[0] = x(5);
_fCoeff[1] = x(3);
_fCoeff[2] = x(4);
_fCoeff[3] = -1.0;
_fCoeff[4] = x(0);
_fCoeff[5] = x(1);
_fCoeff[6] = 0.0;
_fCoeff[7] = x(2);
_fCoeff[8] = 0.0;
_fCoeff[9] = 0.0;
// Get S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2]
double sigma = 0;
FunctionContainer clFuncCont(_fCoeff);
for (std::vector<Base::Vector3d>::const_iterator it = transform.begin(); it != transform.end(); ++it) {
double u = it->x;
double v = it->y;
double z = clFuncCont.F(u, v, 0.0);
sigma += z*z;
}
sigma += dW2 - 2 * x.dot(b);
// This must be caused by some round-off errors. Theoretically it's impossible
// that 'sigma' becomes negative.
if (sigma < 0)
sigma = 0;
if (!_vPoints.empty())
sigma = sqrt(sigma/_vPoints.size());
_fLastResult = static_cast<float>(sigma);
return _fLastResult;
}
template<typename PointSource, typename PointTarget> double
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
PointCloudSource &trans_cloud)
{
// Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
double phi_0 = -score;
// Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
double d_phi_0 = -(score_gradient.dot (step_dir));
Eigen::Matrix<double, 6, 1> x_t;
if (d_phi_0 >= 0)
{
// Not a decent direction
if (d_phi_0 == 0)
return 0;
else
{
// Reverse step direction and calculate optimal step.
d_phi_0 *= -1;
step_dir *= -1;
}
}
// The Search Algorithm for T(mu) [More, Thuente 1994]
int max_step_iterations = 10;
int step_iterations = 0;
// Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
double mu = 1.e-4;
// Curvature condition constant, Equation 1.2 [More, Thuete 1994]
double nu = 0.9;
// Initial endpoints of Interval I,
double a_l = 0, a_u = 0;
// Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
// Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
bool interval_converged = (step_max - step_min) > 0, open_interval = true;
double a_t = step_init;
a_t = std::min (a_t, step_max);
a_t = std::max (a_t, step_min);
x_t = x + step_dir * a_t;
final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
// New transformed point cloud
transformPointCloud (*input_, trans_cloud, final_transformation_);
// Updates score, gradient and hessian. Hessian calculation is unessisary but testing showed that most step calculations use the
// initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
// Calculate phi(alpha_t)
double phi_t = -score;
// Calculate phi'(alpha_t)
double d_phi_t = -(score_gradient.dot (step_dir));
// Calculate psi(alpha_t)
double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t)
double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
// Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
{
// Use auxilary function if interval I is not closed
if (open_interval)
{
a_t = trialValueSelectionMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, psi_t, d_psi_t);
}
else
{
a_t = trialValueSelectionMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, phi_t, d_phi_t);
}
a_t = std::min (a_t, step_max);
a_t = std::max (a_t, step_min);
x_t = x + step_dir * a_t;
final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
// New transformed point cloud
// Done on final cloud to prevent wasted computation
transformPointCloud (*input_, trans_cloud, final_transformation_);
// Updates score, gradient. Values stored to prevent wasted computation.
score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);
// Calculate phi(alpha_t+)
phi_t = -score;
// Calculate phi'(alpha_t+)
d_phi_t = -(score_gradient.dot (step_dir));
// Calculate psi(alpha_t+)
psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t+)
d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
// Check if I is now a closed interval
if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
{
open_interval = false;
// Converts f_l and g_l from psi to phi
f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
g_l = g_l + mu * d_phi_0;
// Converts f_u and g_u from psi to phi
f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
g_u = g_u + mu * d_phi_0;
}
if (open_interval)
{
// Update interval end points using Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, psi_t, d_psi_t);
}
else
{
// Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, phi_t, d_phi_t);
}
step_iterations++;
}
// If inner loop was run then hessian needs to be calculated.
// Hessian is unnessisary for step length determination but gradients are required
// so derivative and transform data is stored for the next iteration.
if (step_iterations)
computeHessian (hessian, trans_cloud, x_t);
return (a_t);
}
int operator()(const Eigen::Matrix<double,Eigen::Dynamic,1> &x,
double &f, Eigen::Matrix<double,Eigen::Dynamic,1> &g) {
f = x.dot(x) - 1.0;
g = 2.0*x;
return 0;
}
Scalar func(
const Eigen::Matrix<Scalar, -1, 1> &beta,
const Eigen::Matrix<Scalar, -1, 1> &x)
{
return beta.dot(x);
}
