Skip to content
Snippets Groups Projects
Commit 0b7fbd39 authored by acoromin's avatar acoromin
Browse files

ceres branch moved to trunk

parent 75c1d979
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 1230 deletions
CMakeLists.txt deleted 100644 → 0
+ 0
− 101
View file @ 75c1d979
# Pre-requisites about cmake itself
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# The project name and the type of project
PROJECT(wolf)
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
ENDIF (NOT CMAKE_BUILD_TYPE)
message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
#Set Flags
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
#Set compiler according C++11 support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
ADD_SUBDIRECTORY(src)
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
ELSE (IRI_DOC_DIR)
ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
ENDIF (IRI_DOC_DIR)
ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
IF (UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "distribution clean"
COMMAND make ARGS clean
COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
TARGET distclean
)
ELSE(UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "distclean only implemented in unix"
TARGET distclean
)
ENDIF(UNIX)
ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
IF (UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "uninstall package"
COMMAND xargs ARGS rm < install_manifest.txt
TARGET uninstall
)
ELSE(UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "uninstall only implemented in unix"
TARGET uninstall
)
ENDIF(UNIX)
IF (UNIX)
SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
SET(CPACK_GENERATOR "DEB")
SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths
INCLUDE(CPack)
ELSE(UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "packaging only implemented in unix"
TARGET uninstall
)
ENDIF(UNIX)
Findwolf.cmake deleted 100644 → 0
+ 0
− 26
View file @ 75c1d979
#edit the following line to add the librarie's header files
FIND_PATH(
wolf_INCLUDE_DIR
NAMES wolf.h
PATHS /usr/local/include/iri-algorithms
)
FIND_LIBRARY(
wolf_LIBRARY
NAMES wolf
PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms)
IF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
SET(wolf_FOUND TRUE)
ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
IF (wolf_FOUND)
IF (NOT wolf_FIND_QUIETLY)
MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}")
ENDIF (NOT wolf_FIND_QUIETLY)
ELSE (wolf_FOUND)
IF (wolf_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find wolf")
ENDIF (wolf_FIND_REQUIRED)
ENDIF (wolf_FOUND)
README.txt deleted 100644 → 0
+ 0
− 6
View file @ 75c1d979
Wolf means Windowed Localization Frames !
Tips:
1. Use with ECLIPSE: If you use eclipse, cd to wolf/, then > source src/make_eclipse_project.bash, before importing the project.
Wolf_ideas.txt deleted 100644 → 0
+ 0
− 96
View file @ 75c1d979
This file serves as a guideline for future developments.
As the ideas here get implemented, we advise you add a /////// OK tag indicating the work is done and working.
1. Getters ///////////// OK
============
source return
vvv >>>
shptr ptr obj map
shptr & .get() * /
ptr / . * /
obj / / & /
map / / / & state: norm; error: const
2. Classes
===============
IDEA: Allow Vehicles, Frames, Captures, Features to live out of the node for enhanced reusability.
In the WOLF nodes, we can have like this:
class FrameBase
{
State* state_ptr;
}
class NodeConstrainer
{
virtual computeError()
}
class NodeFrameBase : public FrameBase, public NodeConstrainer
{
computeError() <-- in case we want to overload
}
3. Precomputing constant values ////////////// OK
===============================
PinHole::precomputeConstants() // call in in constructor /////////// OK
{
intrinsic_matrix_ = bla bla bla;
inverse_intrinsic_matrix_ = bla bla bla
}
Capture::precomputeGlobalPose() // call from Capture::precomputeConstants(), see below //////////// OK
{
global_pose_ = bla bla bla
inverse_global_pose_ = bla bla bla
}
Capture::precomputeConstants() ////////////////// OK
{
precomputeGlobalPose();
for (trans_sensor_iter = bla bla)
{
trans_sensor_iter->precomputeConstants();
}
}
TransSensor::precomputeConstants() = 0; /////////////// OK
TransPinHole::precomputeConstants() /////////////// OK
{
capture_own->globalPose()
capture_other->inverseGlobalPose()
computePoseRelative()
sensor_own->inverseIntrinsicMatrix()
sensor_other->intrinsicMatrix()
fundamental_ = bla bla
}
NodeConstrainer::precomputeConstants() ///////////////// OK
{
for (iter = bla bla bla)
{
*iter->precomputeConstants();
}
}
TOP::computeError() //////////////////// OK
{
precomputeConstants();
for (iter = bla bla bla)
{
*iter->compputeError();
}
}
CorrespondenceBase::precomputeConstants() //////////////// OK not overloaded.
{
// Leave it blank or simply do not overload it. Nobody will call it anyway.
}
src/CMakeLists.txt deleted 100644 → 0
+ 0
− 106
View file @ 75c1d979
#Start WOLF build
MESSAGE("Starting WOLF CMakeLists ...")
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
#find dependencies.
FIND_PACKAGE(Ceres QUIET) #Ceres is not required
IF(Ceres_FOUND)
MESSAGE("Ceres Library FOUND: ceres_wrapper will be built.")
ENDIF(Ceres_FOUND)
#include directories
INCLUDE_DIRECTORIES(.)
IF(Ceres_FOUND)
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
ENDIF(Ceres_FOUND)
#headers
SET(HDRS
capture_base.h
capture_relative.h
capture_gps_fix.h
capture_laser_2D.h
capture_odom_2D.h
correspondence_base.h
correspondence_sparse.h
correspondence_gps_2D.h
correspondence_odom_2D_theta.h
correspondence_odom_2D_complex_angle.h
feature_base.h
feature_gps_fix.h
feature_odom_2D.h
frame_base.h
landmark_base.h
map_base.h
node_base.h
node_terminus.h
node_linked.h
sensor_base.h
sensor_laser_2D.h
sensor_odom_2D.h
sensor_gps_fix.h
state_base.h
state_point.h
state_complex_angle.h
time_stamp.h
trajectory_base.h
wolf.h)
#sources
SET(SRCS
capture_base.cpp
capture_relative.cpp
capture_gps_fix.cpp
capture_laser_2D.cpp
capture_odom_2D.cpp
correspondence_base.cpp
feature_base.cpp
feature_gps_fix.cpp
feature_odom_2D.cpp
frame_base.cpp
landmark_base.cpp
map_base.cpp
node_base.cpp
node_terminus.cpp
sensor_base.cpp
sensor_laser_2D.cpp
sensor_odom_2D.cpp
sensor_gps_fix.cpp
state_base.cpp
state_complex_angle.cpp
time_stamp.cpp
trajectory_base.cpp)
#optional HDRS and SRCS
IF (Ceres_FOUND)
SET(SRCS ${SRCS} ceres_wrapper/complex_angle_parametrization.cpp)
SET(HDRS ${HDRS} ceres_wrapper/complex_angle_parametrization.h)
SET(SRCS ${SRCS} ceres_wrapper/ceres_manager.cpp)
SET(HDRS ${HDRS} ceres_wrapper/ceres_manager.h)
ENDIF(Ceres_FOUND)
# create the shared library
ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
#Link the created library with ceres
IF (Ceres_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
ENDIF(Ceres_FOUND)
#install library
INSTALL(TARGETS ${PROJECT_NAME}
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iri-algorithms
ARCHIVE DESTINATION lib/iri-algorithms)
#install headers
INSTALL(FILES ${HDRS}
DESTINATION include/iri-algorithms)
INSTALL(FILES ../Findwolf.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
#Build examples & tests
MESSAGE("Building examples and tests.")
ADD_SUBDIRECTORY(examples)
src/capture_base.cpp deleted 100644 → 0
+ 0
− 128
View file @ 75c1d979
#include "capture_base.h"
CaptureBase::CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
NodeLinked(MID, "CAPTURE"),
time_stamp_(_ts),
sensor_ptr_(_sensor_ptr)
{
//
}
CaptureBase::CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
NodeLinked(MID, "CAPTURE"),
time_stamp_(_ts),
sensor_ptr_(_sensor_ptr),
data_(_data)
{
//
}
CaptureBase::CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
NodeLinked(MID, "CAPTURE"),
time_stamp_(_ts),
sensor_ptr_(_sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
{
//
}
CaptureBase::~CaptureBase()
{
//std::cout << "Destroying capture...\n";
}
void CaptureBase::linkToFrame(const FrameBaseShPtr& _frm_ptr)
{
linkToUpperNode(_frm_ptr.get());
}
// TODO: Why the linker throws an error when this function is inline...
void CaptureBase::addFeature(FeatureBaseShPtr & _ft_ptr)
{
addDownNode(_ft_ptr);
}
const FrameBasePtr CaptureBase::getFramePtr() const
{
return upperNodePtr();
}
FeatureBaseList* CaptureBase::getFeatureListPtr()
{
return getDownNodeListPtr();
}
void CaptureBase::getCorrespondenceList(CorrespondenceBaseList & _corr_list)
{
std::list<FeatureBaseShPtr>::iterator f_it;
std::list<CorrespondenceBaseShPtr>* c_list_ptr;
std::list<CorrespondenceBaseShPtr>::iterator c_it;
for( f_it = down_node_list_.begin(); f_it != down_node_list_.end(); ++f_it)
{
c_list_ptr = (f_it->get())->getCorrespondenceListPtr();
for( c_it = c_list_ptr->begin(); c_it != c_list_ptr->end(); ++c_it)
{
_corr_list.push_back(*c_it);
}
}
}
TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
}
SensorBasePtr CaptureBase::getSensorPtr() const
{
return sensor_ptr_;
}
SensorType CaptureBase::getSensorType() const
{
return sensor_ptr_->getSensorType();
}
void CaptureBase::setTimeStamp(const TimeStamp & _ts)
{
time_stamp_ = _ts;
}
void CaptureBase::setTimeStampToNow()
{
time_stamp_.setToNow();
}
void CaptureBase::setData(unsigned int _size, const WolfScalar *_data)
{
data_.resize(_size);
for (unsigned int ii=0; ii<_size; ii++) data_(ii) = *(&_data[ii]);
}
void CaptureBase::setData(const Eigen::VectorXs& _data)
{
data_=_data;
}
void CaptureBase::setDataCovariance(const Eigen::MatrixXs& _data_cov)
{
data_covariance_ = _data_cov;
}
void CaptureBase::processCapture()
{
std::cout << "CaptureBase::processCapture()... processing capture" << std::endl;
}
void CaptureBase::printSelf(unsigned int _ntabs, std::ostream & _ost) const
{
NodeLinked::printSelf(_ntabs, _ost);
//printTabs(_ntabs);
//_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
//printNTabs(_ntabs);
//_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
}
src/capture_base.h deleted 100644 → 0
+ 0
− 106
View file @ 75c1d979
#ifndef CAPTURE_BASE_H_
#define CAPTURE_BASE_H_
// Forward declarations for node templates
class FrameBase;
class FeatureBase;
//std includes
//
//Wolf includes
#include "wolf.h"
#include "time_stamp.h"
#include "node_linked.h"
#include "frame_base.h"
#include "feature_base.h"
#include "sensor_base.h"
//class CaptureBase
class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
{
protected:
TimeStamp time_stamp_; ///< Time stamp
SensorBasePtr sensor_ptr_; ///< Pointer to sensor
Eigen::VectorXs data_; ///< raw data
Eigen::MatrixXs data_covariance_; ///< Noise of the capture
Eigen::Vector3s sensor_pose_global_; ///< Sensor pose in world frame: composition of the frame pose and the sensor pose. TODO: use state units
Eigen::Vector3s inverse_sensor_pose_; ///< World pose in the sensor frame: inverse of the global_pose_. TODO: use state units
public:
CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);//TODO: to be removed ??
CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureBase();
/** \brief Set link to Frame
*
* Set link to Frame
*
**/
void linkToFrame(const FrameBaseShPtr& _frm_ptr);
/** \brief Adds a Feature to the down node list
*
* Adds a Feature to the down node list
*
**/
void addFeature(FeatureBaseShPtr & _ft_ptr);
/** \brief Gets up_node_ptr_
*
* Gets up_node_ptr_, which is a pointer to the Frame owning of this Capture
*
**/
const FrameBasePtr getFramePtr() const;
/** \brief Gets a pointer to feature list
*
* Gets a pointer to feature list
*
**/
FeatureBaseList* getFeatureListPtr();
/** \brief Fills the provided list with all correspondences related to this capture
*
* Fills the provided list with all correspondences related to this capture
*
*
**/
//const CorrespondenceBaseList getCorrespondenceList() const;
void getCorrespondenceList(CorrespondenceBaseList & _corr_list);//TODO: Should be const
TimeStamp getTimeStamp() const;
SensorBasePtr getSensorPtr() const;
SensorType getSensorType() const;
void setTimeStamp(const TimeStamp & _ts);
void setTimeStampToNow();
void setData(unsigned int _size, const WolfScalar *_data);
void setData(const Eigen::VectorXs& _data);
void setDataCovariance(const Eigen::MatrixXs& _data_cov);
virtual void processCapture();// = 0;
virtual Eigen::VectorXs computePrior() const = 0;
/** \brief Generic interface to find correspondences
*
* Generic interface to find correspondences between its features and a map (static/slam) or a previous feature
*
**/
virtual void findCorrespondences() = 0;
virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
#endif
src/capture_gps_fix.cpp deleted 100644 → 0
+ 0
− 53
View file @ 75c1d979
#include "capture_gps_fix.h"
CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
CaptureBase(_ts, _sensor_ptr)
{
//
}
CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
CaptureBase(_ts, _sensor_ptr, _data)
{
//
}
CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
CaptureBase(_ts, _sensor_ptr, _data, _data_covariance)
{
//
}
CaptureGPSFix::~CaptureGPSFix()
{
//std::cout << "Destroying GPS fix capture...\n";
}
void CaptureGPSFix::processCapture()
{
FeatureBaseShPtr new_feature = FeatureBaseShPtr(new FeatureGPSFix(CaptureBasePtr(this),data_,data_covariance_));
addFeature(new_feature);
}
Eigen::VectorXs CaptureGPSFix::computePrior() const
{
return data_;
}
void CaptureGPSFix::findCorrespondences()
{
CorrespondenceBaseShPtr gps_correspondence(new CorrespondenceGPS2D(getFeatureListPtr()->front().get(), getFramePtr()->getPPtr()));
getFeatureListPtr()->front()->addCorrespondence(gps_correspondence);
}
//void CaptureGPSFix::printSelf(unsigned int _ntabs, std::ostream & _ost) const
//{
// NodeLinked::printSelf(_ntabs, _ost);
// //printTabs(_ntabs);
// //_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
// //printNTabs(_ntabs);
// //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
//}
src/capture_gps_fix.h deleted 100644 → 0
+ 0
− 32
View file @ 75c1d979
#ifndef CAPTURE_GPS_FIX_H_
#define CAPTURE_GPS_FIX_H_
//std includes
//
//Wolf includes
#include "capture_base.h"
#include "feature_gps_fix.h"
//class CaptureGPSFix
class CaptureGPSFix : public CaptureBase
{
public:
CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
CaptureGPSFix(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureGPSFix();
virtual void processCapture();
virtual Eigen::VectorXs computePrior() const;
virtual void findCorrespondences();
//virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
#endif
src/capture_laser_2D.cpp deleted 100644 → 0
+ 0
− 22
View file @ 75c1d979
#include "capture_laser_2D.h"
CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, const SensorLaser2DPtr & _sensor_ptr, const Eigen::VectorXs& _ranges):
CaptureBase(_ts, _sensor_ptr, _ranges)
{
//
}
CaptureLaser2D::~CaptureLaser2D()
{
//
}
void CaptureLaser2D::processCapture()
{
extractCorners();
}
void CaptureLaser2D::extractCorners()
{
}
src/capture_laser_2D.h deleted 100644 → 0
+ 0
− 43
View file @ 75c1d979
#ifndef CAPTURE_LASER_2D_H_
#define CAPTURE_LASER_2D_H_
//wolf includes
#include "capture_base.h"
#include "sensor_laser_2D.h"
//wolf forward declarations
//#include "feature_corner_2D.h"
class CaptureLaser2D : public CaptureBase
{
public:
/** \brief Constructor
*
* Constructor
*
**/
CaptureLaser2D(const TimeStamp & _ts, const SensorLaser2DPtr & _sensor_ptr, const Eigen::VectorXs& _ranges);
/** \brief Destructor
*
* Destructor
*
**/
virtual ~CaptureLaser2D();
/** \brief Calls the necessary pipeline from raw scan to features.
*
* Calls the necessary pipeline from raw scan to features.
*
**/
virtual void processCapture();
/** \brief Extract corners and push-back to Feature down list
*
* Extract corners and push-back to Feature down list .
*
**/
virtual void extractCorners();
};
#endif /* CAPTURE_LASER_2D_H_ */
src/capture_odom_2D.cpp deleted 100644 → 0
+ 0
− 97
View file @ 75c1d979
#include "capture_odom_2D.h"
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
CaptureRelative(_ts, _sensor_ptr)
{
//
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
CaptureRelative(_ts, _sensor_ptr, _data)
{
//
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
CaptureRelative(_ts, _sensor_ptr, _data, _data_covariance)
{
//
}
CaptureOdom2D::~CaptureOdom2D()
{
//std::cout << "Destroying GPS fix capture...\n";
}
inline void CaptureOdom2D::processCapture()
{
FeatureBaseShPtr new_feature(new FeatureOdom2D(CaptureBasePtr(this),data_,data_covariance_));
addFeature(new_feature);
}
Eigen::VectorXs CaptureOdom2D::computePrior() const
{
assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
FrameBasePtr _previous_frame = getFramePtr()->getPreviousFrame();
if (_previous_frame->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
{
Eigen::VectorXs pose_predicted(4);
Eigen::Map<Eigen::VectorXs> previous_pose(_previous_frame->getPPtr()->getPtr(), 4);
double new_pose_predicted_2 = previous_pose(2) * cos(data_(1)) - previous_pose(3) * sin(data_(1));
double new_pose_predicted_3 = previous_pose(2) * sin(data_(1)) + previous_pose(3) * cos(data_(1));
pose_predicted(0) = previous_pose(0) + data_(0) * new_pose_predicted_2;
pose_predicted(1) = previous_pose(1) + data_(0) * new_pose_predicted_3;
pose_predicted(2) = new_pose_predicted_2;
pose_predicted(3) = new_pose_predicted_3;
return pose_predicted;
}
else
{
Eigen::VectorXs pose_predicted(3);
Eigen::Map<Eigen::VectorXs> previous_pose(_previous_frame->getPPtr()->getPtr(), 3);
pose_predicted(0) = previous_pose(0) + data_(0) * cos(previous_pose(2) + (data_(1)));
pose_predicted(1) = previous_pose(1) + data_(0) * sin(previous_pose(2) + (data_(1)));
pose_predicted(2) = previous_pose(2) + data_(1);
return pose_predicted;
}
}
void CaptureOdom2D::findCorrespondences()
{
if (getFramePtr()->getOPtr()->getStateType() == ST_THETA)
{
CorrespondenceBaseShPtr odom_correspondence(new CorrespondenceOdom2DTheta(getFeatureListPtr()->front().get(),
getFramePtr()->getPreviousFrame()->getPPtr(),
getFramePtr()->getPreviousFrame()->getOPtr(),
getFramePtr()->getPPtr(),
getFramePtr()->getOPtr()));
getFeatureListPtr()->front()->addCorrespondence(odom_correspondence);
}
else
{
CorrespondenceBaseShPtr odom_correspondence(new CorrespondenceOdom2DComplexAngle(getFeatureListPtr()->front().get(),
getFramePtr()->getPreviousFrame()->getPPtr(),
getFramePtr()->getPreviousFrame()->getOPtr(),
getFramePtr()->getPPtr(),
getFramePtr()->getOPtr()));
getFeatureListPtr()->front()->addCorrespondence(odom_correspondence);
}
}
//void CaptureOdom2D::printSelf(unsigned int _ntabs, std::ostream & _ost) const
//{
// NodeLinked::printSelf(_ntabs, _ost);
// //printTabs(_ntabs);
// //_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
// //printNTabs(_ntabs);
// //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
//}
src/capture_odom_2D.h deleted 100644 → 0
+ 0
− 32
View file @ 75c1d979
#ifndef CAPTURE_ODOM_2D_H_
#define CAPTURE_ODOM_2D_H_
//std includes
//
//Wolf includes
#include "capture_relative.h"
#include "feature_odom_2D.h"
//class CaptureGPSFix
class CaptureOdom2D : public CaptureRelative
{
public:
CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
CaptureOdom2D(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureOdom2D();
virtual void processCapture();
virtual Eigen::VectorXs computePrior() const;
virtual void findCorrespondences();
//virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
#endif
src/capture_relative.cpp deleted 100644 → 0
+ 0
− 23
View file @ 75c1d979
#include "capture_relative.h"
CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr) :
CaptureBase(_ts, _sensor_ptr)
{
//
}
CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data) :
CaptureBase(_ts, _sensor_ptr, _data)
{
//
}
CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
CaptureBase(_ts, _sensor_ptr, _data, _data_covariance)
{
//
}
CaptureRelative::~CaptureRelative()
{
//
}
src/capture_relative.h deleted 100644 → 0
+ 0
− 24
View file @ 75c1d979
#ifndef CAPTURE_RELATIVE_H_
#define CAPTURE_RELATIVE_H_
//std includes
//
//Wolf includes
#include "wolf.h"
#include "capture_base.h"
//class CaptureBase
class CaptureRelative : public CaptureBase
{
public:
CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureRelative();
};
#endif
src/ceres_wrapper/ceres_manager.cpp deleted 100644 → 0
+ 0
− 177
View file @ 75c1d979
#include "ceres_manager.h"
CeresManager::CeresManager(ceres::Problem* _ceres_problem) :
ceres_problem_(_ceres_problem)
{
}
CeresManager::~CeresManager()
{
// std::vector<double*> state_units;
// ceres_problem_->GetParameterBlocks(&state_units);
//
// for (uint i = 0; i< state_units.size(); i++)
// removeStateUnit(state_units.at(i));
//
// std::cout << "all state units removed! \n";
// std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n";
// std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << "\n";
}
ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_options)
{
// create summary
ceres::Solver::Summary ceres_summary_;
// run Ceres Solver
ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
//display results
return ceres_summary_;
}
void CeresManager::addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences)
{
//std::cout << _new_correspondences.size() << " new correspondences\n";
while (!_new_correspondences.empty())
{
addCorrespondence(_new_correspondences.front());
_new_correspondences.pop_front();
}
}
void CeresManager::removeCorrespondences()
{
for (uint i = 0; i<correspondence_list_.size(); i++)
{
ceres_problem_->RemoveResidualBlock(correspondence_list_.at(i).first);
}
correspondence_list_.clear();
}
void CeresManager::addCorrespondence(const CorrespondenceBasePtr& _corr_ptr)
{
ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>(blockIdx,_corr_ptr));
}
void CeresManager::addStateUnits(std::list<StateBasePtr>& _new_state_units)
{
while (!_new_state_units.empty())
{
addStateUnit(_new_state_units.front());
_new_state_units.pop_front();
}
}
void CeresManager::removeStateUnit(WolfScalar* _st_ptr)
{
ceres_problem_->RemoveParameterBlock(_st_ptr);
}
void CeresManager::addStateUnit(const StateBasePtr& _st_ptr)
{
//std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
//_st_ptr->print();
switch (_st_ptr->getStateType())
{
case ST_COMPLEX_ANGLE:
{
//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
//ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
break;
}
// case PARAM_QUATERNION:
// {
// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
// break;
// }
case ST_POINT_1D:
case ST_THETA:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_2D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
std::cout << "Unknown Local Parametrization type!" << std::endl;
}
}
ceres::CostFunction* CeresManager::createCostFunction(const CorrespondenceBasePtr& _corrPtr)
{
switch (_corrPtr->getCorrespondenceType())
{
case CORR_GPS_FIX_2D:
{
CorrespondenceGPS2D* specific_ptr = (CorrespondenceGPS2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<CorrespondenceGPS2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case CORR_ODOM_2D_COMPLEX_ANGLE:
{
CorrespondenceOdom2DComplexAngle* specific_ptr = (CorrespondenceOdom2DComplexAngle*)(_corrPtr);
return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DComplexAngle,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case CORR_ODOM_2D_THETA:
{
CorrespondenceOdom2DTheta* specific_ptr = (CorrespondenceOdom2DTheta*)(_corrPtr);
return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DTheta,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
default:
std::cout << "Unknown correspondence type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
return nullptr;
}
}
src/ceres_wrapper/ceres_manager.h deleted 100644 → 0
+ 0
− 54
View file @ 75c1d979
#ifndef CERES_MANAGER_H_
#define CERES_MANAGER_H_
//Ceres includes
#include "ceres/jet.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
//wof includes
#include "wolf.h"
#include "state_base.h"
#include "state_point.h"
#include "state_complex_angle.h"
#include "correspondence_sparse.h"
#include "correspondence_gps_2D.h"
#include "correspondence_odom_2D_theta.h"
#include "correspondence_odom_2D_complex_angle.h"
// ceres wrapper includes
#include "ceres_wrapper/complex_angle_parametrization.h"
/** \brief Ceres manager for WOLF
*
*/
class CeresManager
{
protected:
std::vector<std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>> correspondence_list_;
ceres::Problem* ceres_problem_;
public:
CeresManager(ceres::Problem* _ceres_problem);
~CeresManager();
ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
void addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences);
void removeCorrespondences();
void addCorrespondence(const CorrespondenceBasePtr& _corr_ptr);
void addStateUnits(std::list<StateBasePtr>& _new_state_units);
void removeStateUnit(WolfScalar* _st_ptr);
void addStateUnit(const StateBasePtr& _st_ptr);
ceres::CostFunction* createCostFunction(const CorrespondenceBasePtr& _corrPtr);
};
#endif
src/ceres_wrapper/complex_angle_parametrization.cpp deleted 100644 → 0
+ 0
− 45
View file @ 75c1d979
#include "complex_angle_parametrization.h"
//ComplexAngleParameterization::ComplexAngleParameterization()
//{
// //
//}
ComplexAngleParameterization::~ComplexAngleParameterization()
{
//
}
bool ComplexAngleParameterization::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
{
x_plus_delta_raw[0] = x_raw[0] * cos(delta_raw[0]) - x_raw[1] * sin(delta_raw[0]);
x_plus_delta_raw[1] = x_raw[1] * cos(delta_raw[0]) + x_raw[0] * sin(delta_raw[0]);
//normalize
//double norm = sqrt(x_plus_delta_raw[0] * x_plus_delta_raw[0] + x_plus_delta_raw[1] * x_plus_delta_raw[1]);
//std::cout << "(before normalization) norm = " << norm << std::endl;
//x_plus_delta_raw[0] /= norm;
//x_plus_delta_raw[1] /= norm;
return true;
}
bool ComplexAngleParameterization::ComputeJacobian(const double* x, double* jacobian) const
{
jacobian[0] = -x[1];
jacobian[1] = x[0];
return true;
}
int ComplexAngleParameterization::GlobalSize() const
{
return 2;
}
int ComplexAngleParameterization::LocalSize() const
{
return 1;
}
src/ceres_wrapper/complex_angle_parametrization.h deleted 100644 → 0
+ 0
− 24
View file @ 75c1d979
#ifndef COMPLEX_ANGLE_PARAMETRIZATION_H_
#define COMPLEX_ANGLE_PARAMETRIZATION_H_
//Ceres includes
#include "ceres/ceres.h"
class ComplexAngleParameterization : public ceres::LocalParameterization
{
public:
//ComplexAngleParameterization();
virtual ~ComplexAngleParameterization();
virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const;
virtual bool ComputeJacobian(const double* x, double* jacobian) const;
virtual int GlobalSize() const;
virtual int LocalSize() const;
};
#endif
src/correspondence_base.cpp deleted 100644 → 0
+ 0
− 35
View file @ 75c1d979
#include "correspondence_base.h"
CorrespondenceBase::CorrespondenceBase(const FeatureBasePtr& _ftr_ptr, CorrespondenceType _tp) :
NodeLinked(BOTTOM, "CORRESPONDENCE", _ftr_ptr),
type_(_tp),
measurement_ptr_(_ftr_ptr->getMeasurementPtr()),
measurement_covariance_ptr_(_ftr_ptr->getMeasurementCovariancePtr())
{
//
}
CorrespondenceBase::~CorrespondenceBase()
{
//
}
CorrespondenceType CorrespondenceBase::getCorrespondenceType() const
{
return type_;
}
const Eigen::VectorXs * CorrespondenceBase::getMeasurementPtr()
{
return upperNodePtr()->getMeasurementPtr();
}
FeatureBasePtr CorrespondenceBase::getFeaturePtr() const
{
return upperNodePtr();
}
CaptureBasePtr CorrespondenceBase::getCapturePtr() const
{
return upperNodePtr()->upperNodePtr();
}
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