Skip to content
Snippets Groups Projects
Commit 556665b2 authored by Andrea Censi's avatar Andrea Censi
Browse files

ok: covarianza uguale; adesso pulisco il debug

parent 486aaa3f
No related branches found
No related tags found
No related merge requests found
......@@ -46,14 +46,15 @@ class ICP
journal "iteration #{iteration}"
journal "x_old #{to_j(x_old)}"
find_correspondences_tricks(x_old)
x_new, dx_dy1, dx_dy2 = compute_next_estimate( x_old)
if params[:useCorrTricks] == 1
find_correspondences_tricks(x_old)
else
find_correspondences(x_old)
end
x_new = compute_next_estimate(x_old)
sigma = 0.01 # cm
cov_x = (sigma*sigma) * dx_dy1 * (dx_dy1.trans) +
(sigma*sigma) * dx_dy2 * (dx_dy2.trans);
sigma = params[:sigma]
journal "x_new #{to_j(x_new)}"
......@@ -61,9 +62,8 @@ class ICP
new_delta = pose_diff(x_new, x_old)
journal "delta #{to_j(new_delta)}"
# puts "#{iteration} x_new = #{pv(x_new)} delta = #{pv(new_delta)}"+
# " neg #{new_delta.trans * delta>0} error #{@total_error} "
# puts " cov_x #{pm(cov_x)}"
puts "#{iteration} x_old = #{pv(x_old)} x_new = #{pv(x_new)} "
delta = new_delta
if delta[0,1].nrm2 < params[:epsilon_xy] &&
......@@ -75,6 +75,38 @@ class ICP
x_old = x_new
end
dx_dy1=Matrix.alloc(3,laser_ref.nrays)
dx_dy2=Matrix.alloc(3,laser_sens.nrays)
if 1 == params[:doComputeCovariance]
dx_dy1, dx_dy2 =
compute_covariance_exact(laser_ref, laser_sens, x_new)
end
cov_x = (sigma*sigma) * dx_dy1 * (dx_dy1.trans) +
(sigma*sigma) * dx_dy2 * (dx_dy2.trans);
puts "sigma : #{sigma}"
# puts "dx_dy1 : #{dx_dy1}"
# puts "dx_dy2 : #{dx_dy2}"
puts "cov_x : #{cov_x}"
if false
num_dx_dy1, num_dx_dy2 =
compute_covariance(laser_ref, laser_sens, x_new)
num_cov_x = (sigma*sigma) * num_dx_dy1 * (num_dx_dy1.trans) +
(sigma*sigma) * num_dx_dy2 * (num_dx_dy2.trans)
puts "num_dx_dy1 : #{num_dx_dy1}"
puts "num_dx_dy2 : #{num_dx_dy2}"
puts "num_cov_x : #{num_cov_x}"
end
res = Hash.new
res[:x] = x_new
res[:iterations] = iteration
......@@ -124,18 +156,10 @@ class ICP
# journal_correspondences("candidate", correspondences)
journal_correspondences("correspondences", laser_sens.corr)
corrs = corrs.compact
puts "Found #{corrs.size} correspondences."
corrs = corrs.compact
puts "Found #{corrs.size} correspondences."
x_new = GPC.gpc(corrs)
# cov = compute_covariance(laser_ref, laser_sens, correspondences, x_old)
# dx_dy1, dx_dy2 =
# compute_covariance_exact(laser_ref, laser_sens, x_old)
dx_dy1=Matrix.alloc(3,laser_ref.nrays)
dx_dy2=Matrix.alloc(3,laser_sens.nrays)
return x_new, dx_dy1, dx_dy2
return x_new
end
end
......@@ -195,8 +195,8 @@ class ICP
if j_up.nil? or j_down.nil?
[j_up,j_down].compact[0]
else
p_prev = laser_ref.p[j_up]
p_next = laser_ref.p[j_down]
p_prev = laser_ref.p[j_down]
p_next = laser_ref.p[j_up]
dist_prev = (p_prev-p_i_w).nrm2;
dist_next = (p_next-p_i_w).nrm2;
if dist_prev < dist_next then j_down else j_up end
......
......@@ -27,12 +27,34 @@ class ICP
v_alpha = v_alpha / v_alpha.nrm2
m = v_alpha*v_alpha.trans
d2J_dt2 += 2 * m
d2J_dt_dtheta += 2 * (rot(theta+PI/2)*p_k).trans * m
d2J_dtheta2 += 2 * (rot(theta)*p_k+t-q_k).trans *
d2J_dt2_k = 2*m
d2J_dt_dtheta_k = 2 * (rot(theta+PI/2)*p_k).trans * m
d2J_dtheta2_k = 2 * (rot(theta)*p_k+t-q_k).trans *
m * rot(theta+PI/2) * p_k + 2 * (rot(theta+PI/2)*p_k).trans * m *
rot(theta+PI/2) * p_k
if i == 45
d2J_dtheta2_k_1 = 2 * (rot(theta)*p_k+t-q_k).trans *
m * rot(theta+PI/2) * p_k;
d2J_dtheta2_k_2 = 2 * (rot(theta+PI/2)*p_k).trans * m *
rot(theta+PI/2) * p_k;
puts "d2J_dtheta2_k =\n #{d2J_dtheta2_k}"
puts "d2J_dtheta2_k_1 =\n #{d2J_dtheta2_k_1}"
puts "d2J_dtheta2_k_2 =\n #{d2J_dtheta2_k_2}"
puts "t = #{t}"
puts "theta = #{theta}"
puts "p_k = #{p_k}"
puts "q_k = #{q_k}"
puts "v2 = #{(rot(theta)*p_k+t-q_k)}"
end
d2J_dt2 += d2J_dt2_k
d2J_dt_dtheta += d2J_dt_dtheta_k
d2J_dtheta2 += d2J_dtheta2_k
###########
......@@ -51,10 +73,9 @@ class ICP
dC_drho_j1, dC_drho_j2 = dC_drho_j12(laser_ref, laser_sens, j1, j2)
v_j1 = laser_ref.v(j1)
v_j2 = laser_ref.v(j2)
d2Jk_dtheta_drho_j1 = 2 * ( -v_j1.trans*m+(rot(theta)*p_k+t-q_k).trans*dC_drho_j1)*
rot(theta+PI/2)*p_k
d2Jk_dtheta_drho_j1 =
2*( -v_j1.trans*m+(rot(theta)*p_k+t-q_k).trans*dC_drho_j1)*rot(theta+PI/2)*p_k
d2Jk_dt_drho_j1 = 2 * (-v_j1.trans*m+(rot(theta)*p_k+t-q_k).trans*dC_drho_j1)
d2J_dxdy1.col(j1)[0] += d2Jk_dt_drho_j1[0]
......@@ -71,7 +92,19 @@ class ICP
d2J_dxdy1.col(j2)[1] += d2Jk_dt_drho_j2[1]
d2J_dxdy1.col(j2)[2] += d2Jk_dtheta_drho_j2
if i==45 then
puts "Corr i=#{i} j1=#{j1} j2=#{j2}"
puts "C_k=\n#{m}";
puts "dC_drho_j1=\n#{dC_drho_j1}"
puts "dC_drho_j2=\n#{dC_drho_j2}"
d =
2 * (rot(theta)*p_k+t-q_k).trans *
m * rot(theta+PI/2) * p_k + 2 * (rot(theta+PI/2)*p_k).trans * m *
rot(theta+PI/2) * p_k
puts "Contribution = \n#{d}"
end
end
# put the pieces together
d2J_dx2 = Matrix.alloc(3,3)
d2J_dx2[0,0]=d2J_dt2[0,0]
......@@ -82,9 +115,17 @@ class ICP
d2J_dx2[2,1]=d2J_dx2[1,2]=d2J_dt_dtheta[1]
d2J_dx2[2,2] = d2J_dtheta2
puts "d2J_dx2 = #{d2J_dx2}"
puts "inv(d2J_dx2) = #{d2J_dx2.inv}"
dx_dy1 = -d2J_dx2.inv * d2J_dxdy1
dx_dy2 = -d2J_dx2.inv * d2J_dxdy2
j1=laser_sens.corr[24].j1
j2=laser_sens.corr[24].j2
puts "cov0_x = #{dx_dy1*dx_dy1.trans+dx_dy2*dx_dy2.trans}"
return dx_dy1, dx_dy2
end
......
class ICP
def compute_covariance(laser_ref, laser_sens, correspondences, x_new)
fJ = create_J_function(laser_ref, laser_sens, correspondences)
def compute_covariance(laser_ref, laser_sens, x_new)
fJ = create_J_function(laser_ref, laser_sens)
mx = x_new
my1 = Vector.alloc(laser_ref.points.map{|p| p.reading}).col
my2 = Vector.alloc(laser_sens.points.map{|p| p.reading}).col
my1 = Vector.alloc(laser_ref.readings).col
my2 = Vector.alloc(laser_sens.readings).col
# tot = fJ.call(x,y1,y2)
# puts "tot= #{tot} , total_error = #{@total_error}"
......@@ -76,17 +76,23 @@ class ICP
puts "d2J_dxdy1 = #{d2J_dxdy1.trans}"
puts "d2J_dxdy2 = #{d2J_dxdy2.trans}"
return d2, d2J_dxdy1, d2J_dxdy2
dx_dy1 = - d2.inv * d2J_dxdy1
dx_dy2 = - d2.inv * d2J_dxdy2
return dx_dy1, dx_dy2
end
def create_J_function(laser_ref, laser_sens, correspondences)
def create_J_function(laser_ref, laser_sens)
Proc.new { |x,y1,y2|
fJtot = 0;
for c in correspondences
next if c.nil?
p_i = laser_sens.points[c.i ].v * y2[c.i]
p_j1 = laser_ref .points[c.j1].v * y1[c.j1]
p_j2 = laser_ref .points[c.j2].v * y1[c.j2]
for i in 0..laser_sens.nrays-1
next if not laser_sens.corr[i].valid
j1 = laser_sens.corr[i].j1
j2 = laser_sens.corr[i].j2
p_i = laser_sens.v(i) * y2[i]
p_j1 = laser_ref.v(j1) * y1[j1]
p_j2 = laser_ref.v(j2) * y1[j2]
v_alpha = rot(PI/2) * (p_j1-p_j2)
v_alpha = v_alpha / v_alpha.nrm2
......
......@@ -59,9 +59,9 @@ class LaserData
for i in 0..nrays-1
@p[i] = Vector[GSL::NAN,GSL::NAN].col
@corr[i] = Correspondence.new;
@corr[i].valid = false;
@corr[i].j1 = -1;
@corr[i].j2 = -1;
@corr[i].valid = false
@corr[i].j1 = -1
@corr[i].j2 = -1
end
# jump tables
......@@ -94,33 +94,6 @@ class LaserData
end
end
=begin
class LaserPoint
include MathUtils
attr_accessor :reading
attr_accessor :intensity
attr_accessor :theta
# Estimated alpha (relative to robot)
attr_accessor :alpha
attr_accessor :alpha_valid
# Covariance of estimated alpha (relative to robot)
attr_accessor :cov_alpha
# Cluster id (integer)
attr_accessor :cluster
attr_accessor :valid
def alpha_valid?; @alpha_valid end
def valid?; @valid end
def v; MathUtils.vers(theta) end
def cartesian; MathUtils.vers(theta)*reading; end
end
=end
def standard_parameters
#include MathUtils
p = Hash.new
......@@ -138,7 +111,9 @@ def standard_parameters
p[:clusteringThreshold] = 0.05
p[:orientationNeighbourhood] = 3
p[:useCorrTricks] = 1;
p[:doComputeCovariance] = 1;
p[:doAlphaTest] = 1
p[:doAlphaTest_thresholdDeg]=20
......
......@@ -55,6 +55,7 @@ def scan_matching(io, scan_matcher)
icp.params[:firstGuess] = u
icp.params[:maxIterations] = 20
m = Matching.new
m.x, m.dx_dy1, m.dx_dy2 = icp.scan_matching
......
......@@ -48,8 +48,10 @@ def scan_matching(klass,scan_list,input,output,params)
sm.params[:laser_sens] = laser_sens;
sm.params[:firstGuess] = u
# sm.params[:laser_sens] = laser_ref;
# sm.params[:firstGuess] = GSL::Vector.alloc(0.2,0.2,deg2rad(30))
if true
sm.params[:laser_sens] = laser_ref;
sm.params[:firstGuess] = GSL::Vector.alloc(0,0,0)
end
res = nil
realtime = Benchmark.realtime do
......
......@@ -39,7 +39,7 @@ int main(int argc, const char*argv[]) {
params.doAlphaTest = 0;
params.outliers_maxPerc = 0.85;
params.doVisibilityTest = 1;
params.doComputeCovariance = 0;
params.doComputeCovariance = 1;
int num_matchings = 0;
int num_iterations = 0;
......
......@@ -78,11 +78,13 @@ void egsl_pop() {
assert(cid>=0);//, "No egsl_push before?");
egsl_contexts[cid].nvars = 0;
cid--;
#if 0
if(cid==0) {
printf("egsl: total allocations: %d cache hits:%d\n",
egsl_total_allocations, egsl_cache_hits);
printf("egsl: sizeof(val) = %d\n",(int)sizeof(val));
}
#endif
}
val egsl_alloc(size_t rows, size_t columns) {
......
......@@ -6,7 +6,7 @@
#define atv(v,i) egsl_atv(v,i)
#define atm(v,i,j) egsl_atm(v,i,j)
#define sub(v1,v2) egsl_sub(v1,v2)
#define minus(v) egsl_scale(-1.0,v1)
#define minus(v) egsl_scale(-1.0,v)
#define sum(v1,v2) egsl_sum(v1,v2)
#define sum3(v1,v2,v3) egsl_sum(v1,egsl_sum(v2,v3))
#define tr(v) egsl_transpose(v)
......
......@@ -21,7 +21,7 @@ val dC_drho(val p1, val p2) {
val C_k = compute_C_k(p1, p2);
val p1b = sum(p1, sc(eps/egsl_norm(p1),p1));
val C_k_eps = compute_C_k(p1b,p2);
return sc(1/eps, sub(C_k, C_k_eps));
return sc(1/eps, sub(C_k_eps,C_k));
}
......@@ -65,22 +65,47 @@ void compute_covariance_exact(
val v4 = vers(theta + laser_sens->theta[i] + M_PI/2);
val C_k = compute_C_k(p_j1, p_j2);
add_to(d2J_dt2, sc(2.0, C_k));
add_to(d2J_dt_dtheta, sc(2.0,m(C_k,v1)) ); // FIXME: check this
add_to(d2J_dtheta2, sc(2.0, sum( m3(tr(v2),C_k,v1), m3(tr(v1),C_k,v1))));
val d2J_dt2_k = sc(2.0, C_k);
val d2J_dt_dtheta_k = sc(2.0,m(C_k,v1));
val d2J_dtheta2_k = sc(2.0, sum( m3(tr(v2),C_k,v1), m3(tr(v1),C_k,v1)));
val d2J_dtheta2_k_1 = sc(2.0, m3(tr(v2),C_k,v1));
val d2J_dtheta2_k_2 = sc(2.0, m3(tr(v1),C_k,v1));
// 2 * (rot(theta)*p_k+t-q_k).trans * m * rot(theta+PI/2) * p_k
// + 2 * (rot(theta+PI/2)*p_k).trans * m * rot(theta+PI/2) * p_k
if(i==45) {
egsl_print("t",t);
printf("theta = %f \n",theta);
egsl_print("p_i",p_i);
egsl_print("p_j1",p_j1);
egsl_print("v2",v2);
// egsl_print("d2J_dt2_k",d2J_dt2_k);
// egsl_print("d2J_dt_dtheta_k",d2J_dt_dtheta_k);
egsl_print("d2J_dtheta2_k",d2J_dtheta2_k);
egsl_print("d2J_dtheta2_k_1",d2J_dtheta2_k_1);
egsl_print("d2J_dtheta2_k_2",d2J_dtheta2_k_2);
}
add_to(d2J_dt2, d2J_dt2_k);
add_to(d2J_dt_dtheta, d2J_dt_dtheta_k ); // FIXME: check this
add_to(d2J_dtheta2, d2J_dtheta2_k);
// ^-- ok
// for measurement rho_i in the second scan
val d2Jk_dtdrho_i = sc(2.0, m(C_k,v3)); // FIXME: check this
val d2Jk_dtheta_drho_i = sc(2.0, sum( m3(tr(v2),C_k,v4), m3(tr(v3),C_k,v1)));
add_to_col(d2J_dxdy2, (size_t)i, comp_col(d2Jk_dtdrho_i, d2Jk_dtheta_drho_i));
add_to_col(d2J_dxdy2, (size_t)i, comp_col(d2Jk_dtdrho_i, d2Jk_dtheta_drho_i));
// for measurements rho_j1, rho_j2 in the first scan
val dC_drho_j1 = dC_drho(p_j1, p_j2);
val dC_drho_j2 = dC_drho(p_j2, p_j1);
val v_j1 = vers(laser_ref->theta[j1]);
val v_j2 = vers(laser_ref->theta[j2]);
val d2Jk_dt_drho_j1 = sum(sc(-2.0,m(C_k,v_j1)), sc(2.0,m(dC_drho_j1,v2)));
val d2Jk_dtheta_drho_j1 = sum( sc(-2.0, m3(tr(v_j1),C_k,v1)), m3(tr(v2),dC_drho_j1,v1));
......@@ -91,17 +116,27 @@ void compute_covariance_exact(
val d2Jk_dtheta_drho_j2 = sc(2.0, m3( tr(v2), dC_drho_j2, v1));
add_to_col(d2J_dxdy1, (size_t)j2, comp_col(d2Jk_dt_drho_j2, d2Jk_dtheta_drho_j2));
if(i==45) {
printf("Corr i=%d j1=%d j2=%d\n",i,j1,j2);
egsl_print("C_k ",C_k);
egsl_print("dC_drho_j1",dC_drho_j1);
egsl_print("dC_drho_j2",dC_drho_j2);
egsl_print("d2J_dtheta2",d2J_dtheta2);
egsl_print("Contribution to d2/dtheta",
sc(2.0, sum( m3(tr(v2),C_k,v1), m3(tr(v1),C_k,v1))));
}
egsl_pop();
}
egsl_print("d2J_dt2",d2J_dt2);
/* egsl_print("d2J_dt2",d2J_dt2);
egsl_print("d2J_dt_dtheta",d2J_dt_dtheta);
egsl_print("d2J_dtheta2",d2J_dtheta2);
egsl_print("tr(d2J_dt_dtheta)",tr(d2J_dt_dtheta));
egsl_print("row1", comp_row(d2J_dt2, d2J_dt_dtheta));
egsl_print("row2", comp_row(tr(d2J_dt_dtheta),d2J_dtheta2));
val d2J_dx2 = comp_col( comp_row(d2J_dt2, d2J_dt_dtheta),
comp_row(tr(d2J_dt_dtheta),d2J_dtheta2));
*/
// composes matrix d2J_dx2 from the pieces
val d2J_dx2 = comp_col( comp_row( d2J_dt2 , d2J_dt_dtheta),
comp_row(tr(d2J_dt_dtheta), d2J_dtheta2));
egsl_print("d2J_dx2",d2J_dx2);
egsl_print("inv(d2J_dx2)",inv(d2J_dx2));
......@@ -109,11 +144,13 @@ void compute_covariance_exact(
// egsl_print("d2J_dxdy1",d2J_dxdy1);
// egsl_print("d2J_dxdy2",d2J_dxdy2);
val edx_dy1 = m(inv(d2J_dx2), d2J_dxdy1);
val edx_dy2 = m(inv(d2J_dx2), d2J_dxdy2);
val edx_dy1 = sc(-1.0, m(inv(d2J_dx2), d2J_dxdy1));
val edx_dy2 = sc(-1.0, m(inv(d2J_dx2), d2J_dxdy2));
// egsl_print("dx_dy1",dx_dy1);
// egsl_print("dx_dy2",dx_dy2);
// egsl_print("dx_dy1",tr(edx_dy1));
// egsl_print("dx_dy2",tr(edx_dy2));
val cov0_x = sum(m(edx_dy1,tr(edx_dy1)),m(edx_dy2,tr(edx_dy2)) );
egsl_print("cov0_x",cov0_x);
val cov_x = sc(0.01*0.01,sum(m(edx_dy1,tr(edx_dy1)),m(edx_dy2,tr(edx_dy2)) ));
......
......@@ -173,8 +173,8 @@ void icp_loop(struct sm_params*params, const gsl_vector*start, gsl_vector*x_new,
break;
}
double error;
kill_outliers_trim(params, x_old, &error);
double error=0;
// kill_outliers_trim(params, x_old, &error);
int num_corr_after = ld_num_valid_correspondences(laser_sens);
*total_error = error;
......
......@@ -11,7 +11,7 @@ def Sm.params2method(params, ob)
ob.__send__(method, value)
# puts "Setting #{method} #{value}"
else
# puts "Structure does not have method #{method}"
puts "Structure does not have method #{method}"
end
}
end
......
......@@ -29,6 +29,9 @@ struct sm_params {
double outliers_maxPerc;
int doVisibilityTest;
int useCorrTricks;
int doComputeCovariance;
};
struct sm_result {
......
......@@ -5,11 +5,12 @@ require 'sm_icp'
require 'rsm_sm_loop'
scan_matcher = Sm::ICPC
#scan_matcher = ICP
#scan_matcher = GPM_then_ICP
name = scan_matcher.new.name
params = standard_parameters
=begin
params[:doAlphaTest] = 0
params[:doVisibilityTest] = 1
params[:restart] = 1
......@@ -17,10 +18,13 @@ params[:restart_threshold_mean_error] = 3.0 / 300.0
params[:restart_dt]= 0.03
params[:restart_dtheta]= 1.5 * 3.14 /180
params[:outliers_maxPerc] = 0.85;
=end
params[:doComputeCovariance] = 1
scan_list = (447..450).to_a
scan_list = []
scan_list = [3]
scan_list = [0]
input = File.open '../mbicp_tro_experiment/laserazosSM3.log'
output = File.open "out/laserazosSM3.#{name}.#{params.hash}.log", "w"
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment