No commit message

No commit message

sm/CMakeLists.txt

@@ -17,6 +17,7 @@ PROJECT (icp C)
 
 ADD_LIBRARY(sm STATIC icp.c icp_correspondences_dumb.c 
 icp_correspondences_tricks.c icp_loop.c journal.c laser_data.c math_utils.c
+icp_covariance.c easy_gsl.c
 icp_outliers.c gsl_stack.c orientation.c clustering.c gpm.c 
 lib/gpc/gpc.c
 lib/gpc/gpc_utils.c
@@ -25,7 +26,7 @@ lib/gpc/gpc_utils.c
 TARGET_LINK_LIBRARIES(sm ${GSL_LIBRARIES})
 
 
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall -W -Wconversion  -Wunreachable-code ")
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused -Wall -Wno-long-double -W -Wconversion  -Wunreachable-code ")
 SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb")
 SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Ilib/gpc")
 
@@ -34,10 +35,13 @@ INSTALL(TARGETS sm ARCHIVE DESTINATION lib)
 INSTALL(FILES sm.h  DESTINATION include)
 
 
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mtune=powerpc -O3")
+#SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mtune=powerpc -O3")
 
 ADD_EXECUTABLE(sm_test sm_test.c)
 TARGET_LINK_LIBRARIES(sm_test sm ${GSL_LIBRARIES})
 
 ADD_EXECUTABLE(test_math_utils test_math_utils.c)
 TARGET_LINK_LIBRARIES(test_math_utils sm ${GSL_LIBRARIES})
+
+ADD_EXECUTABLE(easy_gsl_test easy_gsl_test.c easy_gsl.c)
+TARGET_LINK_LIBRARIES(easy_gsl_test  ${GSL_LIBRARIES})

sm/easy_gsl.c

+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_linalg.h>
+#include <assert.h>
+#include <math.h>
+
+#include "easy_gsl.h"
+#define MAX_VALS 1024
+#define MAX_CONTEXTS 1024
+
+#define INVALID (val_from_context_and_index(1000,1000))
+
+struct egsl_variable {
+	gsl_matrix * gsl_m;
+};
+
+struct egsl_context {
+	int nvars;
+	struct egsl_variable vars[MAX_VALS]; 
+};
+
+int cid=0;
+struct egsl_context egsl_contexts[MAX_CONTEXTS];
+
+void error() {
+	
+}
+
+void egsl_init_context(int i) {
+	
+}
+
+void egsl_free_context(int i) {
+	struct egsl_context * c = egsl_contexts+i;
+	int v;
+	for(v=0;v<c->nvars;v++) {
+		gsl_matrix_free(c->vars[v].gsl_m);
+	}
+	c->nvars = 0;
+}
+
+void egsl_push() {
+	cid++;
+	assert(cid<MAX_CONTEXTS);// "Maximum number of contexts reached");
+	egsl_init_context(cid);
+}
+
+void egsl_pop() {
+	assert(cid>=0);//, "No egsl_push before?");
+	egsl_free_context(cid);
+	cid--;
+}
+
+int its_context(val v) {
+	return (v>>16) & 0xffff;
+}
+
+int its_var_index(val v) {
+	return v & 0xffff;
+}
+
+val val_from_context_and_index(int cid, int index) {
+	return (cid<<16) | index;
+}
+
+void check_valid_val(val v) {
+	int context = its_context(v);
+	if(context>cid) {
+		printf("Val is from invalid context (%d>%d)\n",context,cid);
+	}
+	int var_index = its_var_index(v);
+	if(var_index >= egsl_contexts[context].nvars) {
+		printf("Val is invalid  (%d>%d)\n",var_index, egsl_contexts[context].nvars);		
+	}
+	error();
+}
+
+struct egsl_variable * get_var(val v) {
+	check_valid_val(v);
+	int context = its_context(v);
+	int var_index = its_var_index(v);
+	return &(egsl_contexts[context].vars[var_index]);
+}
+
+gsl_matrix * egsl_gslm(val v) {
+	return get_var(v)->gsl_m;
+}
+
+void egsl_print(const char*str, val v) {
+	gsl_matrix * m = egsl_gslm(v);
+	size_t i,j;
+	printf("%s = \n",str);
+	for(i=0;i<m->size1;i++) {
+		printf("   ");
+		for(j=0;j<m->size2;j++) {
+			printf("%f ", gsl_matrix_get(m,i,j));
+		}
+		printf("\n");
+	}	
+}
+
+val egsl_alloc(int rows, int columns) {
+	struct egsl_context*c = egsl_contexts+cid;
+	if(c->nvars>=MAX_VALS) {
+		printf("Limit reached\n");
+		error();
+	}
+	int index = c->nvars;
+	c->vars[index].gsl_m = gsl_matrix_alloc((size_t)rows,(size_t)columns);
+	val v = val_from_context_and_index(cid,index);
+	c->nvars++;
+	return v;
+}
+
+double* egsl_atmp(val v, int i, int j) {
+	gsl_matrix * m = egsl_gslm(v);
+	return gsl_matrix_ptr(m,(size_t)i,(size_t)j);
+}
+
+val egsl_vFda(int rows, int cols, const double *a) {
+	val v = egsl_alloc(rows, cols);
+
+	int i; int j;
+	for(i=0;i<rows;i++)
+	for(j=0;j<cols;j++) {
+		*egsl_atmp(v,i,j) = a[i+j*cols];
+	}
+	return v;
+}
+
+val egsl_vFa(int rows, const double*a) {
+	val v = egsl_alloc(rows,1);
+	int i;
+	for(i=0;i<rows;i++)
+		*egsl_atmp(v,i,0) =  a[i];
+	return v;
+}
+
+val egsl_rot(double theta) {
+	double R[2*2] = {
+		cos(theta), -sin(theta),
+		sin(theta),  cos(theta)
+	};
+	return egsl_vFda(2,2,R);
+}
+
+val egsl_zeros(int rows, int columns) {
+	return INVALID;
+}
+
+val egsl_scale(double s, val v){
+	return INVALID;
+}
+
+val egsl_sum(val v1, val v2){
+	return INVALID;
+}
+
+val egsl_sum3(val v1, val v2, val v3){
+	return INVALID;
+}
+
+val egsl_mult(val v1, val v2){
+	gsl_matrix * a = egsl_gslm(v1);
+	gsl_matrix * b = egsl_gslm(v2);
+	val v = egsl_alloc(a->size1,b->size2);
+	gsl_matrix * ab = egsl_gslm(v); 
+	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,a,b,0.0,ab);
+	return v;
+}
+
+val egsl_transpose(val v1){
+	return INVALID;
+}
+
+val egsl_inverse(val v1){
+	return INVALID;
+}
+
+double egsl_norm(val v1){
+	return INVALID;
+}
+
+double egsl_atv(val v1,  int i){
+	return INVALID;
+}
+
+double egsl_atm(val v1, int i, int j){
+	return INVALID;
+}
+
+
+val egsl_sub(val v1,val v2){
+	return INVALID;
+}
+
+
+val egsl_compose_col(val v1, val v2){
+	return INVALID;
+}
+
+val egsl_vers(double theta){
+	return INVALID;
+}
+
+
+void egsl_v2da(val v1, double*a){
+
+}
+
+val egsl_vFgslv(const gsl_vector*v){
+	return INVALID;
+}
+
+val egsl_vFgslm(const gsl_matrix*m){
+	return INVALID;
+}
+
+gsl_matrix* egsl_v2gslm(val v1){
+	return 0;
+}
+
+void egsl_add_to(val v1, val v2) {
+	
+}
+
+void egsl_add_to_col(val v1, int j, val v2) {
+	
+}
+

sm/easy_gsl.h

+#ifndef H_EASY_GSL
+#define H_EASY_GSL
+
+#include <gsl/gsl_vector.h>
+#include <gsl/gsl_matrix.h>
+
+typedef int val;
+
+void egsl_push();
+void egsl_pop();
+void egsl_print(const char*str, val);
+
+
+val egsl_zeros(int rows, int columns);
+val egsl_scale(double, val);
+val egsl_sum(val, val);
+val egsl_sum3(val, val, val);
+val egsl_mult(val, val);
+val egsl_transpose(val);
+val egsl_inverse(val);
+double egsl_norm(val);
+
+double egsl_atv(val, int i);
+double egsl_atm(val, int i, int j);
+
+val egsl_sub(val,val);
+val egsl_sum(val v1,val v2);
+val egsl_compose_col(val v1, val v2);
+
+void egsl_add_to(val v1, val v2);
+void egsl_add_to_col(val v1, int j, val v2);
+
+val egsl_vers(double theta);
+val egsl_rot(double theta);
+
+val egsl_vFa(int rows, const double*);
+val egsl_vFda(int rows, int columns, const double*);
+void egsl_v2a(val, double**);
+void egsl_v2da(val, double*);
+
+val egsl_vFgslv(const gsl_vector*);
+val egsl_vFgslm(const gsl_matrix*);
+gsl_matrix* egsl_v2gslm(val);
+
+#endif
\ No newline at end of file

sm/easy_gsl_macros.h

+
+
+#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 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)
+#define m(v1,v2)         egsl_mult(v1,v2)
+#define m3(v1,v2,v3)     egsl_mult(v1,egsl_mult(v2,v3))
+#define comp_col(v1,v2)  egsl_compose_col(v1,v2)
+
+#define zeros(rows,cols) egsl_zeros(rows,cols)
+#define vers(th)         egsl_vers(th)
+#define rot(theta)       egsl_rot(theta)
+
+#define sc(d,v)         egsl_scale(d, v)
+#define add_to(v1,v2)   egsl_add_to(v1, v2)
+#define add_to_col(v1,j,v2)   egsl_add_to_col(v1, j, v2)

sm/easy_gsl_test.c

+#include <math.h>
+#include "easy_gsl.h"
+#include "easy_gsl_macros.h"
+
+int main() {
+	
+	egsl_push();
+	
+	val R = rot(M_PI/2);
+
+	egsl_print("R", R);
+	
+	
+	double p[2] = {1,2};
+	
+	val vp = egsl_vFa(2,p);
+	
+	egsl_print("vp", vp);
+	
+	val vrot = m(R, vp); 
+	
+	egsl_print("vrot", vrot);
+	
+	egsl_pop();
+}
\ No newline at end of file

sm/icp_covariance.c

+#include <math.h>
+#include "laser_data.h"
 #include <gpc_utils.h>
-
-void compute_C_k(const gsl_vector*p_j1, const gsl_vector*p_j2, gsl_matrix*C_k)  {
-	double alpha = M_PI/2 + atan2( gvg(p_j1,1)-gvg(p_j2,1), gvg(p_j1,0)-gvg(p_j2,0));
-	
-	gms(C_k,0,0, cos(alpha)*cos(alpha));
-	gms(C_k,1,0, sin(alpha)*cos(alpha));
-	gms(C_k,0,1, cos(alpha)*sin(alpha));
-	gms(C_k,1,1, sin(alpha)*sin(alpha));
+#include "easy_gsl.h"
+#include "easy_gsl_macros.h"
+
+val compute_C_k(val p_j1, val p_j2)  {	
+	val d = sub(p_j1, p_j2);
+	double alpha = M_PI/2 + atan2( atv(d,1), atv(d,0));
+	double c = cos(alpha); double s = sin(alpha);
+	double m[2][2] = {
+		{c*c, c*s},
+		{c*s, s*s}
+	};
+	return egsl_vFda(2,2,m);
 }
 
-// Computes a' * B * c where B is symmetric
-double quad(const gsl_matrix*a, const gsl_matrix*B, const gsl_matrix*c) {
-	return gmg(a,0,0)*gmg(c,0,0)*gmg(B,0,0) + gmg(a,1,0)*gmg(c,1,0)*gmg(B,1,1)  +
-	 	 2*gmg(a,1,0)*gmg(c,0,1)*gmg(B,1,0);
-}
 
-void set_polar(gsl_matrix*v,double theta,double rho) {
-	gms(v, 0,0, cos(theta)*rho);
-	gms(v, 1,0, sin(theta)*rho);
+
+val dC_drho(val p1, val p2) {
+	double eps = 0.001;
+
+	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));
 }
 
-void compute_covariance_exact(LDP laser_ref, LDP laser_sens, int*valid, gsl_vector*x){
-	MAT(d2J_dxdy1,3,laser_ref->nrays);
-	MAT(d2J_dxdy2,3,laser_sens->nrays);
+
+void compute_covariance_exact(LDP laser_ref, LDP laser_sens, gsl_vector*x) {
+	egsl_push();
 	
+	val d2J_dxdy1 = zeros(3,laser_ref->nrays);
+	val d2J_dxdy2 = zeros(3,laser_sens->nrays);
+
 	// the three pieces of d2J_dx2
-	MAT(d2J_dt2,2,2);
-	MAT(d2J_dt_dtheta,2,1);
-	MAT(d2J_dtheta2,1,1);
+	val d2J_dt2       = zeros(2,2);
+	val d2J_dt_dtheta = zeros(2,1);
+	val d2J_dtheta2   = zeros(1,1);
 	
-	MAT(Ck,2,2);	
-	MAT(v1,2,1); MAT(v1,1,2);
-	MAT(v2,2,1); MAT(v2,1,2);
-	MAT(v_i,2,1);
-	MAT(v3,2,1); MAT(v3t,1,2);
-	MAT(v4,2,1); MAT(v4t,1,2);
+	double theta = x->data[2];
+	val t = egsl_vFa(2,x->data);
 	
 	int i; 
 	for(i=0;i<laser_sens->nrays;i++) {
 		if(!ld_valid_corr(laser_sens,i)) continue;
+		egsl_push();
 
 		int j1 = laser_sens->corr[i].j1;
 		int j2 = laser_sens->corr[i].j2;
 
-		gsl_vector*p_i  = laser_sens->p[i];
-		gsl_vector*p_j1  = laser_ref->p[j1];
-		
-		gsl_vector *p_j2  = laser_ref->p[j2];
+		val p_i  = egsl_vFgslv(laser_sens->p[i]);
+		val p_j1 = egsl_vFgslv(laser_ref ->p[j1]);
+		val p_j2 = egsl_vFgslv(laser_ref ->p[j2]);
 		
 		// v1 := rot(theta+PI/2)*p_i
-		set_polar(v1, theta+PI/2 + laser_sens->theta[i], laser_sens->readings[i] );
-		// v3 := rot(theta)*v_i)
+		val v1 = m(rot(theta+M_PI/2), p_i);		
+		// v2 := (rot(theta)*p_i+t-p_j1)
+		val v2 = sum3( m(rot(theta),p_i), t, minus(p_j1));
+		// v3 := rot(theta)*v_i
+		val v3 = vers(theta + laser_sens->theta[i]);
 		// v4 := rot(theta+PI/2)*v_i;
-		set_polar(v3, theta + laser_sens->theta[i], 1 );
-		set_polar(v4, theta + PI/2 + laser_sens->theta[i], 1 );
-
-		m_trans(v1,v1t); m_trans(v2,v2t); m_trans(v3,v3t); m_trans(v4,v4t);
-		
-		v2 := (rot(theta)*p_i+t-p_j1)
-
-		compute_C_k(q_k,other,C_k);
+		val v4 = vers(theta + laser_sens->theta[i] + M_PI/2);
 		
-		d2J_dt2       += 2 * C_k
-		d2J_dt_dtheta += 2 * v1t*C_k
-		d2J_dtheta2   += 2 * quad(v2,C_k,v1) + 2 * quad(v1,C_k,v1);
+		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(tr(v1),C_k)));
+		add_to(d2J_dtheta2, sc(2.0, sum( m3(tr(v2),C_k,v1), m3(tr(v1),C_k,v1))));
 		
 		// for measurement rho_i  in the second scan
+		val d2Jk_dtdrho_i = sc(2.0, m(tr(v3), C_k));
+		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, i, comp_col(d2Jk_dtdrho_i, d2Jk_dtheta_drho_i));
 		
-		d2Jk_dtdrho_i = 2 * v3t * C_k
-		d2Jk_dtheta_drho_i = 2*quad(v2,C_k,v4)+ 2 *quad(v3,C_k,v1)
-		
-		d2J_dxdy2.col(c.i)[0] += d2Jk_dtdrho_i[0]
-		d2J_dxdy2.col(c.i)[1] += d2Jk_dtdrho_i[1]
-		d2J_dxdy2.col(c.i)[2] += d2Jk_dtheta_drho_i
-	
 		// for measurements rho_j1, rho_j2 in the first scan
-		MAT(dC_drho_j1,2,2);
-		MAT(dC_drho_j2,2,2);
-		 
-		dC_drho_j12(laser_ref, laser_sens, c)
-		
-		v_j1  := = laser_ref.points[c.j1].v
-		v_j2  := = laser_ref.points[c.j2].v
-		v_j1t := 
-		v_j2t :=
 		
-		d2Jk_dtheta_drho_j1 = -2*quad(v_j1,C_k,v1) + quad(v2,dC_drho_j1,v1);
+		val dC_drho_j1 = dC_drho(p_j1, p_j2);
+		val dC_drho_j2 = dC_drho(p_j2, p_j1);
 		
-		d2Jk_dt_drho_j1 = 2 * (-v_j1t*m+v2t*dC_drho_j1)
+		val v_j1 = vers(laser_ref->theta[j1]);
+		val v_j2 = vers(laser_ref->theta[j2]);
 		
-		d2J_dxdy1.col(c.j1)[0] += d2Jk_dt_drho_j1[0]
-		d2J_dxdy1.col(c.j1)[1] += d2Jk_dt_drho_j1[1]
-		d2J_dxdy1.col(c.j1)[2] += d2Jk_dtheta_drho_j1
-		
-		# for measurement rho_j2
-		d2Jk_dtheta_drho_j2 = 2 * quad(v2, dC_drho_j2, v1);
-			
-		d2Jk_dt_drho_j2 = 2*v2t * dC_drho_j2 
-		
-		d2J_dxdy1.col(c.j2)[0] += d2Jk_dt_drho_j2[0]
-		d2J_dxdy1.col(c.j2)[1] += d2Jk_dt_drho_j2[1]
-		d2J_dxdy1.col(c.j2)[2] += d2Jk_dtheta_drho_j2
+		val d2Jk_dt_drho_j1 = sum(sc(-2.0,m(tr(v_j1),C_k)), sc(2.0,m(tr(v2),dC_drho_j1)));
+		val d2Jk_dtheta_drho_j1 = sum( sc(-2.0, m3(tr(v_j1),C_k,v1)), m3(tr(v2),dC_drho_j1,v1));
+		add_to_col(d2J_dxdy1, j1, comp_col(d2Jk_dt_drho_j1, d2Jk_dtheta_drho_j1));
 		
+		// for measurement rho_j2
+		val d2Jk_dt_drho_j2 = 2 * m( tr(v2), dC_drho_j2);
+		val d2Jk_dtheta_drho_j2 = 2 * m3( tr(v2), dC_drho_j2, v1);
+		add_to_col(d2J_dxdy1, j2, comp_col(d2Jk_dt_drho_j2, d2Jk_dtheta_drho_j2));
+
+		egsl_pop();
 	}
-	
+	egsl_pop();	
 }
 
-def compute_covariance_exact(laser_ref, laser_sens, correspondences, x)
-	y1 = Vector.alloc(laser_ref.points.map{|p| p.reading}).col
-	y2 = Vector.alloc(laser_sens.points.map{|p| p.reading}).col
-
-	d2J_dxdy2 = Matrix.alloc(3, laser_sens.nrays)
-	d2J_dxdy1 = Matrix.alloc(3, laser_ref.nrays)
-	
-	t = Vector.alloc(x[0],x[1]).col
-	theta = x[2].to_f;
-	
-	# the three pieces of d2J_dx2
-	d2J_dt2       = Matrix.alloc(2,2)
-	d2J_dt_dtheta = Vector.alloc(0,0).col
-	d2J_dtheta2   = 0
-	
-	for c in correspondences.compact
-		p_k  = laser_sens.points[c.i ].cartesian
-		q_k  = laser_ref .points[c.j1].cartesian
-
-		other  = laser_ref .points[c.j2].cartesian
-		v_alpha = rot(PI/2) * (q_k-other)
-		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 * 
-		   m * rot(theta+PI/2) * p_k + 2 * (rot(theta+PI/2)*p_k).trans * m *
-			rot(theta+PI/2) * p_k
-			
-		###########
-		
-			
-		# for measurement rho_i  in the second scan
-		v_i = laser_sens.points[c.i].v
-		d2Jk_dtdrho_i = 2 * (rot(theta)*v_i).trans * m
-		d2Jk_dtheta_drho_i = 2*(rot(theta)*p_k+t-q_k).trans*m*rot(theta+PI/2)*v_i +
-			2 *(rot(theta)*v_i).trans*m*rot(theta+PI/2)*p_k
-		
-		d2J_dxdy2.col(c.i)[0] += d2Jk_dtdrho_i[0]
-		d2J_dxdy2.col(c.i)[1] += d2Jk_dtdrho_i[1]
-		d2J_dxdy2.col(c.i)[2] += d2Jk_dtheta_drho_i
-	
-		# for measurements rho_j1, rho_j2 in the first scan
-		dC_drho_j1, dC_drho_j2 = dC_drho_j12(laser_ref, laser_sens, c)
-		
-		v_j1 = laser_ref.points[c.j1].v
-		v_j2 = laser_ref.points[c.j2].v
-		
-		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(c.j1)[0] += d2Jk_dt_drho_j1[0]
-		d2J_dxdy1.col(c.j1)[1] += d2Jk_dt_drho_j1[1]
-		d2J_dxdy1.col(c.j1)[2] += d2Jk_dtheta_drho_j1
-		
-		# for measurement rho_j2
-		d2Jk_dtheta_drho_j2 = 2*(rot(theta)*p_k+t-q_k).trans * dC_drho_j2 *
-			rot(theta+PI/2)*p_k;
-			
-		d2Jk_dt_drho_j2 = 2*(rot(theta)*p_k+t-q_k).trans * dC_drho_j2 
-		
-		d2J_dxdy1.col(c.j2)[0] += d2Jk_dt_drho_j2[0]
-		d2J_dxdy1.col(c.j2)[1] += d2Jk_dt_drho_j2[1]
-		d2J_dxdy1.col(c.j2)[2] += d2Jk_dtheta_drho_j2
-		
-	end
-	# put the pieces together
-	d2J_dx2 = Matrix.alloc(3,3)
-	d2J_dx2[0,0]=d2J_dt2[0,0]
-	d2J_dx2[1,0]=d2J_dt2[1,0]
-	d2J_dx2[1,1]=d2J_dt2[1,1]
-	d2J_dx2[0,1]=d2J_dt2[0,1]
-	d2J_dx2[2,0]=d2J_dx2[0,2]=d2J_dt_dtheta[0]
-	d2J_dx2[2,1]=d2J_dx2[1,2]=d2J_dt_dtheta[1]
-	d2J_dx2[2,2] = d2J_dtheta2
-
-	dx_dy1 =  -d2J_dx2.inv * d2J_dxdy1
-	dx_dy2 =  -d2J_dx2.inv * d2J_dxdy2
-	
-	return dx_dy1, dx_dy2
-end
-
-def getC(rho_j1,v_j1,rho_j2,v_j2)
-	p_j1 = v_j1 * rho_j1
-	p_j2 = v_j2 * rho_j2
-	v_alpha = rot(PI/2) * (p_j1-p_j2)
-	v_alpha = v_alpha / v_alpha.nrm2
-	m = v_alpha*(v_alpha.trans)
-	m
-end
-
-def dC_drho_j12(laser_ref, laser_sens, c)
-	
-	rho_j1 = laser_ref.points[c.j1].reading
-	  v_j1 = laser_ref.points[c.j1].v
-	rho_j2 = laser_ref.points[c.j2].reading
-	  v_j2 = laser_ref.points[c.j2].v
-
-	eps = 0.001;
-	
-	dC_drho_j1 = 
-	  (getC(rho_j1+eps,v_j1,rho_j2,v_j2)-
-		getC(rho_j1    ,v_j1,rho_j2,v_j2))/eps;
-
-	dC_drho_j2 = 
-	  (getC(rho_j1,v_j1,rho_j2+eps,v_j2)-
-		getC(rho_j1,v_j1,rho_j2    ,v_j2))/eps;
-
-	return dC_drho_j1, dC_drho_j2
-	
-end