diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp index e6c256c192b55ef6174859b1549df3c26f957a9c..b9c14f9d02b085c601adbf30cdf37b277d08653f 100644 --- a/demos/cosyslam.cpp +++ b/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 diff --git a/demos/demo_toy_pbe.cpp b/demos/demo_toy_pbe.cpp index e1a8e0fe1fa9cc32e27454f10c17ee235661708b..b8941930d607e418f9827416b471016a026c6c1f 100644 --- a/demos/demo_toy_pbe.cpp +++ b/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); diff --git a/include/objectslam/capture/capture_object.h b/include/objectslam/capture/capture_object.h index c5b0b2b83412fb953acc28f46d77fdf7c65e1bd3..1a49c2c30b36e3ac542d18db2a5e526f39833874 100644 --- a/include/objectslam/capture/capture_object.h +++ b/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; diff --git a/src/capture/capture_object.cpp b/src/capture/capture_object.cpp index c2b1a341cac109474cc2c3bf23686dad68259888..eb209e96e67fc6ee3e80dd6ffb5d781a277e36ac 100644 --- a/src/capture/capture_object.cpp +++ b/src/capture/capture_object.cpp @@ -1,12 +1,87 @@ #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() diff --git a/src/yaml/capture_yaml.cpp b/src/yaml/capture_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..582c1b88455cd64c7d1e9674e391e0e0004acc56 --- /dev/null +++ b/src/yaml/capture_yaml.cpp @@ -0,0 +1,50 @@ +/** + * \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 diff --git a/test/gtest_capture_object.cpp b/test/gtest_capture_object.cpp index ae24f0da5c9583e8fc80648a3709ccc7dd62a3d6..89cd13e06546ae56f601ab640ab2a4edf2310979 100644 --- a/test/gtest_capture_object.cpp +++ b/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); diff --git a/yaml_error/obj_000023.yaml b/yaml_error/obj_000023.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f9130e7674fd7e20cbaaadf44cb87d1cb57c96ec --- /dev/null +++ b/yaml_error/obj_000023.yaml @@ -0,0 +1,27 @@ +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 diff --git a/yaml_error/obj_000025.yaml b/yaml_error/obj_000025.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3e032574af13a503caaa6b06016423828f7232aa --- /dev/null +++ b/yaml_error/obj_000025.yaml @@ -0,0 +1,9 @@ +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 diff --git a/yaml_error/obj_000026.yaml b/yaml_error/obj_000026.yaml new file mode 100644 index 0000000000000000000000000000000000000000..742335b117a54f36b077dc0b1a5b83113eacc440 --- /dev/null +++ b/yaml_error/obj_000026.yaml @@ -0,0 +1,9 @@ +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