diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 7c7a623a5c89b351b26641a8735ecac9c62509bd..861dcf413618d6b16ad6029eea721b404f5ecd33 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,3 +1,5 @@
+# Matrix product test
+#ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
# ProcessorMotion classes test
ADD_EXECUTABLE(test_motion test_motion.cpp)
diff --git a/src/examples/test_matrix_prod.cpp b/src/examples/test_matrix_prod.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd6ffddb6585f80c37b0d3a2e028492311ee2937
--- /dev/null
+++ b/src/examples/test_matrix_prod.cpp
@@ -0,0 +1,156 @@
+/**
+ * \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;
+}
+
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 95631ee150b9b84994a65378ee862b3b65382fdb..193ba291790f133b32c3d6febe2ab8d27332fbbe 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/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");
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 87f962096afc80bfab44cf309de4e9a9b320a04b..e4b32dd56846702b568efb52ee3032112338e028 100644
--- a/src/processor_odom_3D.h
+++ b/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)
diff --git a/src/sensor_base.h b/src/sensor_base.h
index ba8cf1530644da2b502577062ee02b788a318a5f..0f6fcdf6c855b95ea07328c0cc37aa2648b323c1 100644
--- a/src/sensor_base.h
+++ b/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);
diff --git a/src/sensor_factory.h b/src/sensor_factory.h
index ba4462a50a41229dad311020ca4f3be74b2136fb..71ebf9575b7b089327ac4205019dc8b1d27a89ab 100644
--- a/src/sensor_factory.h
+++ b/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(),
*