/* Linear penalty function for bounding var to the interval $-a < var < a $ * var The scalar that should be bounded * a lower and upper absolute bound * epsilon safty margin (move bound to the interior of the interval) * penaltyBoundToIntervalDerivative * return Penalty / cost value that is nonzero if the constraint is not satisfied */
inlinedoublepenaltyBoundToInterval(constdouble& var,constdouble& a,constdouble& epsilon) { if (var < -a+epsilon) { return (-var - (a - epsilon)); } // 怀疑这里应该是 >= if (var <= a-epsilon) { return0.; } else { return (var - (a - epsilon)); } }
/** * Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ * var The scalar that should be bounded * a lower bound * b upper bound * epsilon safty margin (move bound to the interior of the interval) * penaltyBoundToIntervalDerivative * return Penalty / cost value that is nonzero if the constraint is not satisfied */ inlinedoublepenaltyBoundToInterval(constdouble& var,constdouble& a, constdouble& b, constdouble& epsilon) { if (var < a+epsilon) { return (-var + (a + epsilon)); } if (var <= b-epsilon) { return0.; } else { return (var - (b - epsilon)); } }
雅格比矩阵的推导
雅格比矩阵的计算被注释了,以为使用重写的雅格比可以加快运算速度,但是Github上有作者的解释,看了似懂非懂:对于一些nontrivial(非显而易见的,难以直观理解的) cost functions (比如 penalizing the distance between a line and a polygon) 最终表达式实在太长了,是否 central difference approximation is slower (2 times evaluating the function) then evaluating the exact Jacobian.