Skip to content
Snippets Groups Projects
Commit 4ac43f4a authored by Cesar Debeunne's avatar Cesar Debeunne
Browse files

edit capture object & add new models

parent a0034488
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
demos/cosyslam.cpp
+ 4
− 21
View file @ 4ac43f4a
......@@ -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
+ 12
− 3
View file @ 4ac43f4a
......@@ -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
+ 25
− 7
View file @ 4ac43f4a
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]
yaml_error/obj_000026.yaml
+ 24
− 6
View file @ 4ac43f4a
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]
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment