diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index b9c14f9d02b085c601adbf30cdf37b277d08653f..23b6e315782d514457ab1abc8a63c8a20167d502 100644
--- a/demos/cosyslam.cpp
+++ b/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;
diff --git a/src/capture/capture_object.cpp b/src/capture/capture_object.cpp
index eb209e96e67fc6ee3e80dd6ffb5d781a277e36ac..d22f0e4a57f55c6c7725ec77a25da05c87ca9a4e 100644
--- a/src/capture/capture_object.cpp
+++ b/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
diff --git a/yaml_error/obj_000025.yaml b/yaml_error/obj_000025.yaml
index 3e032574af13a503caaa6b06016423828f7232aa..8a649028aeb5afadde98f4d409600bf430f4bdac 100644
--- a/yaml_error/obj_000025.yaml
+++ b/yaml_error/obj_000025.yaml
@@ -1,9 +1,27 @@
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
+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,
+ 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]
diff --git a/yaml_error/obj_000026.yaml b/yaml_error/obj_000026.yaml
index 742335b117a54f36b077dc0b1a5b83113eacc440..8a649028aeb5afadde98f4d409600bf430f4bdac 100644
--- a/yaml_error/obj_000026.yaml
+++ b/yaml_error/obj_000026.yaml
@@ -1,9 +1,27 @@
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
+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,
+ 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]