add covariance models

demos/cosyslam.cpp

@@ -167,6 +167,7 @@ int main()
     int idx_pose = 4;
     int idx_obj = 3;
     int idx_t = 9;
+    int idx_s = 6;
 
     ///////////////
     // SLAM loop //
@@ -188,7 +189,9 @@ int main()
         object_name = csv_values.at(idx_obj).second.at(i);
         double t;
         t = std::stod(csv_values.at(idx_t).second.at(i));
-        ObjectDetection det = {.measurement = isometry_to_posevec(c_M_o), .meas_cov = cov, .object_type = object_name};
+        double s;
+        s = std::stod(csv_values.at(idx_s).second.at(i));
+        ObjectDetection det = {.measurement = isometry_to_posevec(c_M_o), .detection_score = s, .meas_cov = cov, .object_type = object_name};
 
         if (t == t_cur){
             // Capture on the current frame

demos/demo_toy_pbe.cpp

@@ -95,8 +95,8 @@ int main() {
     // t0
     Isometry3d kf0_M_lmk0 = w_M_kf0.inverse() * w_M_lmk0;
     Isometry3d kf0_M_lmk1 = w_M_kf0.inverse() * w_M_lmk1;
-    ObjectDetection det0_0     = {.measurement = isometry_to_posevec(kf0_M_lmk0), .meas_cov=cov, .object_type = "type0"};
-    ObjectDetection det0_1 = {.measurement = isometry_to_posevec(kf0_M_lmk1), .meas_cov=cov, .object_type = "type1"};
+    ObjectDetection det0_0     = {.measurement = isometry_to_posevec(kf0_M_lmk0), .detection_score=1, .meas_cov=cov, .object_type = "type0"};
+    ObjectDetection det0_1 = {.measurement = isometry_to_posevec(kf0_M_lmk1), .detection_score=1, .meas_cov=cov, .object_type = "type1"};
     dets.push_back(det0_0);
     dets.push_back(det0_1);
 
@@ -110,8 +110,8 @@ int main() {
     dets.clear();
     Isometry3d kf1_M_lmk1 = w_M_kf1.inverse() * w_M_lmk1;
     Isometry3d kf1_M_lmk2 = w_M_kf1.inverse() * w_M_lmk2;
-    ObjectDetection det1_1     = {.measurement = isometry_to_posevec(kf1_M_lmk1), .meas_cov=cov, .object_type = "type1"};
-    ObjectDetection det1_2 = {.measurement = isometry_to_posevec(kf1_M_lmk2), .meas_cov=cov, .object_type = "type2"};
+    ObjectDetection det1_1     = {.measurement = isometry_to_posevec(kf1_M_lmk1), .detection_score=1, .meas_cov=cov, .object_type = "type1"};
+    ObjectDetection det1_2 = {.measurement = isometry_to_posevec(kf1_M_lmk2), .detection_score=1, .meas_cov=cov, .object_type = "type2"};
     dets.push_back(det1_1);
     dets.push_back(det1_2);
 
@@ -125,8 +125,8 @@ int main() {
     dets.clear();
     Isometry3d kf2_M_lmk1 = w_M_kf2.inverse() * w_M_lmk1;
     Isometry3d kf2_M_lmk2 = w_M_kf2.inverse() * w_M_lmk2;
-    ObjectDetection det2_1 = {.measurement = isometry_to_posevec(kf2_M_lmk1), .meas_cov=cov, .object_type = "type1"};
-    ObjectDetection det2_2 = {.measurement = isometry_to_posevec(kf2_M_lmk2), .meas_cov=cov, .object_type = "type2"};
+    ObjectDetection det2_1 = {.measurement = isometry_to_posevec(kf2_M_lmk1), .detection_score=1, .meas_cov=cov, .object_type = "type1"};
+    ObjectDetection det2_2 = {.measurement = isometry_to_posevec(kf2_M_lmk2), .detection_score=1, .meas_cov=cov, .object_type = "type2"};
     dets.push_back(det2_1);
     dets.push_back(det2_2);
 

include/objectslam/capture/capture_object.h

@@ -3,14 +3,51 @@
 
 //Wolf includes
 #include "core/capture/capture_base.h"
+#include "core/math/rotations.h"
+#include "core/common/params_base.h"
+#include "math.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
 
 
 namespace wolf {
 
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsCapture);
+
+struct ParamsCapture : public ParamsBase
+{
+    string object_name;
+    std::vector<double> coefs_err0;
+    std::vector<double> coefs_err1;
+    std::vector<double> coefs_err2;
+    std::vector<double> coefs_err3;
+    std::vector<double> coefs_err4;
+    std::vector<double> coefs_err5;
+
+    ParamsCapture() = default;
+    ParamsCapture(std::string _unique_name, const ParamsServer& _server):
+        ParamsBase(_unique_name, _server)
+    {
+        object_name      = _server.getParam<string>("/object_name");
+        coefs_err0       = _server.getParam<std::vector<double>>("/coefs_err0");
+        coefs_err1       = _server.getParam<std::vector<double>>("/coefs_err1");
+        coefs_err2       = _server.getParam<std::vector<double>>("/coefs_err2");
+        coefs_err3       = _server.getParam<std::vector<double>>("/coefs_err3");
+        coefs_err4       = _server.getParam<std::vector<double>>("/coefs_err4");
+        coefs_err5       = _server.getParam<std::vector<double>>("/coefs_err5");
+    }
+    std::string print() const override
+    {
+        return object_name;
+    }
+};
+
 typedef struct ObjectDetection
 {
     const Eigen::Vector7d measurement;
-    const Eigen::Matrix6d meas_cov; 
+    const double detection_score;
+    Eigen::Matrix6d meas_cov; 
     const std::string object_type;
 
 } ObjectDetection;

src/capture/capture_object.cpp

 #include "objectslam/capture/capture_object.h"
+#include "objectslam/internal/config.h"
 
 
 namespace wolf {
 
+static ParamsCapturePtr createParamsCapture(const std::string & _filename_dot_yaml)
+{
+    YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+    if (config.IsNull())
+    {
+        WOLF_ERROR("Invalid YAML file!");
+        return nullptr;
+    }
+    else if (config["type"].as<std::string>() == "Capture")
+    {
+        ParamsCapturePtr params = std::make_shared<ParamsCapture>();
+
+        params->object_name            = config["object_name"]                  .as<string>();
+        params->coefs_err0             = config["coefs_err0"]                   .as<std::vector<double>>();
+        params->coefs_err1             = config["coefs_err1"]                   .as<std::vector<double>>();
+        params->coefs_err2             = config["coefs_err2"]                   .as<std::vector<double>>();
+        params->coefs_err3             = config["coefs_err3"]                   .as<std::vector<double>>();
+        params->coefs_err4             = config["coefs_err4"]                   .as<std::vector<double>>();
+        params->coefs_err5             = config["coefs_err5"]                   .as<std::vector<double>>();
+
+        return params;
+    }
+    else
+    {
+        WOLF_ERROR("Wrong capture type! Should be \"Capture\"");
+        return nullptr;
+    }
+    return nullptr;
+}
+
+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));
+    double s = det.detection_score;
+
+    // a Lambda to compute error from coeficients
+    auto error = [] (std::vector<double> coefs, double r, double theta, double phi, double s)
+        {
+            // case of degree one polynomial
+            if (coefs.size() == 6){
+                return coefs.at(0) + coefs.at(1)*r + coefs.at(2)*theta + coefs.at(3)*phi + coefs.at(4)*s + coefs.at(5);
+            }
+            // case of degree two polynomial
+            return coefs.at(0) + coefs.at(1)*r + coefs.at(2)*theta + coefs.at(3)*phi + coefs.at(4)*s +
+            coefs.at(5)*r*r + coefs.at(6)*r*theta + coefs.at(7)*r*phi + coefs.at(8)*r*s +
+            coefs.at(9)*theta*theta + coefs.at(10)*theta*phi + coefs.at(11)*theta*s +
+            coefs.at(12)*phi*phi + coefs.at(13)*phi*s + 
+            coefs.at(14)*s*s + coefs.at(15);
+        };
+
+    double err_0 = error(params->coefs_err0, r, theta, phi, s);
+    double err_1 = error(params->coefs_err1, r, theta, phi, s);
+    double err_2 = error(params->coefs_err2, r, theta, phi, s);
+    double err_3 = error(params->coefs_err3, r, theta, phi, s);
+    double err_4 = error(params->coefs_err4, r, theta, phi, s);
+    double err_5 = error(params->coefs_err5, r, theta, phi, s);
+    Vector6d error_vec;
+    error_vec << err_0, err_1, err_2, err_3, err_4, err_5;
+
+    // Compute the matrix from the error vector 
+    Matrix6d cov = error_vec.array().matrix().asDiagonal();
+    cov = cov*cov;
+    return cov;
+}
+
 CaptureObject::CaptureObject(const TimeStamp& ts, SensorBasePtr _sensor_ptr, const ObjectDetections& _object_dets) :
     CaptureBase("CaptureObject", ts, _sensor_ptr),
     object_detections_(_object_dets)
 {
+    string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+    for (auto& elt: object_detections_){
+        ParamsCapturePtr params_capture = createParamsCapture(wolf_root + "/yaml_error/" + elt.object_type + ".yaml");
+        Matrix6d cov = polynomial_covariance(params_capture, elt, 1);
+        elt.meas_cov = cov;
+    }
 }
 
 CaptureObject::~CaptureObject()

src/yaml/capture_yaml.cpp

+/**
+ * \file capture_yaml.cpp
+ *
+ *  Created on: July 20, 2021
+ *      \author: cézou
+ */
+
+
+// wolf
+#include "core/yaml/yaml_conversion.h"
+#include "core/common/factory.h"
+#include "objectslam/capture/capture_object.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsCapturePtr createParamsCapture(const std::string & _filename_dot_yaml)
+{
+    YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+    if (config.IsNull())
+    {
+        WOLF_ERROR("Invalid YAML file!");
+        return nullptr;
+    }
+    else if (config["type"].as<std::string>() == "Capture")
+    {
+        ParamsCapturePtr params = std::make_shared<ParamsCapture>();
+
+        params->object_name            = config["object_name"]                  .as<string>();
+        params->coefs                  = config["coefs"]                        .as<string>();
+
+        return params;
+    }
+    else
+    {
+        WOLF_ERROR("Wrong capture type! Should be \"Capture\"");
+        return nullptr;
+    }
+    return nullptr;
+}
+
+} // namespace [unnamed]
+
+} // namespace wolf
\ No newline at end of file

test/gtest_capture_object.cpp

@@ -31,8 +31,8 @@ TEST_F(CaptureObject_test, type)
 
 TEST_F(CaptureObject_test, getObjectType)
 {
-    ObjectDetection det0 = {.measurement = pose_, .meas_cov=cov_, .object_type = "type0"};
-    ObjectDetection det1 = {.measurement = pose_, .meas_cov=cov_, .object_type = "type1"};
+    ObjectDetection det0 = {.measurement = pose_, .detection_score = 1, .meas_cov=cov_, .object_type = "type0"};
+    ObjectDetection det1 = {.measurement = pose_, .detection_score = 1, .meas_cov=cov_, .object_type = "type1"};
     dets_.push_back(det0);
     dets_.push_back(det1);
 

yaml_error/obj_000023.yaml

+type: "Capture"
+
+object_name: "obj_000023"
+coefs_err0: [    0.0,            -5.49415846,    -0.90561557,     0.25260086,
+   -188.89572449,    -1.84819766,     0.08833344,     0.04349455,
+      6.61270576,     0.01039684,    -0.00797195,     0.88375177,
+      0.00300739,    -0.26274358,    94.42806456, 94.31111302]
+coefs_err1: [    0.0,           -26.3578467,      0.88105233,    -1.21466815,
+   -290.57726492,    -0.16816331,    -0.01549064,     0.07092831,
+     26.52885062,     0.00296199,    -0.0026998,     -0.87981763,
+     -0.00040798,     1.199974,     142.28390016, 148.26856152]
+coefs_err2: [    0.0        ,   -23.4555953 ,     0.04889749,     2.22039061,
+    196.69199305,     0.23828264,     0.03281032,     0.05259529,
+     23.37472142,     0.00116761,    -0.0042342 ,    -0.058832  ,
+      0.00800276,    -2.23231417,  -102.60833938, -94.07424696]
+coefs_err3: [    0.0        ,   617.77050673,    17.86367686,     4.22699675,
+  -7514.28225672,   -10.19709467,    -1.4818733 ,     0.33980374,
+   -610.33511486,     0.08279767,    -0.01845979,   -17.49343384,
+      0.13471899,    -4.40513538,  3860.21826547, 3652.80356821]
+coefs_err4: [    0.0        ,   163.31155923,    -6.05859545,   -29.74259391,
+   8716.71727933,   -10.1898619 ,    -0.19725405,     1.76903902,
+   -156.99581058,     0.09969996,    -0.03699473,     6.13059822,
+      0.05353654,    29.22240716, -4357.60441491, -4360.02244961]
+coefs_err5: [    0.        ,   399.44625291,    -0.51533294,   -13.92739457,
+  -2735.40123406,   -19.36467958,     1.03782671,    -0.04400654,
+   -388.11877975,     0.09935755,    -0.05061968,     0.21596979,
+     -0.03411179,    13.91806081,  1428.47940411, 1305.38400883]
\ No newline at end of file

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]
\ No newline at end of file

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]
\ No newline at end of file