Fast CVX solvers in Matlab - matlab

I am wondering what is the fastest convex optimizer in Matlab or is there any way to speed up current solvers? I'm using CVX, but it's taking forever to solve the optimization problem I have.
The optimization I have is to solve
minimize norm(Ax-b, 2)
subject to
x >= 0
and x d <= delta
where the size of A and b are very large.
Is there any way that I can solve this by a least square solver and then transfer it to the constraint version to make it faster?

I'm not sure what x.d <= delta means, but I'll just assume it's supposed to be x <= delta.
You can solve this problem using the projected gradient method or an accelerated projected gradient method (which is just a slight modification of the projected gradient method, which "magically" converges much faster). Here is some python code that shows how to minimize .5|| Ax - b ||^2 subject to the constraint that 0 <= x <= delta using FISTA, which is an accelerated projected gradient method. More details about the projected gradient method and FISTA can be found for example in Boyd's manuscript on proximal algorithms.
import numpy as np
import matplotlib.pyplot as plt
def fista(gradf,proxg,evalf,evalg,x0,params):
# This code does FISTA with line search
maxIter = params['maxIter']
t = params['stepSize'] # Initial step size
showTrigger = params['showTrigger']
increaseFactor = 1.25
decreaseFactor = .5
costs = np.zeros((maxIter,1))
xkm1 = np.copy(x0)
vkm1 = np.copy(x0)
for k in np.arange(1,maxIter+1,dtype = np.double):
costs[k-1] = evalf(xkm1) + evalg(xkm1)
if k % showTrigger == 0:
print "Iteration: " + str(k) + " cost: " + str(costs[k-1])
t = increaseFactor*t
acceptFlag = False
while acceptFlag == False:
if k == 1:
theta = 1
else:
a = tkm1
b = t*(thetakm1**2)
c = -t*(thetakm1**2)
theta = (-b + np.sqrt(b**2 - 4*a*c))/(2*a)
y = (1 - theta)*xkm1 + theta*vkm1
(gradf_y,fy) = gradf(y)
x = proxg(y - t*gradf_y,t)
fx = evalf(x)
if fx <= fy + np.vdot(gradf_y,x - y) + (.5/t)*np.sum((x - y)**2):
acceptFlag = True
else:
t = decreaseFactor*t
tkm1 = t
thetakm1 = theta
vkm1 = xkm1 + (1/theta)*(x - xkm1)
xkm1 = x
return (xkm1,costs)
if __name__ == '__main__':
delta = 5.0
numRows = 300
numCols = 50
A = np.random.randn(numRows,numCols)
ATrans = np.transpose(A)
xTrue = delta*np.random.rand(numCols,1)
b = np.dot(A,xTrue)
noise = .1*np.random.randn(numRows,1)
b = b + noise
def evalf(x):
AxMinusb = np.dot(A, x) - b
val = .5 * np.sum(AxMinusb ** 2)
return val
def gradf(x):
AxMinusb = np.dot(A, x) - b
grad = np.dot(ATrans, AxMinusb)
val = .5 * np.sum(AxMinusb ** 2)
return (grad, val)
def evalg(x):
return 0.0
def proxg(x,t):
return np.maximum(np.minimum(x,delta),0.0)
x0 = np.zeros((numCols,1))
params = {'maxIter': 500, 'stepSize': 1.0, 'showTrigger': 5}
(x,costs) = fista(gradf,proxg,evalf,evalg,x0,params)
plt.figure()
plt.plot(x)
plt.plot(xTrue)
plt.figure()
plt.semilogy(costs)

Related

How to manually code 1-norm regression as a matlab function, using the below algorithm

I am not sure if what I have done so far is correct, and I need help with the iterative step as I don't understand what is going on in the algorithm. Here is my code. Help to finish this would be much appreciated. Thank you
function x_opt = out(A,b)
%UNTITLED2 Summary of this function goes here
% Detailed explanation goes here
b_vect = b';
m = size(A,1);
n = size(1,A);
set_B = 1:n;
set_B_Comp = n+1:m;
M = inv(A(set_B, :));
is_opt = 0;
x_temp = M*b_vect(set_B);
h = A*x_temp - b_vect;
y_vect = zeros(m, 1);
y_vect(set_B_Comp) = sign(h(set_B_Comp));
y_vect(set_B) = -inv(A(set_B, :))*(A(set_B_Comp, :))'*y_vect(set_B_Comp);
abs_y_B = abs(y_vect(set_B));
if all(abs_y_B <= 1)
x_opt = x_temp;
...
else
all_index_y_vect_more_than_1 = find(abs(y_vect) >= 1);
set_B_index_y_vect_more_than_1 = intersect(set_B, all_index_y_vect_more_than_1);
s = set_B_index_y_vect_more_than_1(1);
y_s = y(s)
t_vect = zeros(m, 1);
temp = inv(A(set_B,:));
t_vect(set_B_Comp) = -(sign(y_s))*(y(set_B_Comp)).*(A(set_B_Comp, :)*temp(:, s));
cur_min = h(set_B_Comp(1))/t_vect(set_B_Comp(1)) + 1;
cur_r = set_B_Comp(1);
for j = set_B_Comp
h_j = h(j);
t_j = t_vect(j);
temp1 = abs(h_j)/t_j;
if (temp1 < cur_min) && (temp1 > 0)
cur_min = temp1;
cur_r = j;
end
end
r = cur_r;
set_B_new = union(setdiff(set_B, s), r);
set_B_Comp_new = setdiff(1:m,set_B_new);
x_new = inv(A(set_B_new, :))*b_vect(set_B_new);
end
x_opt = x_temp;
end
I don't understand what's going on in your algorithm either. It's written without comments or explanations.
However, you can model your problem as a convex optimization problem. A formulation in Python using cvxpy is then quite simple and readable:
#!/usr/bin/env python3
import cvxpy as cp
import numpy as np
# Coefficient of regularization
alpha = 1e-4
# Dimensionality
N = 400
d = 20
# Synthetic data
A = np.random.randn(N, d)
b = np.random.randn(N)
# Define and solve the CVXPY problem.
x = cp.Variable(d)
objective = cp.sum_squares(A # x - b) + alpha * cp.norm1(x)
prob = cp.Problem(cp.Minimize(objective))
optval = prob.solve()
# Print result.
print("Optimal value ", optval)
print("The optimal x is")
print(x.value)
print("The norm of the residual is ", cp.norm(A # x - b, p=2).value)
and gives
Optimal value 328.41957961297607
The optimal x is
[-0.02041302 -0.16156503 0.07215877 0.00505087 0.01188415 -0.01029848
-0.0237066 0.0370556 0.02205413 0.00137185 0.04055319 -0.01157271
0.00369032 0.06349145 0.07494259 -0.04172275 0.04376864 0.02698337
-0.04696984 0.05245699]
The norm of the residual is 18.122348149231115

Modelling membrane evolution over time

I am trying to model the time evolution of a membrane based on the following code in MATLAB.
The basic outline is that the evolution is based on a differential equation
where j=0,1 and x^0 = x, x^1 = y and x^j(s_i) = x^j_i.
My code is the following.
import numpy as np
from matplotlib import pyplot as plt
R0 = 5 #radius
N = 360 #number of intervals
x0 = 2*np.pi*R0/(N/2) #resting membrane lengths
phi = np.linspace(0,2*np.pi, num=360, dtype=float)
R1 = R0 + 0.5*np.sin(20*phi)
X = R1*np.cos(phi)
Y = R1*np.sin(phi)
L = np.linspace(-1,358, num=360, dtype=int)
R = np.linspace(1,360, num=360,dtype=int) #right and left indexing vectors
R[359] = 0
X = R1*np.cos(phi)
Y = R1*np.sin(phi)
plt.plot(X,Y)
plt.axis("equal")
plt.show()
ds = 1/N
ds2 = ds**2
k = 1/10
w = 10**6
for i in range(0,20000):
lengths = np.sqrt( (X[R]-X)**2 + (Y[R]-Y)**2 )
Ex = k/ds2*(X[R] - 2*X + X[L] - x0*( (X[R]-X)/lengths - (X-X[L])/lengths[L]) )
Ey = k/ds2*(Y[R] - 2*Y + Y[L] - x0*( (Y[R]-Y)/lengths - (Y-Y[L])/lengths[L]) )
X = X + 1/w*Ex
Y = Y + 1/w*Ey
plt.plot(X,Y)
plt.axis("equal")
plt.show()
The model is supposed to devolve into a circular membrane, as below
but this is what mine does
Your definition of x0 is wrong.
In the Matlab code, it is equal to
x0 = 2*pi*R/N/2 # which is pi*R/N
while in your Python code it is
x0 = 2*np.pi*R0/(N/2) # which is 4*np.pi*R0/N
Correcting that, the end result is a circular shape, but with a different radius. I'm assuming that this is because of the reduced number of iterations (20000 instead of 1000000).
Edit:
As expected, using the correct number of iterations results in a plot similar to your expected one.

How to reduce the time consumed by the for loop?

I am trying to implement a simple pixel level center-surround image enhancement. Center-surround technique makes use of statistics between the center pixel of the window and the surrounding neighborhood as a means to decide what enhancement needs to be done. In the code given below I have compared the center pixel with average of the surrounding information and based on that I switch between two cases to enhance the contrast. The code that I have written is as follows:
im = normalize8(im,1); %to set the range of pixel from 0-255
s1 = floor(K1/2); %K1 is the size of the window for surround
M = 1000; %is a constant value
out1 = padarray(im,[s1,s1],'symmetric');
out1 = CE(out1,s1,M);
out = (out1(s1+1:end-s1,s1+1:end-s1));
out = normalize8(out,0); %to set the range of pixel from 0-1
function [out] = CE(out,s,M)
B = 255;
out1 = out;
for i = s+1 : size(out,1) - s
for j = s+1 : size(out,2) - s
temp = out(i-s:i+s,j-s:j+s);
Yij = out1(i,j);
Sij = (1/(2*s+1)^2)*sum(sum(temp));
if (Yij>=Sij)
Aij = A(Yij-Sij,M);
out1(i,j) = ((B + Aij)*Yij)/(Aij+Yij);
else
Aij = A(Sij-Yij,M);
out1(i,j) = (Aij*Yij)/(Aij+B-Yij);
end
end
end
out = out1;
function [Ax] = A(x,M)
if x == 0
Ax = M;
else
Ax = M/x;
end
The code does the following things:
1) Normalize the image to 0-255 range and pad it with additional elements to perform windowing operation.
2) Calls the function CE.
3) In the function CE obtain the windowed image(temp).
4) Find the average of the window (Sij).
5) Compare the center of the window (Yij) with the average value (Sij).
6) Based on the result of comparison perform one of the two enhancement operation.
7) Finally set the range back to 0-1.
I have to run this for multiple window size (K1,K2,K3, etc.) and the images are of size 1728*2034. When the window size is selected as 100, the time consumed is very high.
Can I use vectorization at some stage to reduce the time for loops?
The profiler result (for window size 21) is as follows:
The profiler result (for window size 100) is as follows:
I have changed the code of my function and have written it without the sub-function. The code is as follows:
function [out] = CE(out,s,M)
B = 255;
Aij = zeros(1,2);
out1 = out;
n_factor = (1/(2*s+1)^2);
for i = s+1 : size(out,1) - s
for j = s+1 : size(out,2) - s
temp = out(i-s:i+s,j-s:j+s);
Yij = out1(i,j);
Sij = n_factor*sum(sum(temp));
if Yij-Sij == 0
Aij(1) = M;
Aij(2) = M;
else
Aij(1) = M/(Yij-Sij);
Aij(2) = M/(Sij-Yij);
end
if (Yij>=Sij)
out1(i,j) = ((B + Aij(1))*Yij)/(Aij(1)+Yij);
else
out1(i,j) = (Aij(2)*Yij)/(Aij(2)+B-Yij);
end
end
end
out = out1;
There is a slight improvement in the speed from 93 sec to 88 sec. Suggestions for any other improvements to my code are welcomed.
I have tried to incorporate the suggestions given to replace sliding window with convolution and then vectorize the rest of it. The code below is my implementation and I'm not getting the result expected.
function [out_im] = CE_conv(im,s,M)
B = 255;
temp = ones(2*s,2*s);
temp = temp ./ numel(temp);
out1 = conv2(im,temp,'same');
out_im = im;
Aij = im-out1; %same as Yij-Sij
Aij1 = out1-im; %same as Sij-Yij
Mij = Aij;
Mij(Aij>0) = M./Aij(Aij>0); % if Yij>Sij Mij = M/Yij-Sij;
Mij(Aij<0) = M./Aij1(Aij<0); % if Yij<Sij Mij = M/Sij-Yij;
Mij(Aij==0) = M; % if Yij-Sij == 0 Mij = M;
out_im(Aij>=0) = ((B + Mij(Aij>=0)).*im(Aij>=0))./(Mij(Aij>=0)+im(Aij>=0));
out_im(Aij<0) = (Mij(Aij<0).*im(Aij<0))./ (Mij(Aij<0)+B-im(Aij<0));
I am not able to figure out where I'm going wrong.
A detailed explanation of what I'm trying to implement is given in the following paper:
Vonikakis, Vassilios, and Ioannis Andreadis. "Multi-scale image contrast enhancement." In Control, Automation, Robotics and Vision, 2008. ICARCV 2008. 10th International Conference on, pp. 856-861. IEEE, 2008.
I've tried to see if I could get those times down by processing with colfiltand nlfilter, since both are usually much faster than for-loops for sliding window image processing.
Both worked fine for relatively small windows. For an image of 2048x2048 pixels and a window of 10x10, the solution with colfilt takes about 5 seconds (on my personal computer). With a window of 21x21 the time jumped to 27 seconds, but that is still a relative improvement on the times displayed on the question. Unfortunately I don't have enough memory to colfilt using windows of 100x100, but the solution with nlfilter works, though taking about 120 seconds.
Here the code
Solution with colfilt:
function outval = enhancematrix(inputmatrix,M,B)
%Inputmatrix is a 2D matrix or column vector, outval is a 1D row vector.
% If inputmatrix is made of integers...
inputmatrix = double(inputmatrix);
%1. Compute S and Y
normFactor = 1 / (size(inputmatrix,1) + 1).^2; %Size of column.
S = normFactor*sum(inputmatrix,1); % Sum over the columns.
Y = inputmatrix(ceil(size(inputmatrix,1)/2),:); % Center row.
% So far we have all S and Y, one value per column.
%2. Compute A(abs(Y-S))
A = Afunc(abs(S-Y),M);
% And all A: one value per column.
%3. The tricky part. If Y(i)-S(i) > 0 do something.
doPositive = (Y > S);
doNegative = ~doPositive;
outval = zeros(1,size(inputmatrix,2));
outval(doPositive) = (B + A(doPositive) .* Y(doPositive)) ./ (A(doPositive) + Y(doPositive));
outval(doNegative) = (A(doNegative) .* Y(doNegative)) ./ (A(doNegative) + B - Y(doNegative));
end
function out = Afunc(x,M)
% Input x is a row vector. Output is another row vector.
out = x;
out(x == 0) = M;
out(x ~= 0) = M./x(x ~= 0);
end
And to call it, simply do:
M = 1000; B = 255; enhancenow = #(x) enhancematrix(x,M,B);
w = 21 % windowsize
result = colfilt(inputImage,[w w],'sliding',enhancenow);
Solution with nlfilter:
function outval = enhanceimagecontrast(neighbourhood,M,B)
%1. Compute S and Y
normFactor = 1 / (length(neighbourhood) + 1).^2;
S = normFactor*sum(neighbourhood(:));
Y = neighbourhood(ceil(size(neighbourhood,1)/2),ceil(size(neighbourhood,2)/2));
%2. Compute A(abs(Y-S))
test = (Y>=S);
A = Afunc(abs(Y-S),M);
%3. Return outval
if test
outval = ((B + A) * Y) / (A + Y);
else
outval = (A * Y) / (A + B - Y);
end
function aval = Afunc(x,M)
if (x == 0)
aval = M;
else
aval = M/x;
end
And to call it, simply do:
M = 1000; B = 255; enhancenow = #(x) enhanceimagecontrast(x,M,B);
w = 21 % windowsize
result = nlfilter(inputImage,[w w], enhancenow);
I didn't spend much time checking that everything is 100% correct, but I did see some nice contrast enhancement (hair looks particularly nice).
This answer is the implementation that was suggested by Peter. I debugged the implementation and presenting the final working version of the fast implementation.
function [out_im] = CE_conv(im,s,M)
B = 255;
im = ( im - min(im(:)) ) ./ ( max(im(:)) - min(im(:)) )*255;
h = ones(s,s)./(s*s);
out1 = imfilter(im,h,'conv');
out_im = im;
Aij = im-out1; %same as Yij-Sij
Aij1 = out1-im; %same as Sij-Yij
Mij = Aij;
Mij(Aij>0) = M./Aij(Aij>0); % if Yij>Sij Mij = M/(Yij-Sij);
Mij(Aij<0) = M./Aij1(Aij<0); % if Yij<Sij Mij = M/(Sij-Yij);
Mij(Aij==0) = M; % if Yij-Sij == 0 Mij = M;
out_im(Aij>=0) = ((B + Mij(Aij>=0)).*im(Aij>=0))./(Mij(Aij>=0)+im(Aij>=0));
out_im(Aij<0) = (Mij(Aij<0).*im(Aij<0))./ (Mij(Aij<0)+B-im(Aij<0));
out_im = ( out_im - min(out_im(:)) ) ./ ( max(out_im(:)) - min(out_im(:)) );
To call this use the following code
I = imread('pout.tif');
w_size = 51;
M = 4000;
output = CE_conv(I(:,:,1),w_size,M);
The output for the 'pout.tif' image is given below
The execution time for Bigger image and with 100*100 block size is around 5 secs with this implementation.

How can I fit a cosine function?

I wrote a python function to get the parameters of the following cosine function:
param = Parameters()
param.add( 'amp', value = amp_guess, min = 0.1 * amp_guess, max = amp_guess )
param.add( 'off', value = off_guess, min = -10, max = 10 )
param.add( 'shift', value = shift_guess[0], min = 0, max = 2 * np.pi, )
fit_values = minimize( self.residual, param, args = ( azi_unique, los_unique ) )
def residual( self, param, azi, data ):
"""
Parameters
----------
Returns
-------
"""
amp = param['amp'].value
off = param['off'].value
shift = param['shift'].value
model = off + amp * np.cos( azi - shift )
return model - data
In Matlab how can get the amplitude, offset and shift of the cosine function?
My experience tells me that it's always good to depend as little as possible on toolboxes. For your particular case, the model is simple and doing it manually is pretty straightforward.
Assuming that you have the following model:
y = B + A*cos(w*x + phi)
and that your data is equally-spaced, then:
%// Create some bogus data
A = 8;
B = -4;
w = 0.2;
phi = 1.8;
x = 0 : 0.1 : 8.4*pi;
y = B + A*cos(w*x + phi) + 0.5*randn(size(x));
%// Find kick-ass initial estimates
L = length(y);
N = 2^nextpow2(L);
B0 = (max(y(:))+min(y(:)))/2;
Y = fft(y-B0, N)/L;
f = 5/(x(2)-x(1)) * linspace(0,1,N/2+1);
[A0,I] = max( 2*abs(Y(1:N/2+1)) );
w0 = f(I);
phi0 = 2*imag(Y(I));
%// Refine the fit
sol = fminsearch(#(t) sum( (y(:)-t(1)-t(2)*cos(t(3)*x(:)+t(4))).^2 ), [B0 A0 w0 phi0])
Results:
 
sol = %// B was -4 A was 8 w was 0.2 phi was 1.8
-4.0097e+000 7.9913e+000 1.9998e-001 1.7961e+000
MATLAB has a function called lsqcurvefit in the optimisation toolbox:
lsqcurvefit(fun,X0,xdata,ydata,lbound,ubound);
where fun is the function to fit, x0 is the initial parameter guess, xdata and ydata are self-explanatory, and lbound and ubound are the lower and upper bounds to the parameters. So, for instance, you might have a function:
% x(1) = amp
% x(2) = shift
% x(3) = offset
% note cosd instead of cos, because your data appears to be in degrees
cosfit = #(x,xdata) x(1) .* cosd(xdata - x(2)) + x(3);
You would then call the lsqcurvefit function as follows:
guess = [7,150,0.5];
lbound = [-10,0,-10]
ubound = [10,360,10]
fit_values = lsqcurvefit(cosfit,guess,azi_unique,los_unique,lbound,ubound);

How can I find a matching between two independent sets of features extracted from two images of the same scene captured by two different cameras?

I need to find matching between two independent sets of features extracted from two images of the same scene captured by two different cameras.
I'm using the HumanEvaI data set, so I have the calibration matrices of the cameras (in this particular case BW1 and BW2).
I can not use method like simple correlation, SIFT or SURF to solve the problem because the cameras are quite far away from each other and also rotated. So the differences between the images are big and there is occlusion as well.
I have been focused in finding an Homography between the captured images based on ground truth points matching that I have been able to build due to the calibration information I already have.
Once I have this homography I will use a perfect matching (Hungarian algorithm) to find the best correspondence. The importance of the homography here is that is the way I have to calculate the distance between the points.
So far everything seems fine, my problem is that I haven't been able to find a good homography . I have tried RANSAC method, gold standard method with sampson distance (this is a nonlinear optimization approach) and mainly everything from a book called 'Multiple View Geometry in Computer Vision' Second Edition by Richard Hartley.
I have implemented everything in matlab so far.
Is there another way to do this? I'm I in the right path? If so what could I have been doing wrong?
You can try these two methods:
A new point matching algorithm for non-rigid registration (uses Thin-plate Spline) - relatively slower.
Point Set Registration: Coherent Point Drift (faster and more accurate I feel).
As far as 2nd method is concerned, I feel that it gives very good registration result in presence of outliers, it is fast and is able to recover complex transformations. But the 1st method is also a well-known method in registration field and you may try that as well.
Try understanding the core of the algorithm and then move on to the codes available online.
Thin plate spline here - Download the TPS-RPM demo.
Point Set Registration: Coherent Point Drift here
You might find my solution interesting. It is a pure numpy implementation of the Coherent Point Drift algorithm.
Here is an example:
from functools import partial
from scipy.io import loadmat
import matplotlib.pyplot as plt
import numpy as np
import time
class RigidRegistration(object):
def __init__(self, X, Y, R=None, t=None, s=None, sigma2=None, maxIterations=100, tolerance=0.001, w=0):
if X.shape[1] != Y.shape[1]:
raise 'Both point clouds must have the same number of dimensions!'
self.X = X
self.Y = Y
(self.N, self.D) = self.X.shape
(self.M, _) = self.Y.shape
self.R = np.eye(self.D) if R is None else R
self.t = np.atleast_2d(np.zeros((1, self.D))) if t is None else t
self.s = 1 if s is None else s
self.sigma2 = sigma2
self.iteration = 0
self.maxIterations = maxIterations
self.tolerance = tolerance
self.w = w
self.q = 0
self.err = 0
def register(self, callback):
self.initialize()
while self.iteration < self.maxIterations and self.err > self.tolerance:
self.iterate()
callback(X=self.X, Y=self.Y)
return self.Y, self.s, self.R, self.t
def iterate(self):
self.EStep()
self.MStep()
self.iteration = self.iteration + 1
def MStep(self):
self.updateTransform()
self.transformPointCloud()
self.updateVariance()
def updateTransform(self):
muX = np.divide(np.sum(np.dot(self.P, self.X), axis=0), self.Np)
muY = np.divide(np.sum(np.dot(np.transpose(self.P), self.Y), axis=0), self.Np)
self.XX = self.X - np.tile(muX, (self.N, 1))
YY = self.Y - np.tile(muY, (self.M, 1))
self.A = np.dot(np.transpose(self.XX), np.transpose(self.P))
self.A = np.dot(self.A, YY)
U, _, V = np.linalg.svd(self.A, full_matrices=True)
C = np.ones((self.D, ))
C[self.D-1] = np.linalg.det(np.dot(U, V))
self.R = np.dot(np.dot(U, np.diag(C)), V)
self.YPY = np.dot(np.transpose(self.P1), np.sum(np.multiply(YY, YY), axis=1))
self.s = np.trace(np.dot(np.transpose(self.A), self.R)) / self.YPY
self.t = np.transpose(muX) - self.s * np.dot(self.R, np.transpose(muY))
def transformPointCloud(self, Y=None):
if not Y:
self.Y = self.s * np.dot(self.Y, np.transpose(self.R)) + np.tile(np.transpose(self.t), (self.M, 1))
return
else:
return self.s * np.dot(Y, np.transpose(self.R)) + np.tile(np.transpose(self.t), (self.M, 1))
def updateVariance(self):
qprev = self.q
trAR = np.trace(np.dot(self.A, np.transpose(self.R)))
xPx = np.dot(np.transpose(self.Pt1), np.sum(np.multiply(self.XX, self.XX), axis =1))
self.q = (xPx - 2 * self.s * trAR + self.s * self.s * self.YPY) / (2 * self.sigma2) + self.D * self.Np/2 * np.log(self.sigma2)
self.err = np.abs(self.q - qprev)
self.sigma2 = (xPx - self.s * trAR) / (self.Np * self.D)
if self.sigma2 <= 0:
self.sigma2 = self.tolerance / 10
def initialize(self):
self.Y = self.s * np.dot(self.Y, np.transpose(self.R)) + np.repeat(self.t, self.M, axis=0)
if not self.sigma2:
XX = np.reshape(self.X, (1, self.N, self.D))
YY = np.reshape(self.Y, (self.M, 1, self.D))
XX = np.tile(XX, (self.M, 1, 1))
YY = np.tile(YY, (1, self.N, 1))
diff = XX - YY
err = np.multiply(diff, diff)
self.sigma2 = np.sum(err) / (self.D * self.M * self.N)
self.err = self.tolerance + 1
self.q = -self.err - self.N * self.D/2 * np.log(self.sigma2)
def EStep(self):
P = np.zeros((self.M, self.N))
for i in range(0, self.M):
diff = self.X - np.tile(self.Y[i, :], (self.N, 1))
diff = np.multiply(diff, diff)
P[i, :] = P[i, :] + np.sum(diff, axis=1)
c = (2 * np.pi * self.sigma2) ** (self.D / 2)
c = c * self.w / (1 - self.w)
c = c * self.M / self.N
P = np.exp(-P / (2 * self.sigma2))
den = np.sum(P, axis=0)
den = np.tile(den, (self.M, 1))
den[den==0] = np.finfo(float).eps
self.P = np.divide(P, den)
self.Pt1 = np.sum(self.P, axis=0)
self.P1 = np.sum(self.P, axis=1)
self.Np = np.sum(self.P1)
def visualize(X, Y, ax):
plt.cla()
ax.scatter(X[:,0] , X[:,1], color='red')
ax.scatter(Y[:,0] , Y[:,1], color='blue')
plt.draw()
plt.pause(0.001)
def main():
fish = loadmat('./data/fish.mat')
X = fish['X'] # number-of-points x number-of-dimensions array of fixed points
Y = fish['Y'] # number-of-points x number-of-dimensions array of moving points
fig = plt.figure()
fig.add_axes([0, 0, 1, 1])
callback = partial(visualize, ax=fig.axes[0])
reg = RigidRegistration(X, Y)
reg.register(callback)
plt.show()
if __name__ == '__main__':
main()
Another method I think you might find useful is described here.
This approach tries to fit local models to group of points. Its global optimization method allows it to outperform RANSAC when several distinct local models exists.
I also believe they have code available.