cosyslam demo

demos/CMakeLists.txt

@@ -6,4 +6,10 @@ TARGET_LINK_LIBRARIES(demo_toy_pbe
     ${PLUGIN_NAME}
 )
 
+ADD_EXECUTABLE(cosyslam cosyslam.cpp)
+TARGET_LINK_LIBRARIES(cosyslam 
+    ${wolfcore_LIBRARIES}
+    ${PLUGIN_NAME}
+)
+
 

demos/cosyslam.cpp

+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <stdexcept>
+#include <utility>
+#include <string>
+#include <Eigen/Dense>
+
+#include "core/problem/problem.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include "core/sensor/sensor_pose.h" // not necessary
+#include "core/frame/frame_base.h"
+
+#include "objectslam/processor/processor_tracker_landmark_object.h"
+#include "objectslam/capture/capture_object.h"
+#include "objectslam/internal/config.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+Isometry3d posevec_to_isometry(Vector7d pose)
+{
+    return Translation<double, 3>(pose.head<3>()) * Quaterniond(pose.tail<4>());
+}
+
+Vector7d isometry_to_posevec(Isometry3d M)
+{
+    Vector7d pose_vec;
+    pose_vec << M.translation(), Quaterniond(M.rotation()).coeffs();
+    return pose_vec;
+}
+
+std::vector<std::pair<std::string, std::vector<std::string>>> read_csv(std::string filename)
+{
+    // create an input filestream
+    std::ifstream myFile(filename);
+
+    // Create a vector of <string, int vector> pairs to store the result
+    std::vector<std::pair<std::string, std::vector<std::string>>> result;
+
+    // Make sure the file is open
+    if (!myFile.is_open())
+        throw std::runtime_error("Could not open file");
+
+    // Variables
+    std::string line, colname, val;
+
+    // read the columns name
+    if (myFile.good())
+    {
+
+        // extract the first line in the file
+        std::getline(myFile, line);
+
+        // Create a stringstream from line
+        std::stringstream ss(line);
+
+        // Extract each column name
+        while (std::getline(ss, colname, ','))
+        {
+
+            // Initialize and add <colname, string vector> pairs to result
+            result.push_back({colname, std::vector<std::string>{}});
+        }
+    }
+
+    // Read data, line by line
+    while (std::getline(myFile, line))
+    {
+        // Create a stringstream of the current line
+        std::stringstream ss(line);
+
+        // Keep track of the current column index
+        int colIdx = 0;
+
+        // Extract each string and store it in result
+        while (std::getline(ss, val, ','))
+        {
+            result.at(colIdx).second.push_back(val);
+            colIdx++;
+        }
+    }
+
+    // Close file
+    myFile.close();
+
+    return result;
+}
+
+Isometry3d str_to_isometry(std::string pose)
+{
+    // Let's initialize the eigen objects
+    Matrix4d pose_matrix;
+
+    // Parse the string
+    std::string::size_type sz;
+    for (int i = 0; i < 4; i++)
+    {
+        for (int j = 0; j < 4; j++)
+        {
+            pose_matrix(i, j) = std::stod(pose, &sz);
+            pose = pose.substr(sz);
+        }
+    }
+
+    // Create the transformation
+    Isometry3d trans;
+    trans.linear() = pose_matrix.topLeftCorner(3, 3);
+    trans.translation() = pose_matrix.topRightCorner(3, 1);
+
+    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()
+{
+
+    std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+    std::string filename = wolf_root + "/demos/test.csv";
+    std::vector<std::pair<std::string, std::vector<std::string>>> csv_values;
+    csv_values = read_csv(filename);
+
+    /////////////////////////////
+    // setup the wolf problem
+    /////////////////////////////
+
+    ProblemPtr problem = Problem::create("PO", 3);
+    SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+    solver->getSolverOptions().max_num_iterations = 200;
+
+    ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
+    auto sen = problem->installSensor("SensorPose", "sensor_pose", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), params_sp);
+    auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/processor_tracker_landmark_object.yaml");
+    auto prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject>(prc);
+
+    VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
+    VectorComposite sig0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
+    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
+
+    ObjectDetections dets;
+
+    // Parsing variables
+    int len = csv_values.at(0).second.size();
+    int idx_cov = 9;
+    int idx_pose = 4;
+    int idx_obj = 3;
+    int idx_t = 8;
+
+    for (int i = 0; i < len; i++)
+    {
+        std::cout << i << std::endl;
+
+        // Measurement data
+        Isometry3d c_M_o;
+        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));
+        std::string object_name;
+        object_name = csv_values.at(idx_obj).second.at(i);
+        double t;
+        t = std::stod(csv_values.at(idx_t).second.at(i));
+
+        // Capture processing
+        dets.clear();
+        ObjectDetection det = {.measurement = isometry_to_posevec(c_M_o), .meas_cov = cov, .object_type = object_name};
+        dets.push_back(det);
+        CaptureObjectPtr cap = std::make_shared<CaptureObject>(t, sen, dets);
+        cap->process();
+    }
+
+    std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
+    std::cout << report << std::endl;
+
+    problem->print(4,true,true,true);
+
+    // Save the results
+    std::fstream file_res;
+    std::fstream file_map;
+    std::string res_filename = wolf_root + "/demos/result.csv";
+    std::string map_filename = wolf_root + "/demos/map.csv";
+    file_res.open(res_filename, std::fstream::out);
+    file_map.open(map_filename, std::fstream::out);
+
+    // Write headers for csv files
+    std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw\n";
+    file_res << header_res;
+    std::string header_map = "id,px,py,pz,qx,qy,qz,qw\n";
+    file_map << header_map;
+
+    VectorComposite state_est;
+    CaptureBasePtr cap_cosy;
+    int counter = 0;
+    for (auto& elt: problem->getTrajectory()->getFrameMap()){ 
+        TimeStamp t = elt.first; 
+        FrameBasePtr kf = elt.second; 
+        state_est = kf->getState();
+        file_res << counter << "," 
+                 << t << ","
+                 << state_est['P'](0) << ","
+                 << state_est['P'](1) << ","
+                 << state_est['P'](2) << "," 
+                 << state_est['O'](0) << "," 
+                 << state_est['O'](1) << "," 
+                 << state_est['O'](2) << "," 
+                 << state_est['O'](3) << "," 
+                 << "\n";
+        counter++; 
+    }
+
+    counter = 0;
+    for (auto& elt: problem->getMap()->getLandmarkList()){
+        state_est = elt->getState();
+        file_map << counter << ","
+                 << state_est['P'](0) << ","
+                 << state_est['P'](1) << ","
+                 << state_est['P'](2) << "," 
+                 << state_est['O'](0) << "," 
+                 << state_est['O'](1) << "," 
+                 << state_est['O'](2) << "," 
+                 << state_est['O'](3) << "," 
+                 << "\n";
+        counter++; 
+    } 
+    file_res.close();
+    file_map.close();
+
+    return 0;
+}
\ No newline at end of file

demos/map.csv

+id,px,py,pz,qx,qy,qz,qw
+0,0.0118101,-0.0103998,0.42091,-0.00461907,0.771858,-0.628642,0.0949934,

demos/processor_tracker_landmark_object.yaml

@@ -5,7 +5,7 @@ time_tolerance: 0.1222
 vote:
     voting_active:              true
     min_time_vote:              0 # s
-    max_time_vote:              1.001 # s
+    max_time_vote:              0.1 # s
     min_features_for_keyframe:  1
     nb_vote_for_every_first:    0
 

demos/result.csv

+id,t,px,py,pz,qx,qy,qz,qw
+0,0.000000000,1.84381e-13,-2.798e-11,-1.70113e-12,6.32843e-10,3.91335e-11,7.99363e-11,1,
+1,0.099518000,-0.0104137,0.0077851,0.00181263,0.012509,0.00531949,0.00100668,0.999907,
+2,0.199035000,-0.0150023,0.00500016,0.00324575,0.0109784,0.0036419,-0.00238622,0.99993,
+3,0.298553000,-0.022212,0.000700687,0.00358512,0.00702993,0.00392236,0.00168232,0.999966,
+4,0.398070000,-0.043199,-0.00203051,0.00757717,0.00688483,0.0165713,0.00568339,0.999823,
+5,0.497588000,-0.0460949,-0.00298447,0.0056032,0.00580757,0.00856018,0.00211528,0.999944,
+6,0.597106000,-0.0498825,-0.00230925,0.0059157,0.0083041,0.00695186,0.0006663,0.999941,
+7,0.696623000,-0.056499,-0.00266882,0.00454185,0.009491,0.00548888,0.0055494,0.999924,
+8,0.796141000,-0.0875173,-0.021327,0.0180811,-0.0102778,0.0388762,0.0188157,0.999014,
+9,0.895658000,-0.0784256,-0.0213351,0.0145278,-0.0111415,0.030505,0.0135028,0.999381,
+10,0.995176000,-0.0933745,-0.0336061,0.0249545,-0.0261619,0.0606001,0.0267275,0.997461,
+11,1.094694000,-0.1056,-0.033298,0.0261718,-0.0239909,0.0825309,0.0372965,0.995601,
+12,1.194211000,-0.114915,-0.0265249,0.034208,-0.0172216,0.0956748,0.0446871,0.99426,
+13,1.293729000,-0.120425,-0.0362641,0.042266,-0.0238592,0.100829,0.0534247,0.993182,
+14,1.393246000,-0.139274,-0.0487197,0.0550595,-0.0270022,0.121781,0.0730903,0.989494,
+15,1.492764000,-0.148259,-0.0450491,0.0579103,-0.0184896,0.128445,0.0712859,0.988978,
+16,1.592281000,-0.143472,-0.0414073,0.0573522,-0.0171456,0.122076,0.0652742,0.990224,
+17,1.691799000,-0.137973,-0.0432384,0.0556763,-0.0227657,0.115468,0.0665541,0.990818,
+18,1.791317000,-0.121846,-0.0433565,0.0480688,-0.0251673,0.094104,0.059017,0.993493,
+19,1.890834000,-0.120699,-0.0420157,0.0497147,-0.0242278,0.0955087,0.0511068,0.993821,
+20,1.990352000,-0.11225,-0.0407377,0.0476949,-0.0237189,0.0901472,0.047366,0.994519,
+21,2.089869000,-0.0896885,-0.0431687,0.0457076,-0.0332902,0.0737242,0.0410016,0.995879,
+22,2.189387000,-0.0916551,-0.0425754,0.0545092,-0.0343731,0.0951581,0.0446645,0.993865,
+23,2.288905000,-0.0473993,-0.0139747,0.0420146,-0.0011778,0.0459912,0.0279583,0.99855,
+24,2.388422000,-0.0266703,-0.0259846,0.0467664,-0.0246655,0.0347434,0.0307871,0.998617,
+25,2.454767000,-0.0158515,-0.0183473,0.0454522,-0.0187268,0.029791,0.0266378,0.999026,
+26,2.554285000,0.0168385,-0.00714708,0.0500795,-0.00987754,-0.00299172,0.0229145,0.999684,
+27,2.653802000,0.0323923,-0.00174456,0.0462748,-0.00618191,-0.0126582,0.0187626,0.999725,
+28,2.753320000,0.0587395,-0.00156211,0.0519053,-0.0110108,-0.0357037,0.0100864,0.999251,
+29,2.852837000,0.0736532,0.00130476,0.051088,-0.0275864,-0.0449859,0.00827483,0.998572,
+30,2.952355000,0.103807,0.0131061,0.0595308,-0.0228591,-0.0827264,0.00852907,0.996274,
+31,3.051873000,0.123449,0.019038,0.0606726,-0.0147712,-0.107694,0.000944441,0.994074,
+32,3.151390000,0.14723,0.030637,0.0689698,-0.00947022,-0.144728,-0.00303976,0.989422,
+33,3.250908000,0.171337,0.0251339,0.0783293,-0.0232564,-0.176322,-0.00718808,0.984032,
+34,3.350425000,0.182818,0.0106731,0.0887363,-0.0390438,-0.191863,-0.00832991,0.980609,
+35,3.449943000,0.195935,0.0117309,0.096873,-0.0330681,-0.200225,-0.00886433,0.979152,
+36,3.549461000,0.224604,0.00575342,0.113806,-0.036768,-0.218022,-0.0211271,0.975022,
+37,3.648978000,0.238399,0.0162188,0.121392,-0.0255938,-0.21069,-0.0144261,0.977111,
+38,3.748496000,0.248664,0.0159724,0.12947,-0.0324751,-0.206609,-0.0127105,0.977802,
+39,3.848013000,0.26324,0.0166943,0.137962,-0.0306332,-0.221492,-0.0177966,0.974518,
+40,3.947531000,0.258683,0.0158601,0.141042,-0.0376487,-0.212692,-0.0146157,0.976284,
+41,4.047048000,0.251891,0.0153758,0.138987,-0.0379931,-0.201472,-0.0117588,0.978686,
+42,4.146566000,0.251266,0.00827269,0.147562,-0.0434442,-0.199653,-0.0218317,0.97866,
+43,4.246083999,0.240591,0.0179148,0.146538,-0.0253465,-0.18316,-0.0089846,0.982715,
+44,4.345601000,0.217858,0.0172101,0.126079,-0.0233371,-0.152513,0.00630314,0.988006,
+45,4.445119000,0.211473,0.0181028,0.127235,-0.0173805,-0.154801,0.00703134,0.987768,
+46,4.544636000,0.194919,0.0120887,0.123016,-0.0238574,-0.136024,0.00767788,0.990388,
+47,4.644154000,0.17808,0.00930125,0.118699,-0.0350668,-0.112572,0.0168418,0.992882,
+48,4.743672000,0.164653,0.000632572,0.114512,-0.0441494,-0.0993138,0.0134491,0.993985,
+49,4.843189000,0.149283,0.0124682,0.105175,-0.0258574,-0.0887002,0.0249288,0.995411,
+50,4.942707000,0.120361,0.0113026,0.0938186,-0.0238952,-0.0649532,0.0316011,0.997102,
+51,5.042224000,0.091644,0.0020601,0.0857252,-0.0326401,-0.0446763,0.0373065,0.997771,
+52,5.141742000,0.0565977,-0.0104995,0.0827468,-0.0392545,-0.0180505,0.0400642,0.998263,
+53,5.241259000,0.0375933,-0.0261576,0.0810137,-0.0460884,-0.00750037,0.0391917,0.99814,
+54,5.340777000,0.0303904,-0.0354298,0.0674925,-0.0439438,-0.00208782,0.0453312,0.998003,
+55,5.440295000,0.0163903,-0.0400611,0.0535455,-0.0465724,0.0192031,0.0500442,0.997476,
+56,5.539812000,0.0112069,-0.0443687,0.035413,-0.0499705,0.0266689,0.0487605,0.997203,
+57,5.639330000,0.0125292,-0.0419698,0.0161972,-0.0490445,0.0323669,0.0402546,0.99746,
+58,5.738847000,0.0134146,-0.0450088,-0.00116133,-0.0526253,0.0396234,0.038302,0.997093,
+59,5.838365000,0.0149326,-0.0483298,-0.0216591,-0.0526733,0.0444989,0.0324132,0.997093,
+60,5.937882000,0.0152858,-0.053366,-0.0406393,-0.0529505,0.046936,0.0347396,0.996888,
+61,6.037400000,0.0162815,-0.062939,-0.065322,-0.0569219,0.0447831,0.0329997,0.996828,
+62,6.136918000,0.00414485,-0.0690722,-0.0866392,-0.055911,0.0554775,0.033223,0.99634,
+63,6.236435000,0.00472297,-0.0686868,-0.110617,-0.0541613,0.0504789,0.0306583,0.996784,
+64,6.335953000,0.00398202,-0.0752438,-0.131449,-0.0600321,0.0459694,0.0270537,0.99677,
+65,6.435470000,0.0172541,-0.0724002,-0.155408,-0.0551584,0.0327342,0.0210449,0.997719,
+66,6.534988000,0.0171284,-0.0823973,-0.185068,-0.061622,0.0245121,0.0211813,0.997574,
+67,6.634506000,0.0152082,-0.0775584,-0.209378,-0.0563504,0.00859359,0.0140341,0.998275,
+68,6.734023000,0.0150379,-0.0703369,-0.248667,-0.0513236,-0.0102172,0.00688141,0.998606,
+69,6.833541000,0.0162264,-0.0740766,-0.27287,-0.0575273,-0.0296271,0.00960728,0.997858,
+70,6.899886000,0.0178112,-0.0752163,-0.282423,-0.0715167,-0.0408183,0.0020104,0.996602,
+71,7.032576000,0.0346229,-0.0621397,-0.290854,-0.067904,-0.0653595,-0.00767216,0.995519,
+72,7.132093000,0.0386856,-0.0683244,-0.294024,-0.0763505,-0.063426,-0.0145615,0.994955,
+73,7.231611000,0.0425411,-0.0479403,-0.289069,-0.0630441,-0.0632099,-0.0163591,0.995873,
+74,7.331129000,0.0525976,-0.00951206,-0.288911,-0.0490016,-0.0703121,-0.0135914,0.996228,
+75,7.430646000,0.0723648,0.0273674,-0.278411,-0.0366176,-0.0876624,-0.0159621,0.995349,
+76,7.530164000,0.0705168,0.0427728,-0.270447,-0.0402731,-0.0879527,-0.0193786,0.995122,
+77,7.629681000,0.0925511,0.04559,-0.249387,-0.0498127,-0.102857,-0.0256313,0.993117,
+78,7.729199000,0.089142,0.0537832,-0.230176,-0.0564244,-0.0925481,-0.0275528,0.993726,
+79,7.828717000,0.0821574,0.0693259,-0.206654,-0.0511938,-0.0792254,-0.0183612,0.995372,
+80,7.928234000,0.0845874,0.0797422,-0.184352,-0.0399376,-0.0712882,-0.015193,0.99654,
+81,8.027752000,0.0815232,0.0538593,-0.162747,-0.056871,-0.0603787,-0.0130122,0.996469,
+82,8.127269000,0.0791333,0.0578464,-0.13617,-0.0481003,-0.0535571,-0.00277099,0.997402,
+83,8.226787000,0.0785472,0.0431487,-0.11077,-0.0520426,-0.0549184,-0.000718456,0.997133,
+84,8.326304000,0.0724949,0.0525543,-0.0889835,-0.0334881,-0.0567562,-0.00390391,0.997819,
+85,8.425822000,0.0706863,0.0645771,-0.0584816,-0.0228621,-0.0561139,-0.00659697,0.998141,
+86,8.525340000,0.072803,0.046609,-0.0332548,-0.0241582,-0.0583373,-0.00348289,0.997999,
+87,8.624857000,0.069845,0.029253,-0.00308634,-0.0348911,-0.0498759,0.00177635,0.998144,
+88,8.724375000,0.0692812,0.0193106,0.0171899,-0.0355564,-0.0466686,0.00576222,0.998261,
+89,8.823892000,0.0697556,0.0192878,0.0346468,-0.0220945,-0.0443491,0.0107119,0.998714,
+90,8.923410000,0.0769873,0.00953147,0.0571878,-0.0197131,-0.0494594,0.0119115,0.998511,
+91,9.022927000,0.0697975,0.000874978,0.0746585,-0.0122585,-0.0318222,0.0160514,0.999289,
+92,9.122445000,0.0573281,-0.0206904,0.0874244,-0.0273264,-0.00317054,0.023698,0.999341,
+93,9.221963000,0.0603567,-0.0392483,0.0869072,-0.0449545,0.00192528,0.0225709,0.998732,
+94,9.321480000,0.056046,-0.0497229,0.0857417,-0.0675955,0.0106935,0.0198066,0.997459,
+95,9.420998000,0.0561238,-0.059022,0.0757563,-0.101091,0.00633783,0.00813291,0.994824,
+96,9.520515000,0.058809,-0.0559871,0.0657321,-0.108285,-0.000745311,0.000813119,0.994119,
+97,9.620033000,0.0419056,-0.0707626,0.0517127,-0.148834,0.0207255,0.0134448,0.988554,
+98,9.719551000,0.0272557,-0.0902503,0.0489875,-0.198014,0.0349692,0.0208193,0.979354,
+99,9.819068000,0.0309949,-0.101266,0.0402726,-0.209646,0.0183186,0.0148517,0.977493,
+100,9.918586000,0.0394371,-0.110608,0.0288683,-0.225181,-0.000799872,0.0125256,0.974236,
+101,10.018103000,0.040246,-0.123661,0.0251319,-0.241906,-0.00314017,0.0143593,0.970188,
+102,10.117621000,0.0540018,-0.127667,0.0226253,-0.25238,-0.0132895,0.00485339,0.967525,
+103,10.217138000,0.0631226,-0.130795,0.0252264,-0.261089,-0.0139497,0.00357404,0.965207,
+104,10.316656000,0.0703676,-0.155488,0.0362531,-0.281187,-0.0138255,0.0103031,0.959498,
+105,10.416174000,0.0752088,-0.162294,0.0426866,-0.293234,-0.0158885,0.0170967,0.955756,
+106,10.515691000,0.0889143,-0.177128,0.0442056,-0.297123,-0.0303522,0.0145315,0.954246,
+107,10.615209000,0.0843939,-0.182431,0.0418268,-0.293906,-0.0248731,0.0239809,0.95521,
+108,10.714726000,0.0881223,-0.181943,0.0378129,-0.289595,-0.0337823,0.0165374,0.95641,
+109,10.814244000,0.068785,-0.17998,0.0321479,-0.286489,-0.0111194,0.0238578,0.957722,
+110,10.913762000,0.0769703,-0.169494,0.0296451,-0.268371,-0.0219404,0.0137906,0.962967,
+111,11.013279000,0.069634,-0.148636,0.0225558,-0.233637,-0.00744512,0.0190131,0.97211,
+112,11.112797000,0.0632018,-0.14079,0.0226302,-0.212582,0.00352945,0.0238063,0.976847,
+113,11.212314000,0.0531655,-0.125949,0.0216365,-0.184704,0.0174035,0.023767,0.982353,
+114,11.311832000,0.0399421,-0.110837,0.0297894,-0.165153,0.0408712,0.0292283,0.984987,
+115,11.411349000,0.0381459,-0.0954268,0.0427067,-0.13699,0.0530509,0.0291966,0.98872,
+116,11.510867000,0.0396894,-0.0826852,0.0521846,-0.109137,0.0619422,0.0305627,0.991624,
+117,11.610385000,0.0497736,-0.0611696,0.0646781,-0.0730433,0.0497281,0.0233725,0.995814,
+118,11.709902000,0.0533534,-0.0569946,0.0731508,-0.0583674,0.0493631,0.0132993,0.996985,
+119,11.809420000,0.0491284,-0.0531697,0.0820769,-0.0556825,0.0547627,0.00542945,0.996931,
+120,11.908937000,0.0448327,-0.0548328,0.0820293,-0.0444708,0.0542148,-0.0142108,0.997437,
+121,12.008455000,0.0458415,-0.0639888,0.0767052,-0.0491098,0.0473662,-0.0457252,0.996621,
+122,12.107973000,0.050727,-0.070487,0.0784987,-0.0666401,0.0294514,-0.0782115,0.994271,
+123,12.207490000,0.0572982,-0.0671847,0.0764452,-0.0667752,0.0117672,-0.106679,0.991979,
+124,12.307008000,0.0619866,-0.0567163,0.0702281,-0.0475529,0.000751524,-0.136666,0.989475,
+125,12.406525000,0.0755938,-0.0410174,0.066231,-0.0214464,-0.0205741,-0.169173,0.985138,
+126,12.506043000,0.081834,-0.0376735,0.0629567,-0.0132537,-0.0304331,-0.205027,0.978193,
+127,12.605560000,0.0654946,-0.030638,0.0581242,-0.0061305,-0.00770281,-0.229276,0.973312,
+128,12.705078000,0.0576659,-0.0299603,0.0556941,-0.00502055,0.00559095,-0.249172,0.96843,
+129,12.804596000,0.0543494,-0.033653,0.0553541,-0.0155178,0.00772726,-0.266656,0.963636,
+130,12.904113000,0.0713449,-0.043482,0.057064,-0.0301959,-0.0202825,-0.252863,0.966818,
+131,13.003631000,0.0680846,-0.040269,0.052959,-0.0252205,-0.0106395,-0.221969,0.974669,
+132,13.103148000,0.0718916,-0.0409655,0.0552942,-0.0292326,-0.0168455,-0.190047,0.981195,
+133,13.202666000,0.0648447,-0.0534973,0.0590963,-0.0542405,-0.0033763,-0.148762,0.987379,
+134,13.302183000,0.0539831,-0.0727274,0.0587357,-0.0899558,0.0121011,-0.106478,0.990164,
+135,13.401701000,0.0431021,-0.0733317,0.053925,-0.0920229,0.0275593,-0.0484777,0.994194,
+136,13.501219000,0.0321343,-0.0693872,0.0549765,-0.0955572,0.0323155,-0.0150932,0.994785,
+137,13.600736000,0.0320299,-0.0763657,0.0563521,-0.100079,0.0285241,0.0149607,0.994458,
+138,13.700254000,0.0297575,-0.0939197,0.0524466,-0.103565,0.029246,0.0382136,0.993458,
+139,13.799771000,0.0320771,-0.0937671,0.0497685,-0.0863712,0.0155977,0.0391974,0.995369,
+140,13.899289000,0.0316049,-0.103048,0.0462776,-0.0856385,0.010246,0.0402937,0.995458,
+141,13.998807000,0.0299156,-0.101265,0.0419338,-0.0895835,0.0153219,0.0404324,0.99504,
+142,14.098324000,0.032881,-0.0949276,0.0422738,-0.0916909,0.0118868,0.0379333,0.994994,
+143,14.197842000,0.0330255,-0.0918447,0.0412266,-0.0894177,0.0122519,0.0399016,0.995119,
+144,14.297359000,0.0338196,-0.0932731,0.0424361,-0.0917032,0.00997358,0.0334223,0.995175,
+145,14.396877000,0.0306872,-0.0954926,0.0437848,-0.0958261,0.0150253,0.0387382,0.994531,
+146,14.496394000,0.0313544,-0.0918321,0.0469432,-0.0943184,0.0135618,0.0411557,0.994599,
+147,14.595912000,0.0342644,-0.0906378,0.048435,-0.0933246,0.00878221,0.034582,0.994996,
+148,14.695430000,0.0285105,-0.0891108,0.0481628,-0.0907282,0.0160101,0.0344015,0.995153,
+149,14.794947000,0.0304786,-0.0898752,0.0498181,-0.0918774,0.0142336,0.0309056,0.995189,
+150,14.894465000,0.0314291,-0.0904777,0.0523856,-0.091754,0.0120071,0.0309784,0.995227,
+151,14.993982000,0.030663,-0.0927115,0.0534406,-0.0948109,0.0122253,0.033287,0.994864,
+152,15.093500000,0.0312085,-0.0939924,0.0539139,-0.096421,0.00975606,0.0308862,0.994813,
+153,15.193018000,0.0239234,-0.0929377,0.0537589,-0.0972926,0.018603,0.0304691,0.994615,
+154,15.292535000,0.0235872,-0.0931819,0.0559615,-0.101282,0.0188743,0.0287564,0.994263,
+155,15.392053000,0.0248763,-0.091923,0.0552244,-0.098166,0.0148479,0.0277217,0.994673,
+156,15.491570000,0.0266008,-0.091795,0.0562652,-0.0985591,0.0114943,0.0239646,0.994776,
+157,15.591088000,0.0268559,-0.0906483,0.0566647,-0.0985346,0.0123805,0.0207945,0.994839,
+158,15.690605000,0.0247102,-0.0886304,0.055933,-0.0970209,0.0165746,0.0198804,0.994946,
+159,15.790123000,0.0231996,-0.0890532,0.0576433,-0.0992022,0.0189015,0.0200766,0.994685,
+160,15.889641000,0.0255563,-0.0849885,0.0578528,-0.094851,0.0168522,0.0194006,0.99516,
+161,15.989158000,0.0277467,-0.0848212,0.0593826,-0.0953652,0.0136328,0.0173066,0.995199,
+162,16.088676000,0.0277667,-0.0833906,0.0598822,-0.0931765,0.0134077,0.0174467,0.995406,

include/objectslam/feature/feature_object.h

@@ -14,7 +14,7 @@ class FeatureObject : public FeatureBase
 {
     public:
 
-        FeatureObject(const Eigen::Vector7d & _measurement, 
+        FeatureObject(const Eigen::Vector7d & _measurement,
                       const Eigen::Matrix6d & _meas_covariance, 
                       const std::string & _object_type,
                       UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
@@ -25,7 +25,7 @@ class FeatureObject : public FeatureBase
          * Returns object type
          * 
          **/
-        std::string getObjectType() const; 
+        std::string getObjectType() const;
 
 
     private: