Merge branch 'master' into motion_laser

src/examples/CMakeLists.txt

+# Matrix product test
+#ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
 
 # ProcessorMotion classes test
 ADD_EXECUTABLE(test_motion test_motion.cpp)

src/examples/test_matrix_prod.cpp

+/**
+ * \file test_matrix_prod.cpp
+ *
+ *  Created on: May 26, 2016
+ *      \author: jsola
+ */
+
+#include "eigen3/Eigen/Dense"
+
+//std includes
+#include <ctime>
+#include <iostream>
+#include <iomanip>
+
+#define DECLARE_MATRICES(s) \
+        Matrix<double, s, s, RowMajor> R1, R2, Ro; \
+        Matrix<double, s, s, ColMajor> C1, C2, Co;
+
+#define INIT_MATRICES(s) \
+        R1.setRandom(s, s);\
+        R2.setRandom(s, s);\
+        C1.setRandom(s, s);\
+        C2.setRandom(s, s);\
+        Ro.setRandom(s, s);\
+        Co.setRandom(s, s);
+
+
+#define LOOP_MATRIX(N,Mo,M1,M2) \
+        for (int i = 0; i < N; i++) \
+        { \
+            Mo = M1 * M2; \
+            M1(2,2) = Mo(2,2); \
+        }
+
+#define EVALUATE_MATRIX(N,Mo,M1,M2) \
+        t0 = clock(); \
+        LOOP_MATRIX(N,Mo,M1,M2) \
+        t1 = clock(); \
+        std::cout << std::setw(15) << Mo(2,2) << "\t";
+
+#define EVALUATE_ALL \
+        EVALUATE_MATRIX(N, Ro, R1, R2)\
+        std::cout << "Time Ro = R * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Ro, R1, C2)\
+        std::cout << "Time Ro = R * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Ro, C1, R2)\
+        std::cout << "Time Ro = C * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Ro, C1, C2)\
+        std::cout << "Time Ro = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Co, R1, R2)\
+        std::cout << "Time Co = R * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Co, R1, C2)\
+        std::cout << "Time Co = R * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Co, C1, R2)\
+        std::cout << "Time Co = C * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
+        EVALUATE_MATRIX(N, Co, C1, C2)\
+        std::cout << "Time Co = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N \
+        << "ns <-- this is the Eigen default!" << std::endl;
+
+
+
+/**
+ * We multiply matrices and see how long it takes.
+ * We compare different combinations of row-major and column-major to see which one is the fastest.
+ * We can select the matrix size.
+ */
+int main()
+{
+    using namespace Eigen;
+
+    int N = 100000;
+    const int S = 6;
+    Matrix<double, 16, S - 3 + 1> results;
+    clock_t t0, t1;
+
+    // All dynamic sizes
+    {
+        Matrix<double, Dynamic, Dynamic, RowMajor> R1, R2, Ro;
+        Matrix<double, Dynamic, Dynamic, ColMajor> C1, C2, Co;
+
+        for (int s = 3; s <= S; s++)
+        {
+            std::cout << "Timings for dynamic matrix product. R: row major matrix. C: column major matrix. " << s << "x"
+                    << s << " matrices." << std::endl;
+
+            INIT_MATRICES(s)
+            EVALUATE_ALL
+
+        }
+    }
+    // Statics, one by one
+    {
+        const int s = 3;
+        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
+                << " matrices." << std::endl;
+
+        DECLARE_MATRICES(s)
+        INIT_MATRICES(s)
+        EVALUATE_ALL
+    }
+    {
+        const int s = 4;
+        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
+                << " matrices." << std::endl;
+
+        DECLARE_MATRICES(s)
+        INIT_MATRICES(s)
+        EVALUATE_ALL
+    }
+    {
+        const int s = 5;
+        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
+                << " matrices." << std::endl;
+
+        DECLARE_MATRICES(s)
+        INIT_MATRICES(s)
+        EVALUATE_ALL
+    }
+    {
+        const int s = 6;
+        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
+                << " matrices." << std::endl;
+
+        DECLARE_MATRICES(s)
+        INIT_MATRICES(s)
+        EVALUATE_ALL
+    }
+
+    std::cout << "Test q and R rotations" << std::endl;
+    Eigen::Quaterniond q(Eigen::Vector4d::Random().normalized());
+    Eigen::Matrix3d R = q.matrix();
+    Eigen::Vector3d v; v << 1,2,3; double vn = v.norm();
+
+    N *= 100;
+
+    t0 = clock();
+    for (int i = 0; i < N; i++)
+    {
+        v = R * v;
+    }
+    t1 = clock();
+    std::cout << "Time w = R * v: " << (double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;
+    std::cout << "v norm change: " << 10*logl((long double)v.norm()/(long double)vn) << " dB" << std::endl;
+
+    v << 1,2,3;
+    t0 = clock();
+    for (int i = 0; i < N; i++)
+    {
+        v = q * v;
+    }
+    t1 = clock();
+    std::cout << "Time w = q * v: " << (double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;
+    std::cout << "v norm change: " << 10*logl((long double)v.norm()/(long double)vn) << " dB" << std::endl;
+    return 0;
+}
+

src/examples/test_wolf_factories.cpp

@@ -15,6 +15,7 @@
 #include "../processor_odom_3D.h"
 #include "../processor_imu.h"
 #include "../processor_gps.h"
+#include "../processor_image.h"
 
 #include "../problem.h"
 
@@ -31,14 +32,6 @@ int main(void)
     using namespace std;
 
 
-    cout << "\n====== Registering creators in the Wolf Factories ======" << endl;
-
-    cout << "If you look above, you see the registered creators.\n"
-            "There is only one attempt per class, and it is successful!\n"
-            "We do this by registering in the class\'s .cpp file.\n"
-            "\n"
-            "See [wolf]/src/examples/test_sensor_factory.cpp for the way to add sensors and processors to wolf::Problem." << endl;
-
     /**=============================================================================================
      * Get wolf root directory from the environment variable WOLF_ROOT
      * To make this work, you need to set the environment variable WOLF_ROOT:
@@ -56,6 +49,15 @@ int main(void)
     std::cout << "\nwolf directory for configuration files: " << WOLF_CONFIG << std::endl;
     //==============================================================================================
 
+    cout << "\n====== Registering creators in the Wolf Factories =======" << endl;
+
+    cout << "If you look above, you see the registered creators.\n"
+            "There is only one attempt per class, and it is successful!\n"
+            "We do this by registering in the class\'s .cpp file.\n"
+            "\n"
+            "- See \'" << WOLF_ROOT << "/src/examples/test_wolf_factories.cpp\'\n"
+            "  for the way to install sensors and processors to wolf::Problem." << endl;
+
     // Start creating the problem
 
     Problem problem(FRM_PO_3D);
@@ -64,12 +66,16 @@ int main(void)
     Eigen::VectorXs pq_3d(7), po_2d(3), p_3d(3);
     IntrinsicsOdom2D intr_odom2d;
 
-    cout << "\n================= Intrinsics Factory ===================" << endl;
+    cout << "\n================== Intrinsics Factory ===================" << endl;
 
     // Use params factory for camera intrinsics
     IntrinsicsBase* intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", WOLF_CONFIG + "/camera.yaml");
+    ProcessorParamsBase* params_ptr = ProcessorParamsFactory::get().create("IMAGE", WOLF_CONFIG + "/processor_image_ORB.yaml");
+
+    cout << "CAMERA with intrinsics      : " << ((IntrinsicsCamera*)intr_cam_ptr)->pinhole_model.transpose() << endl;
+    cout << "Processor IMAGE image width : " << ((ProcessorImageParameters*)params_ptr)->image.width << endl;
 
-    cout << "\n==================== Sensor Factory ====================" << endl;
+    cout << "\n==================== Install Sensors ====================" << endl;
 
     // Install sensors
     problem.installSensor("CAMERA",     "front left camera",    pq_3d,  intr_cam_ptr);
@@ -79,9 +85,7 @@ int main(void)
     problem.installSensor("IMU",        "inertial",             pq_3d);
     problem.installSensor("GPS",        "GPS raw",              p_3d);
     problem.installSensor("ODOM 2D",    "aux odometer",         po_2d,  &intr_odom2d);
-
-    // Full YAML support: Add this sensor and recover a pointer to it
-    SensorBase* sen_ptr = problem.installSensor("CAMERA", "rear camera", pq_3d, WOLF_CONFIG + "/camera.yaml");
+    problem.installSensor("CAMERA", "rear camera", pq_3d, WOLF_ROOT + "/src/examples/camera.yaml");
 
     // print available sensors
     for (auto sen : *(problem.getHardwarePtr()->getSensorListPtr())){
@@ -90,9 +94,8 @@ int main(void)
                 << ": " << setw(8) << sen->getType()
                 << " | name: " << sen->getName() << endl;
     }
-    cout << "\twith intrinsics: " << sen_ptr->getIntrinsicPtr()->getVector().transpose() << endl;
 
-    cout << "\n=================== Processor Factory ===================" << endl;
+    cout << "\n=================== Install Processors ===================" << endl;
 
     // Install processors and bind them to sensors -- by sensor name!
     problem.installProcessor("ODOM 2D", "main odometry",    "main odometer");

src/processor_odom_3D.h

@@ -156,9 +156,7 @@ inline void ProcessorOdom3D::deltaMinusDelta(const Eigen::VectorXs& _delta1, con
 
 inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
 {
-    Eigen::VectorXs delta_zero(7);
-    delta_zero << 0, 0, 0, 0, 0, 0, 1;;
-    return delta_zero;
+    return (Eigen::VectorXs(7) << 0,0,0, 0,0,0,1).finished(); // p, q
 }
 
 inline Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _motion, TimeStamp& _ts)

src/sensor_base.h

@@ -58,9 +58,9 @@ class SensorBase : public NodeLinked<HardwareBase, ProcessorBase>
          * \param _tp Type of the sensor  (types defined at wolf.h)
          * \param _p_ptr StateBlock pointer to the sensor position
          * \param _o_ptr StateBlock pointer to the sensor orientation
-         * \param _intr_ptr TODO update documentation
-         * \param _noise_size TODO update documentation
-         * \param _extr_dyn TODO update documentation
+         * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
+         * \param _noise_size dimension of the noise term
+         * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
          *
          **/
         SensorBase(const SensorType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr, StateBlock* _intr_ptr, const unsigned int _noise_size, const bool _extr_dyn = false);
@@ -71,9 +71,9 @@ class SensorBase : public NodeLinked<HardwareBase, ProcessorBase>
          * \param _tp Type of the sensor  (types defined at wolf.h)
          * \param _p_ptr StateBlock pointer to the sensor position
          * \param _o_ptr StateBlock pointer to the sensor orientation
-         * \param _intr_ptr TODO update documentation
-         * \param _noise_std TODO update documentation
-         * \param _extr_dyn TODO update documentation
+         * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
+         * \param _noise_std standard deviations of the noise term
+         * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
          *
          **/
         SensorBase(const SensorType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr, StateBlock* _intr_ptr, const Eigen::VectorXs & _noise_std, const bool _extr_dyn = false);

src/sensor_factory.h

@@ -44,12 +44,12 @@ namespace wolf
  * Creators must be registered to the factory before they can be invoked for sensor creation.
  *
  * This documentation shows you how to:
- *   - Access the Factory
+ *   - Access the factory
  *   - Register and unregister creators
  *   - Create sensors
  *   - Write a sensor creator for SensorCamera (example).
  *
- * #### Accessing the Factory
+ * #### Accessing the factory
  * The SensorFactory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
  * To obtain an instance of it, use the static method get(),
  *