Add point_t

Author: chutsu

src/xyz_se.c

@@ -4284,7 +4284,9 @@ void marg_factor_free(marg_factor_t *marg) {
 
   // Jacobians
   free(marg->param_types);
-  free(marg->param_ptrs);
+  if (marg->param_ptrs) {
+    free(marg->param_ptrs);
+  }
   free(marg->params);
   free(marg->r);
   for (int i = 0; i < marg->num_params; i++) {
@@ -4647,14 +4649,12 @@ static void marg_factor_hessian_decomp(marg_factor_t *marg) {
     for (int i = 0; i < (r * r); i++) {
       diff += pow(H_[i] - marg->H_marg[i], 2);
     }
-    diff = sqrt(diff);
 
     if (diff > 1e-2) {
       marg->eigen_decomp_ok = 0;
       LOG_WARN("J' * J != H_marg. Diff is %.2e\n", diff);
       LOG_WARN("This is bad ... Usually means marginalization is bad!\n");
     }
-    printf("J' * J != H_marg. Diff is %.2e\n", diff);
 
     free(Jt);
     free(H_);

src/xyz_se.h

@@ -318,6 +318,16 @@ void time_delay_setup(time_delay_t *time_delay, const real_t param);
 void time_delay_copy(const time_delay_t *src, time_delay_t *dst);
 void time_delay_print(const char *prefix, const time_delay_t *exts);
 
+///////////
+// POINT //
+///////////
+
+typedef struct point_t {
+  real_t x;
+  real_t y;
+  real_t z;
+} point_t;
+
 ///////////
 // JOINT //
 ///////////
@@ -495,12 +505,13 @@ typedef struct pose_factor_t {
   real_t covar[6 * 6];
   real_t sqrt_info[6 * 6];
 
+  real_t r[6];
   int r_size;
-  int num_params;
-  int param_types[1];
 
+  int param_types[1];
   real_t *params[1];
-  real_t r[6];
+  int num_params;
+
   real_t *jacs[1];
   real_t J_pose[6 * 6];
 } pose_factor_t;
@@ -523,12 +534,13 @@ typedef struct ba_factor_t {
   real_t sqrt_info[2 * 2];
   real_t z[2];
 
+  real_t r[2];
   int r_size;
-  int num_params;
-  int param_types[3];
 
+  int param_types[3];
   real_t *params[3];
-  real_t r[2];
+  int num_params;
+
   real_t *jacs[3];
   real_t J_pose[2 * 6];
   real_t J_feature[2 * 3];
@@ -557,12 +569,13 @@ typedef struct camera_factor_t {
   real_t sqrt_info[2 * 2];
   real_t z[2];
 
+  real_t r[2];
   int r_size;
+
   int num_params;
   int param_types[4];
-
   real_t *params[4];
-  real_t r[2];
+
   real_t *jacs[4];
   real_t J_pose[2 * 6];
   real_t J_extrinsic[2 * 6];