void mmf::OptSO3ApproxGD::LineSearch(Eigen::Vector3f* J, float* f) {
float delta = 1.;
SO3f thetaNew = theta_;
ComputeJacobian(thetaPrev_, thetaNew, J, f);
float fNew = *f;
Eigen::Vector3f d = -(*J)/J->norm();
// std::cout << "\tJ=" << J->transpose() << std::endl
// << "\td=" << d.transpose() << std::endl;
float m = J->dot(d);
while (*f-fNew < -c_*m*delta && delta > 1e-16) {
delta *= ddelta_;
thetaNew = theta_+delta*d;
//std::cout << thetaNew << std::endl;
ComputeJacobian(thetaPrev_, thetaNew, NULL, &fNew);
// std::cout << *f-fNew << " <? " << -c_*m*delta
// << "\tfNew=" << fNew << "\tdelta=" << delta << std::endl;
}
*J = delta*d;
*f = fNew;
}
template <class T> double GaussNewton<T>::Fit(DArray &c) {
double sum = 0.;
DArray res(x_.size(), 0.);
DMatrix jac(x_.size(), t_.GetC()), dc(1, t_.GetC());
do {
//break;
ComputeResiduals(res);
ComputeJacobian(jac);
ComputeDeltas(jac, res, dc);
cerr << "Deltas: " << endl;
PrintMatrix(dc);
} while(RoundToZero(UpdateParameters(dc, c)) != 0.);
ComputeResiduals(res);
for(int i = 0; i < res.size(); ++i) {
sum += res[i] * res[i];
}
return sqrt(sum);
}
void BackwardEulerIvpOdeSolver::CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
double timeStep,
double time,
std::vector<double>& rCurrentYValues,
std::vector<double>& rNextYValues)
{
// Check the size of the ODE system matches the solvers expected
assert(mSizeOfOdeSystem == pAbstractOdeSystem->GetNumberOfStateVariables());
unsigned counter = 0;
// const double eps = 1e-6 * rCurrentGuess[0]; // Our tolerance (should use min(guess) perhaps?)
const double eps = 1e-6; // JonW tolerance
double norm = 2*eps;
std::vector<double> current_guess(mSizeOfOdeSystem);
current_guess.assign(rCurrentYValues.begin(), rCurrentYValues.end());
while (norm > eps)
{
// Calculate Jacobian and mResidual for current guess
ComputeResidual(pAbstractOdeSystem, timeStep, time, rCurrentYValues, current_guess);
ComputeJacobian(pAbstractOdeSystem, timeStep, time, rCurrentYValues, current_guess);
// // Update norm (our style)
// norm = ComputeNorm(mResidual);
// Solve Newton linear system
SolveLinearSystem();
// Update norm (JonW style)
norm = ComputeNorm(mUpdate);
// Update current guess
for (unsigned i=0; i<mSizeOfOdeSystem; i++)
{
current_guess[i] -= mUpdate[i];
}
counter++;
assert(counter < 20); // avoid infinite loops
}
rNextYValues.assign(current_guess.begin(), current_guess.end());
}
