function p = midpoint(v, t)

siz = size(v);
num = siz(2);

%v1 and v2 are the direction of features in world coordinate
for m = 1:num
  v(:,m) = v(:,m) / norm(v(:,m));
end

M = zeros(3,3);
b = zeros(3,1);
for m = 1:num
  M = M + eye(3,3) - v(:,m) * v(:,m)';
  b = b + t(:,m) - (t(:,m)' * v(:,m)) * v(:,m);
end

p = inv(M) * b;
