Organize gps library and files

CMakeLists.txt

@@ -6,6 +6,9 @@ if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW) 
   cmake_policy(SET CMP0003 NEW)
 endif(COMMAND cmake_policy)
+# MAC OSX RPATH
+SET(CMAKE_MACOSX_RPATH 1)
+
 
 # The project name and the type of project
 PROJECT(wolf)
@@ -40,9 +43,6 @@ endif()
 
 ADD_SUBDIRECTORY(src)
 
-# MAC OSX RPATH
-SET(CMAKE_MACOSX_RPATH 1)
-
 FIND_PACKAGE(Doxygen)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)

doc/doxygen.conf

@@ -2313,7 +2313,7 @@ PLANTUML_JAR_PATH      =
 # Minimum value: 0, maximum value: 10000, default value: 50.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
-DOT_GRAPH_MAX_NODES    = 25
+DOT_GRAPH_MAX_NODES    = 30
 
 # The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
 # generated by dot. A depth value of 3 means that only nodes reachable from the
@@ -2325,7 +2325,7 @@ DOT_GRAPH_MAX_NODES    = 25
 # Minimum value: 0, maximum value: 1000, default value: 0.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
-MAX_DOT_GRAPH_DEPTH    = 2
+MAX_DOT_GRAPH_DEPTH    = 5
 
 # Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
 # background. This is disabled by default, because dot on Windows does not seem

src/CMakeLists.txt

@@ -4,7 +4,8 @@ MESSAGE("Starting WOLF CMakeLists ...")
 CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 
 #CMAKE modules
-SET(CMAKE_MODULE_PATH ../cmake_modules)
+SET(CMAKE_MODULE_PATH ${CMAKE_ROOT}/Modules/; ../cmake_modules)
+MESSAGE(STATUS ${CMAKE_MODULE_PATH})
 
 #find dependencies. 
 FIND_PACKAGE(Ceres QUIET) #Ceres is not required
@@ -24,7 +25,7 @@ IF(laser_scan_utils_FOUND)
     MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
 ENDIF(laser_scan_utils_FOUND)
 
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
+FIND_PACKAGE(raw_gps_utils ) #raw_gps_utils is not required
 IF(raw_gps_utils_FOUND)
     MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
 ENDIF(raw_gps_utils_FOUND)
@@ -40,8 +41,8 @@ SET(YAMLCPP_USE_STATIC_LIBS TRUE) #Use static library : not working: TO BE REVIS
 FIND_PACKAGE(YamlCpp)
 IF(YAMLCPP_FOUND)
     MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-    MESSAGE("-- YamlCpp includes: " ${YAMLCPP_INCLUDE_DIR})
-    MESSAGE("-- YamlCpp library: " ${YAMLCPP_LIBRARY})
+    MESSAGE(STATUS ${YAMLCPP_INCLUDE_DIR})
+    MESSAGE(STATUS ${YAMLCPP_LIBRARY})
 ELSEIF(YAMLCPP_FOUND)
     MESSAGE("yaml-cpp Library NOT FOUND!")
 ENDIF(YAMLCPP_FOUND)
@@ -105,10 +106,10 @@ SET(HDRS
     constraint_analytic.h
     constraint_epipolar.h
     constraint_sparse.h
+    constraint_fix.h
     constraint_gps_2D.h
     constraint_gps_pseudorange_3D.h
     constraint_gps_pseudorange_2D.h
-    constraint_fix.h
     constraint_odom_2D.h
     constraint_odom_2D_analytic.h
     constraint_corner_2D.h
@@ -149,7 +150,7 @@ SET(HDRS
     sensor_base.h
     sensor_camera.h
     sensor_factory.h
-#    sensor_gps.h      # What is this doing here?
+    sensor_gps.h
     sensor_gps_fix.h
     sensor_imu.h
     sensor_odom_2D.h
@@ -180,6 +181,7 @@ SET(SRCS
     feature_base.cpp
     feature_corner_2D.cpp
     feature_gps_fix.cpp
+    feature_gps_pseudorange.cpp
     feature_fix.cpp
     feature_odom_2D.cpp
     frame_base.cpp
@@ -204,7 +206,7 @@ SET(SRCS
     sensor_base.cpp
     sensor_camera.cpp
     sensor_factory.cpp
-#    sensor_gps.cpp   # What is this doing here?
+    sensor_gps.cpp
     sensor_gps_fix.cpp
     sensor_imu.cpp
     sensor_odom_2D.cpp
@@ -254,15 +256,11 @@ ENDIF(laser_scan_utils_FOUND)
 IF (raw_gps_utils_FOUND)
     SET(HDRS ${HDRS}
         capture_gps.h
-        feature_gps_pseudorange.h
         processor_gps.h
-        sensor_gps.h
         )
     SET(SRCS ${SRCS}
         capture_gps.cpp
-        feature_gps_pseudorange.cpp
         processor_gps.cpp
-        sensor_gps.cpp
         )
 ENDIF(raw_gps_utils_FOUND)
 

src/capture_gps.h

@@ -3,7 +3,7 @@
 
 
 // Wolf includes
-#include "raw_gps_utils/satellites_obs.h"
+#include "iri-algorithms/raw_gps_utils/satellites_obs.h"
 #include "capture_base.h"
 
 namespace wolf {

src/examples/test_sensor_factory.cpp

@@ -18,6 +18,9 @@
 #include "../sensor_imu.h"
 #include "../processor_imu.h"
 
+#include "../sensor_gps.h"
+//#include "../processor_gps.h"
+
 #include "../problem.h"
 
 #include <iostream>

src/processor_gps.cpp

@@ -6,11 +6,11 @@
 #include "feature_gps_pseudorange.h"
 #include "constraint_gps_pseudorange_2D.h"
 
+namespace wolf
+{
 
-namespace wolf {
-
-ProcessorGPS::ProcessorGPS() : ProcessorBase(PRC_GPS_RAW),
-        //sensor_gps_ptr_((SensorGPS*)(upperNodePtr())), //TODO here there's a crash. Look at what they'll do in processorLaser and modify as consequence
+ProcessorGPS::ProcessorGPS() :
+        ProcessorBase(PRC_GPS_RAW), //sensor_gps_ptr_((SensorGPS*)(upperNodePtr())), //TODO here there's a crash. Look at what they'll do in processorLaser and modify as consequence
         capture_gps_ptr_(nullptr)
 {
     setType("GPS");
@@ -19,46 +19,50 @@ ProcessorGPS::ProcessorGPS() : ProcessorBase(PRC_GPS_RAW),
 
 ProcessorGPS::~ProcessorGPS()
 {
+}
 
+void ProcessorGPS::init(CaptureBase* _capture_ptr)
+{
 }
 
-/*
- * Extract feature from the capture and create constraint
- */
 void ProcessorGPS::process(CaptureBase* _capture_ptr)
 {
     std::cout << "ProcessorGPS::process(GPScapture)" << std::endl;
-
     //TODO add assert with dynamic_cast when it will be ready
-    capture_gps_ptr_ = (CaptureGPS*)(_capture_ptr);
-
+    capture_gps_ptr_ = (CaptureGPS*)((((_capture_ptr))));
     //std::cout << "Extracting gps features..." << std::endl;
     rawgpsutils::SatellitesObs obs = capture_gps_ptr_->getData();
-
-
-    for(unsigned int i = 0; i < obs.measurements_.size(); ++i)
+    for (unsigned int i = 0; i < obs.measurements_.size(); ++i)
     {
         Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_;
         Scalar pr = obs.measurements_[i].pseudorange_;
-
         capture_gps_ptr_->addFeature(new FeatureGPSPseudorange(sat_pos, pr, gps_covariance_));
     }
     //std::cout << "gps features extracted" << std::endl;
-
-
     //std::cout << "Establishing constraints to gps features..." << std::endl;
-    for(auto i_it = capture_gps_ptr_->getFeatureListPtr()->begin(); i_it != capture_gps_ptr_->getFeatureListPtr()->end(); i_it++)
+    for (auto i_it = capture_gps_ptr_->getFeatureListPtr()->begin();
+            i_it != capture_gps_ptr_->getFeatureListPtr()->end(); i_it++)
     {
-        capture_gps_ptr_->getFeatureListPtr()->front()->addConstraint( new ConstraintGPSPseudorange2D((*i_it)) );
+        capture_gps_ptr_->getFeatureListPtr()->front()->addConstraint(new ConstraintGPSPseudorange2D((*i_it)));
     }
     //std::cout << "Constraints established" << std::endl;
 }
 
-ProcessorBase* ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+bool ProcessorGPS::voteForKeyFrame()
+{
+    return false;
+}
+
+bool ProcessorGPS::keyFrameCallback(wolf::FrameBase*)
+{
+    return false;
+}
+
+wolf::ProcessorBase* ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
 {
     ProcessorGPS* prc_ptr = new ProcessorGPS();
     prc_ptr->setName(_unique_name);
     return prc_ptr;
 }
 
-} // namespace wolf
+}

src/processor_gps.h

@@ -5,9 +5,11 @@
 #ifndef WOLF_PROCESSOR_GPS_H
 #define WOLF_PROCESSOR_GPS_H
 
+class CaptureGPS;
+
 // Wolf includes
 #include "processor_base.h"
-#include "capture_gps.h"
+//#include "capture_gps.h"
 
 // Std includes
 
@@ -27,24 +29,16 @@ class ProcessorGPS : public ProcessorBase
     public:
         ProcessorGPS();
         virtual ~ProcessorGPS();
-
         virtual void init(CaptureBase* _capture_ptr);
-
         virtual void process(CaptureBase* _capture_ptr);
-
-        virtual bool voteForKeyFrame(){return false;}
-
-        virtual bool keyFrameCallback(wolf::FrameBase*){return false;}
+        virtual bool voteForKeyFrame();
+        virtual bool keyFrameCallback(wolf::FrameBase*);
 
     public:
-        static ProcessorBase* create(const std::string & _unique_name, const ProcessorParamsBase* _params);
+        static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
 
 };
 
-inline void ProcessorGPS::init(CaptureBase* _capture_ptr)
-{
-}
-
 } // namespace wolf
 
 

src/processor_motion.h

@@ -9,7 +9,6 @@
 #define PROCESSOR_MOTION_H_
 
 // Wolf
-#include "processor_motion.h"
 #include "capture_motion2.h"
 #include "motion_buffer.h"
 #include "time_stamp.h"

src/solver/qr_solver.h

@@ -16,8 +16,8 @@
 #include "state_block.h"
 #include "../constraint_sparse.h"
 #include "../constraint_fix.h"
-#include "../constraint_gps_2D.h"
-#include "../constraint_gps_pseudorange_2D.h"
+//#include "../constraint_gps_2D.h"
+//#include "../constraint_gps_pseudorange_2D.h"
 #include "../constraint_odom_2D.h"
 #include "../constraint_corner_2D.h"
 #include "../constraint_container.h"