I made Lagrange multiplier solver.
This can deal matrix without define each element.
http://d.hatena.ne.jp/niitsuma/20080325/1278561405
-----------------------
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=0 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=0 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]) );
-----------------------