edit capture object & add new models

demos/cosyslam.cpp

@@ -112,25 +112,6 @@ Isometry3d str_to_isometry(std::string pose)
     return trans;
 }
 
-Matrix6d str_to_cov(std::string cov)
-{
-    // Initialize the covariance matrix
-    Matrix6d cov_matrix = Matrix6d::Identity();
-
-    // Parse the string
-    std::string::size_type sz;
-    for (int i = 0; i < 6; i++)
-    {
-        for (int j = 0; j < 6; j++)
-        {
-            cov_matrix(i, j) = std::stod(cov, &sz);
-            cov = cov.substr(sz);
-        }
-    }
-
-    return cov_matrix;
-}
-
 int main()
 {
     /////////////////////////
@@ -163,7 +144,6 @@ int main()
 
     // Parsing variables
     int len = csv_values.at(0).second.size();
-    int idx_cov = 10;
     int idx_pose = 4;
     int idx_obj = 3;
     int idx_t = 9;
@@ -184,7 +164,10 @@ int main()
         // Measurement data
         Isometry3d c_M_o = str_to_isometry(csv_values.at(idx_pose).second.at(i));
         Matrix6d cov;
-        cov = str_to_cov(csv_values.at(idx_cov).second.at(i));
+        Vector6d sig;
+        // default covariance value : the real value is computed in the processor
+        sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05;
+        cov = sig.array().matrix().asDiagonal();
         std::string object_name;
         object_name = csv_values.at(idx_obj).second.at(i);
         double t;

src/capture/capture_object.cpp

@@ -35,11 +35,20 @@ static ParamsCapturePtr createParamsCapture(const std::string & _filename_dot_ya
     return nullptr;
 }
 
+Isometry3d posevec_to_isometry(Vector7d pose)
+{
+    return Translation<double, 3>(pose.head<3>()) * Quaterniond(pose.tail<4>());
+}
+
 Matrix6d polynomial_covariance(ParamsCapturePtr params, ObjectDetection det, int degree)
 {
-    double r = det.measurement.head<3>().norm();
-    double theta = toRad(atan2(det.measurement(0), det.measurement(1)));
-    double phi = toRad(asin(det.measurement(2)/r));
+    // We need to get the inverse of the measurement for the error model
+    Isometry3d o_M_c = posevec_to_isometry(det.measurement);
+    Isometry3d c_M_o = o_M_c.inverse();
+
+    double r = c_M_o.translation().norm();
+    double theta = toRad(atan2(c_M_o.translation()(0), c_M_o.translation()(1)));
+    double phi = toRad(asin(c_M_o.translation()(2)/r));
     double s = det.detection_score;
 
     // a Lambda to compute error from coeficients

yaml_error/obj_000025.yaml

 type: "Capture"
 
-object_name: "obj_000025"
-coefs_err0: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err1: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err2: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err3: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err4: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err5: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]
+object_name: "obj_000026"
+coefs_err0: [     0.        ,     84.91150233,     -0.63217472,    -33.62072986,
+     854.66187072,      1.06670745,     -0.02315998,      0.1178933 ,
+     -85.71334945,     -0.00109375,      0.00011923,      0.63880521,
+       0.04854639,     33.49530388,   -425.71182598, -428.75956688 ]
+coefs_err1: [     0.        ,    -47.11994603,     -1.32519479,      2.64488213,
+     449.28645654,      1.46122902,      0.04149583,     -0.21361062,
\ No newline at end of file
+      46.3311533 ,     -0.00190407,      0.00343207,      1.30973293,
+       0.03529835,     -2.65673462,   -231.3012547 , -217.83466642]
+coefs_err2: [     0.        ,     25.40095331,     -0.25129384,      9.34468643,
+    -208.93908017,      1.03854897,     -0.00676161,     -0.03937155,
+     -26.06411687,     -0.00457338,     -0.00036075,      0.25441639,
+       0.00129732,     -9.33973827,    110.69313669, 98.37016376]
+coefs_err3: [     0.        ,   7348.77780448,    -16.9505731 ,  -3112.99358946,
+   85074.66473739,     99.85637165,     -2.28526963,      8.98146452,
+   -7422.1449833 ,     -0.22412428,     -0.35870205,     17.93018179,
+       2.46160136,   3105.66626777, -42480.83569721, -42578.38467992]
+coefs_err4: [     0.        ,  -3182.51625556,    -34.89023849,    167.4091451 ,
+   28231.16177785,    162.95212408,      2.62084467,     -2.63062657,
+    3078.74616147,     -0.26930349,     -0.01140492,     34.09835059,
+      -0.53067687,   -166.26809157, -14630.36803029, -13583.66785708]
+coefs_err5: [     0.        ,  -5955.95025264,    -47.87184666,   3143.17788279,
+  -56570.35371729,    157.33244408,      1.34532515,      4.22946971,
+    5848.95994436,     -0.15287568,     -0.07783475,     47.68975938,
+       2.25385947,  -3149.70693765,  28399.70643467, 28191.87926423]

yaml_error/obj_000026.yaml

 type: "Capture"
 
 object_name: "obj_000026"
-coefs_err0: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err1: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err2: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err3: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err4: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]
-coefs_err5: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]
+coefs_err0: [     0.        ,     84.91150233,     -0.63217472,    -33.62072986,
+     854.66187072,      1.06670745,     -0.02315998,      0.1178933 ,
+     -85.71334945,     -0.00109375,      0.00011923,      0.63880521,
+       0.04854639,     33.49530388,   -425.71182598, -428.75956688 ]
+coefs_err1: [     0.        ,    -47.11994603,     -1.32519479,      2.64488213,
+     449.28645654,      1.46122902,      0.04149583,     -0.21361062,
\ No newline at end of file
+      46.3311533 ,     -0.00190407,      0.00343207,      1.30973293,
+       0.03529835,     -2.65673462,   -231.3012547 , -217.83466642]
+coefs_err2: [     0.        ,     25.40095331,     -0.25129384,      9.34468643,
+    -208.93908017,      1.03854897,     -0.00676161,     -0.03937155,
+     -26.06411687,     -0.00457338,     -0.00036075,      0.25441639,
+       0.00129732,     -9.33973827,    110.69313669, 98.37016376]
+coefs_err3: [     0.        ,   7348.77780448,    -16.9505731 ,  -3112.99358946,
+   85074.66473739,     99.85637165,     -2.28526963,      8.98146452,
+   -7422.1449833 ,     -0.22412428,     -0.35870205,     17.93018179,
+       2.46160136,   3105.66626777, -42480.83569721, -42578.38467992]
+coefs_err4: [     0.        ,  -3182.51625556,    -34.89023849,    167.4091451 ,
+   28231.16177785,    162.95212408,      2.62084467,     -2.63062657,
+    3078.74616147,     -0.26930349,     -0.01140492,     34.09835059,
+      -0.53067687,   -166.26809157, -14630.36803029, -13583.66785708]
+coefs_err5: [     0.        ,  -5955.95025264,    -47.87184666,   3143.17788279,
+  -56570.35371729,    157.33244408,      1.34532515,      4.22946971,
+    5848.95994436,     -0.15287568,     -0.07783475,     47.68975938,
+       2.25385947,  -3149.70693765,  28399.70643467, 28191.87926423]