[maxima] lagrange multiplier solver

I made Lagrange multiplier solver for maxima.
This can deal matrix without define each element.

diff_level_by_variables_sub(expression,variables,diff_level):=map(lambda([x],if x=0 then diff_level else (diff_level_by_variables_sub(x,variables,diff_level +1))),map(lambda([x],diff(expression,x,1) ) ,variables ));
diff_level_by_variables(expression,variables):=diff_level_by_variables_sub(expression,variables,0);
total_diff_level_by_variables(expression,variables):=apply(max,flatten(diff_level_by_variables(expression,variables)));
is_const_or_linear_for_variables(expression,variables):=is(total_diff_level_by_variables(expression,variables)<2);
linear_aproximate_for_variables(expression,variables):=map(lambda([x],if is_const_or_linear_for_variables(x,variables) then  x else 0)    ,expand(expression));


_lagrange_multipliers_:makelist(concat(_lagrange_multiplier_,k),k,1,10);
declare(''_lagrange_multipliers_,scalar);

lagrange_multiplier_solve(obj_fn,eqs,variables,constant_coordinate_diff_mod_matrix):=block(
  [lagrangian
  ,lagrangian_diffs
  ,solved_lagrangian_for_variables
  ,solved_lagrangian_for_variables_without_lagrange_multiplier
  ,eqs_lagrange_multipliers
  ,eqs_lagrange_multipliers_factoed
  ,solved_lagrange_multiplier  
  ,neqs:length(eqs)
  ,lagrange_multipliers:makelist(_lagrange_multipliers_[k],k,1,length(eqs))
  ],
  dotscrules:true,
  dotexptsimp:false,
  dotassoc:false,
  disp(lagrangian:obj_fn-lagrange_multipliers . eqs),
  disp(lagrangian_diffs:makelist(subst(constant_coordinate_diff_mod_matrix[k].variables[k]=variables[k],expand(constant_coordinate_diff_mod_matrix[k].diff(lagrangian,variables[k]))),k,1,length(variables))),
  disp(solved_lagrangian_for_variables:solve(lagrangian_diffs,variables)),
  disp(eqs_lagrange_multipliers:subst(solved_lagrangian_for_variables,eqs)),
  
  /*need change for other problem*/
  eqs_lagrange_multipliers_factoed:eqs_lagrange_multipliers,
  
  disp(solved_lagrange_multiplier:solve(eqs_lagrange_multipliers_factoed,lagrange_multipliers)),
  solved_lagrangian_for_variables_without_lagrange_multiplier:subst(solved_lagrange_multiplier, solved_lagrangian_for_variables)
  /*,[solved_lagrangian_for_variables_without_lagrange_multiplier,[solved_lagrangian_for_variables,solved_lagrange_multiplier]]*/
  );

Usage

Let derivate formulation in

K. Kanatani, Y. Sugaya, and H. Niitsuma, Triangulation from two views revisited: Hartley-Sturm vs. optimal correction
Proceedings of the 19th British Machine Vision Conference (BMVC'08), September 2008, Leeds, U.K., pp. 173--182.
http://www.suri.cs.okayama-u.ac.jp/~kanatani/papers/bmtriang.pdf

/*usage*/

/*
equation(9) <= linear_aproximate_for_variables( (8) ,[dx,dy]

solve min (8) sub (9)

3rd coordinate is constant 
dx/dx3=0
dy/dy3=0
=> Pk.dx=dx where Pk=diag([1,1,0])
*/

result1:lagrange_multiplier_solve(dx.dx+dy.dy,[
 /* linear_aproximate_for_variables(inprod((x-dx),F.(y-dy)),[dx,dy])*/
  linear_aproximate_for_variables(((x-dx).F.(y-dy)),[dx,dy])
  ],[dx,dy],[Pk,Pk]);

/*you will see same formulation (18)*/

/*can not deal nonlinear constraint*/
/*lagrange_multiplier_solve(dx.dx+dy.dy,[(x-dx).F.(y-dy)],[dx,dy],[Pk,Pk]);*/


/*
equation(24) <= linear_aproximate_for_variables( eq(23) ,[dx,dy]

solve min (21) sub (24)

3rd coordinate is constant 
dxh/dxh3=0
dyh/dyh3=0
=> Pk.dx=dx where Pk=diag([1,1,0])
*/

lagrange_multiplier_solve(
(xt+dxh)^^2+(yt+dyh)^^2
,[
/*linear_aproximate_for_variables(inprod((xh-dxh),F.(yh-dyh)),[dxh,dyh])*/
linear_aproximate_for_variables((xh-dxh).F.(yh-dyh),[dxh,dyh])
]
,[dxh,dyh],[Pk,Pk]);


/*you will see same formulation (32) */




/* Convert to C using http://www.rbt.his.u-fukui.ac.jp/~naniwa/pub/maxima/cform.lisp */
load("cform.lisp");

result1;
load(diag);
Pk:diag([1,1,0]);
/*kill(Pk,F,x,y)*/

F:genmatrix(lambda([i,j],f[i][j]),3,3 );
x:makelist(x[i],i,1,3);
y:makelist(y[i],i,1,3);

cform(''(result1[1][1]) );