aslam_optimizer
aslam_optimizer copied to clipboard
JacobianContainer.add should have a _const_ DesignVariable* as first argument. Don't it?
Or the other way around : why was it non const in the first place? I can't see the reason. Currently there are const cast because of that in almost every expression tree leaf implementation. Because their evaluateJacobiansImplementation is mostly const.
template<int D>
void DesignVariableVector<D>::evaluateJacobiansImplementation(JacobianContainer & outJacobians) const
{
SM_ASSERT_EQ_DBG(aslam::InvalidArgumentException, outJacobians.rows(), D, "The Jacobian container dimension doesn't match the state. Are you missing a chain rule?");
outJacobians.add(const_cast<DesignVariableVector<D>*>(this), Eigen::Matrix<double,D,D>::Identity());
}