diff --git a/CMakeLists.txt b/CMakeLists.txt index b7788e0..9914d8f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,6 @@ if (NOT USE_AMENT) set(PROJECT_VERSION 1.1.0) # Find dependencies -find_package(Boost 1.71.0 REQUIRED system) find_package(OpenMP REQUIRED) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") @@ -31,9 +30,7 @@ find_package(lgmath 1.1.0 REQUIRED) file(GLOB_RECURSE SOURCE_FILES "src/*.cpp") add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES}) target_link_libraries(${PROJECT_NAME} - PUBLIC - ${Boost_LIBRARIES} - lgmath + PUBLIC lgmath ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -95,7 +92,6 @@ else() # Find dependencies find_package(ament_cmake REQUIRED) -find_package(Boost REQUIRED system) find_package(OpenMP REQUIRED) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") @@ -106,21 +102,15 @@ find_package(lgmath REQUIRED) # Libraries file(GLOB_RECURSE SOURCE src/*.cpp) add_library(${PROJECT_NAME} ${SOURCE}) -ament_target_dependencies(${PROJECT_NAME} Boost Eigen3 OpenMP lgmath) +ament_target_dependencies(${PROJECT_NAME} Eigen3 OpenMP lgmath) target_include_directories(${PROJECT_NAME} PUBLIC $ $) - ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -# May need the following -ament_export_dependencies( - Boost - Eigen3 - OpenMP - lgmath -) +ament_export_dependencies(eigen3_cmake_module) +ament_export_dependencies(OpenMP Eigen3 lgmath) install( DIRECTORY include/ @@ -128,9 +118,7 @@ install( ) install( - TARGETS - # Libraries - ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -141,41 +129,20 @@ install( endif() # Executables +link_libraries(${PROJECT_NAME}) add_executable(SimpleBAandTrajPrior samples/SimpleBAandTrajPrior.cpp) -target_link_libraries(SimpleBAandTrajPrior ${PROJECT_NAME}) - add_executable(SimpleBAandCATrajPrior samples/SimpleBAandCATrajPrior.cpp) -target_link_libraries(SimpleBAandCATrajPrior ${PROJECT_NAME}) - add_executable(SimpleBundleAdjustment samples/SimpleBundleAdjustment.cpp) -target_link_libraries(SimpleBundleAdjustment ${PROJECT_NAME}) - add_executable(SimpleBundleAdjustmentFullRel samples/SimpleBundleAdjustmentFullRel.cpp) -target_link_libraries(SimpleBundleAdjustmentFullRel ${PROJECT_NAME}) - add_executable(SimpleBundleAdjustmentRelLand samples/SimpleBundleAdjustmentRelLand.cpp) -target_link_libraries(SimpleBundleAdjustmentRelLand ${PROJECT_NAME}) - add_executable(SimpleBundleAdjustmentRelLandX samples/SimpleBundleAdjustmentRelLandX.cpp) -target_link_libraries(SimpleBundleAdjustmentRelLandX ${PROJECT_NAME}) - add_executable(SimplePoseGraphRelax samples/SimplePoseGraphRelax.cpp) -target_link_libraries(SimplePoseGraphRelax ${PROJECT_NAME}) - add_executable(SimpleTrajectoryPrior samples/SimpleTrajectoryPrior.cpp) -target_link_libraries(SimpleTrajectoryPrior ${PROJECT_NAME}) - add_executable(SpherePoseGraphRelax samples/SpherePoseGraphRelax.cpp) -target_link_libraries(SpherePoseGraphRelax ${PROJECT_NAME}) - add_executable(TrustRegionExample samples/TrustRegionExample.cpp) -target_link_libraries(TrustRegionExample ${PROJECT_NAME}) - add_executable(MotionDistortedP2PandCATrajPrior samples/MotionDistortedP2PandCATrajPrior.cpp) -target_link_libraries(MotionDistortedP2PandCATrajPrior ${PROJECT_NAME}) - add_executable(SimpleP2PandCATrajPrior samples/SimpleP2PandCATrajPrior.cpp) -target_link_libraries(SimpleP2PandCATrajPrior ${PROJECT_NAME}) +add_executable(SimplePointCloudAlignment samples/SimplePointCloudAlignment.cpp) if (USE_AMENT) @@ -194,6 +161,7 @@ install( TrustRegionExample MotionDistortedP2PandCATrajPrior SimpleP2PandCATrajPrior + SimplePointCloudAlignment LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} diff --git a/Dockerfile b/Dockerfile index 773aa9b..7bb5db2 100644 --- a/Dockerfile +++ b/Dockerfile @@ -5,7 +5,5 @@ CMD ["/bin/bash"] ARG DEBIAN_FRONTEND=noninteractive RUN apt update - RUN apt install -q -y curl gnupg2 lsb-release build-essential cmake -RUN apt install -q -y libboost-all-dev libomp-dev -RUN apt install -q -y libeigen3-dev \ No newline at end of file +RUN apt install -q -y libeigen3-dev libomp-dev \ No newline at end of file diff --git a/Dockerfile.ROS2 b/Dockerfile.ROS2 index 4edbf2c..6749653 100644 --- a/Dockerfile.ROS2 +++ b/Dockerfile.ROS2 @@ -1,10 +1,10 @@ -FROM ros:foxy +FROM ros:galactic CMD ["/bin/bash"] ARG DEBIAN_FRONTEND=noninteractive RUN apt update -RUN apt install -q -y curl gnupg2 lsb-release build-essential -RUN apt install -q -y libboost-all-dev libomp-dev -RUN apt install -q -y python3-colcon-core python3-colcon-common-extensions \ No newline at end of file +RUN apt install -q -y curl gnupg2 lsb-release build-essential cmake +RUN apt install -q -y python3-colcon-core python3-colcon-common-extensions +RUN apt install -q -y libeigen3-dev libomp-dev \ No newline at end of file diff --git a/Jenkinsfile b/Jenkinsfile index 9a7c9e8..434501d 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -61,7 +61,7 @@ pipeline { dir('lgmath') { git branch: 'master', url: 'https://github.com/utiasASRL/lgmath.git' sh ''' - source /opt/ros/foxy/setup.bash + source /opt/ros/galactic/setup.bash colcon build --symlink-install --cmake-args "-DUSE_AMENT=ON" touch COLGON_IGNORE ''' diff --git a/README.md b/README.md index 263ef6b..db22587 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,6 @@ STEAM (Simultaneous Trajectory Estimation and Mapping) Engine is an optimization - Compiler with C++17 support and OpenMP - CMake (>=3.16) -- Boost (>=1.71.0) - Eigen (>=3.3.7) - [lgmath (>=1.1.0)](https://github.com/utiasASRL/lgmath.git) - (Optional) ROS2 Foxy or later (colcon+ament_cmake) @@ -21,22 +20,6 @@ STEAM (Simultaneous Trajectory Estimation and Mapping) Engine is an optimization sudo apt -q -y install build-essential cmake libomp-dev ``` -### Install Boost - -```bash -# using APT -sudo apt -q -y install libboost-all-dev - -# OR from source -WORKSPACE=~/workspace # choose your own workspace directory -mkdir -p ${WORKSPACE}/boost && cd $_ -wget --no-verbose https://boostorg.jfrog.io/artifactory/main/release/1.71.0/source/boost_1_71_0.tar.gz -tar xzf boost_1_71_0.tar.gz && cd boost_1_71_0 -./bootstrap.sh --with-python=$(which python3) -./b2 cxxflags="-std=gnu++17" install -ldconfig -``` - ### Install Eigen (>=3.3.7) ```bash diff --git a/include/steam.hpp b/include/steam.hpp index de57eea..fa4509f 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -38,6 +38,7 @@ #include #include #include +#include // evaluator - block auto diff #include diff --git a/include/steam/common/Time.hpp b/include/steam/common/Time.hpp index 9a96c8f..c9006eb 100644 --- a/include/steam/common/Time.hpp +++ b/include/steam/common/Time.hpp @@ -10,7 +10,6 @@ #define STEAM_TIME_HPP #include -#include namespace steam { @@ -20,11 +19,11 @@ namespace steam { class Time { public: Time() : nsecs_(0) {} - Time(boost::int64_t nsecs) : nsecs_(nsecs) {} + Time(int64_t nsecs) : nsecs_(nsecs) {} Time(double secs) : nsecs_(secs*1e9) {} - Time(boost::int32_t secs, boost::int32_t nsec) { - boost::int64_t t1 = (boost::int64_t) secs; - boost::int64_t t2 = (boost::int64_t) nsec; + Time(int32_t secs, int32_t nsec) { + int64_t t1 = (int64_t) secs; + int64_t t2 = (int64_t) nsec; this->nsecs_ = t1*1000000000 + t2; } @@ -44,7 +43,7 @@ class Time { /// Usually it makes sense to use this method after *this* has been reduced to a /// duration between two times. ////////////////////////////////////////////////////////////////////////////////////////////// - const boost::int64_t& nanosecs() const { + const int64_t& nanosecs() const { return nsecs_; } @@ -108,7 +107,7 @@ class Time { /// seconds since epoch and 1e9 nsecs. Furthermore, a single base type, rather than /// two combined unsigned int32s to allow nsecs to be used as a key in a std::map. ////////////////////////////////////////////////////////////////////////////////////////////// - boost::int64_t nsecs_; + int64_t nsecs_; }; diff --git a/include/steam/common/Timer.hpp b/include/steam/common/Timer.hpp index 3553d34..1a3b294 100644 --- a/include/steam/common/Timer.hpp +++ b/include/steam/common/Timer.hpp @@ -11,7 +11,6 @@ // system timer #include #include -#include namespace steam { diff --git a/include/steam/evaluator/ErrorEvaluator.hpp b/include/steam/evaluator/ErrorEvaluator.hpp index 61b377f..123b29c 100644 --- a/include/steam/evaluator/ErrorEvaluator.hpp +++ b/include/steam/evaluator/ErrorEvaluator.hpp @@ -21,8 +21,8 @@ template // The maximum dimension of a single state variable perturbation struct ErrorEvaluator{ typedef EvaluatorBase,MEAS_DIM,MEAS_DIM,MAX_STATE_SIZE> type; - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; }; ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/steam/evaluator/EvaluatorBase.hpp b/include/steam/evaluator/EvaluatorBase.hpp index 32241f9..80f6979 100644 --- a/include/steam/evaluator/EvaluatorBase.hpp +++ b/include/steam/evaluator/EvaluatorBase.hpp @@ -28,8 +28,8 @@ class EvaluatorBase public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Default constructor diff --git a/include/steam/evaluator/blockauto/BlockAutomaticEvaluator.hpp b/include/steam/evaluator/blockauto/BlockAutomaticEvaluator.hpp index 6c5b6b6..0c45959 100644 --- a/include/steam/evaluator/blockauto/BlockAutomaticEvaluator.hpp +++ b/include/steam/evaluator/blockauto/BlockAutomaticEvaluator.hpp @@ -22,8 +22,8 @@ class BlockAutomaticEvaluator : public EvaluatorBase public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Default constructor diff --git a/include/steam/evaluator/blockauto/EvalTreeNode-inl.hpp b/include/steam/evaluator/blockauto/EvalTreeNode-inl.hpp index 1ae5b20..07041af 100644 --- a/include/steam/evaluator/blockauto/EvalTreeNode-inl.hpp +++ b/include/steam/evaluator/blockauto/EvalTreeNode-inl.hpp @@ -5,7 +5,6 @@ ////////////////////////////////////////////////////////////////////////////////////////////// #include -#include namespace steam { diff --git a/include/steam/evaluator/blockauto/transform/ComposeInverseTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ComposeInverseTransformEvaluator.hpp index d404a7c..9429e7b 100644 --- a/include/steam/evaluator/blockauto/transform/ComposeInverseTransformEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/ComposeInverseTransformEvaluator.hpp @@ -22,8 +22,8 @@ class ComposeInverseTransformEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor -- T_ba = T_bx * inv(T_ax) diff --git a/include/steam/evaluator/blockauto/transform/ComposeLandmarkEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ComposeLandmarkEvaluator.hpp index 1cee4ca..0de2f34 100644 --- a/include/steam/evaluator/blockauto/transform/ComposeLandmarkEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/ComposeLandmarkEvaluator.hpp @@ -28,8 +28,8 @@ class ComposeLandmarkEvaluator : public BlockAutomaticEvaluator Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/ComposeTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ComposeTransformEvaluator.hpp index 7cf2d34..56b4288 100644 --- a/include/steam/evaluator/blockauto/transform/ComposeTransformEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/ComposeTransformEvaluator.hpp @@ -22,8 +22,8 @@ class ComposeTransformEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp index f4dc1da..3f88007 100644 --- a/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp @@ -20,8 +20,8 @@ class ConstAccTransformEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/ConstVelTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ConstVelTransformEvaluator.hpp index e80c91a..0719be0 100644 --- a/include/steam/evaluator/blockauto/transform/ConstVelTransformEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/ConstVelTransformEvaluator.hpp @@ -20,8 +20,8 @@ class ConstVelTransformEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/FixedTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/FixedTransformEvaluator.hpp index 8402917..4fd4e93 100644 --- a/include/steam/evaluator/blockauto/transform/FixedTransformEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/FixedTransformEvaluator.hpp @@ -20,8 +20,8 @@ class FixedTransformEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/InverseTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/InverseTransformEvaluator.hpp index 9f8ac6f..ad0938b 100644 --- a/include/steam/evaluator/blockauto/transform/InverseTransformEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/InverseTransformEvaluator.hpp @@ -22,8 +22,8 @@ class InverseTransformEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/LogMapEvaluator.hpp b/include/steam/evaluator/blockauto/transform/LogMapEvaluator.hpp index 86cc440..bf598f4 100644 --- a/include/steam/evaluator/blockauto/transform/LogMapEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/LogMapEvaluator.hpp @@ -27,8 +27,8 @@ class LogMapEvaluator : public BlockAutomaticEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/PositionEvaluator.hpp b/include/steam/evaluator/blockauto/transform/PositionEvaluator.hpp index 1fe7d89..9449754 100644 --- a/include/steam/evaluator/blockauto/transform/PositionEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/PositionEvaluator.hpp @@ -26,8 +26,8 @@ class PositionEvaluator : public BlockAutomaticEvaluator Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/blockauto/transform/TransformStateEvaluator.hpp b/include/steam/evaluator/blockauto/transform/TransformStateEvaluator.hpp index b0210a0..1bb9f71 100644 --- a/include/steam/evaluator/blockauto/transform/TransformStateEvaluator.hpp +++ b/include/steam/evaluator/blockauto/transform/TransformStateEvaluator.hpp @@ -20,8 +20,8 @@ class TransformStateEvaluator : public TransformEvaluator public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/samples/ImuErrorEval.hpp b/include/steam/evaluator/samples/ImuErrorEval.hpp index e705677..ecc889a 100644 --- a/include/steam/evaluator/samples/ImuErrorEval.hpp +++ b/include/steam/evaluator/samples/ImuErrorEval.hpp @@ -19,8 +19,8 @@ class ImuErrorEval : public ErrorEvaluator<6,6>::type public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/samples/LinearFuncErrorEval.hpp b/include/steam/evaluator/samples/LinearFuncErrorEval.hpp index f5b8751..0b79c2b 100644 --- a/include/steam/evaluator/samples/LinearFuncErrorEval.hpp +++ b/include/steam/evaluator/samples/LinearFuncErrorEval.hpp @@ -20,8 +20,8 @@ class LinearFuncErrorEval : public ErrorEvaluator::type public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/samples/PointToPointErrorEval.hpp b/include/steam/evaluator/samples/PointToPointErrorEval.hpp index c5e0279..ebeed00 100644 --- a/include/steam/evaluator/samples/PointToPointErrorEval.hpp +++ b/include/steam/evaluator/samples/PointToPointErrorEval.hpp @@ -24,8 +24,8 @@ class PointToPointErrorEval : public ErrorEvaluator<4,6>::type public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/samples/PointToPointErrorEval2.hpp b/include/steam/evaluator/samples/PointToPointErrorEval2.hpp new file mode 100644 index 0000000..6b34c5e --- /dev/null +++ b/include/steam/evaluator/samples/PointToPointErrorEval2.hpp @@ -0,0 +1,69 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file PointToPointErrorEval2.cpp +/// +/// \author Yuchen Wu, ASRL +/// \brief This evaluator was developed in the context of ICP (Iterative +/// Closest Point) implementation. It is different from +/// PointToPointErrorEval in that it takes points in cartesian +/// coordinates instead of homogeneous coordinates. +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_POINT_TO_POINT_ERROR_EVALUATOR_2_HPP +#define STEAM_POINT_TO_POINT_ERROR_EVALUATOR_2_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief The distance between two points living in their respective frame is +/// used as our error function. +////////////////////////////////////////////////////////////////////////////////////////////// +class PointToPointErrorEval2 : public ErrorEvaluator<3, 6>::type { + public: + /// Convenience typedefs + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// \param T_rq Transformation matrix from query to reference. + /// \param reference A point in the 'reference' frame expressed in cartesian + /// coordinates. + /// \param query A point in the 'query' frame expressed in cartesian + /// coordinates. + ////////////////////////////////////////////////////////////////////////////////////////////// + PointToPointErrorEval2(const se3::TransformEvaluator::ConstPtr &T_rq, + const Eigen::Vector3d &reference, + const Eigen::Vector3d &query); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state + /// variables + ////////////////////////////////////////////////////////////////////////////////////////////// + bool isActive() const override; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 3-d measurement error (x, y, z) + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix evaluate() const override; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 3-d measurement error (x, y, z) and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate( + const Eigen::Matrix &lhs, + std::vector> *jacs) const; + + private: + se3::TransformEvaluator::ConstPtr T_rq_; + + Eigen::Matrix D_ = Eigen::Matrix::Zero(); + + Eigen::Vector4d reference_ = Eigen::Vector4d::Constant(1); + Eigen::Vector4d query_ = Eigen::Vector4d::Constant(1); +}; + +} // namespace steam + +#endif // STEAM_POINT_TO_POINT_ERROR_EVALUATOR_2_HPP \ No newline at end of file diff --git a/include/steam/evaluator/samples/PositionErrorEval.hpp b/include/steam/evaluator/samples/PositionErrorEval.hpp index a71bd85..0fbae2a 100644 --- a/include/steam/evaluator/samples/PositionErrorEval.hpp +++ b/include/steam/evaluator/samples/PositionErrorEval.hpp @@ -19,8 +19,8 @@ class PositionErrorEval : public ErrorEvaluator<3, 6>::type { public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor - error is difference between linear component of 'T' and zero diff --git a/include/steam/evaluator/samples/StereoCameraErrorEval.hpp b/include/steam/evaluator/samples/StereoCameraErrorEval.hpp index f5f498d..b9da09e 100644 --- a/include/steam/evaluator/samples/StereoCameraErrorEval.hpp +++ b/include/steam/evaluator/samples/StereoCameraErrorEval.hpp @@ -18,8 +18,8 @@ namespace stereo { ////////////////////////////////////////////////////////////////////////////////////////////// struct CameraIntrinsics { /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; /// \brief Stereo baseline double b; @@ -131,8 +131,8 @@ class StereoCameraErrorEval : public ErrorEvaluator<4,6>::type public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/samples/StereoCameraErrorEvalX.hpp b/include/steam/evaluator/samples/StereoCameraErrorEvalX.hpp index 8f75ed3..71a4b36 100644 --- a/include/steam/evaluator/samples/StereoCameraErrorEvalX.hpp +++ b/include/steam/evaluator/samples/StereoCameraErrorEvalX.hpp @@ -24,8 +24,8 @@ class StereoCameraErrorEvalX : public ErrorEvaluatorX struct CameraIntrinsics { /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; /// \brief Stereo baseline double b; @@ -44,8 +44,8 @@ class StereoCameraErrorEvalX : public ErrorEvaluatorX }; /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/evaluator/samples/TransformErrorEval.hpp b/include/steam/evaluator/samples/TransformErrorEval.hpp index b624467..c9174c6 100644 --- a/include/steam/evaluator/samples/TransformErrorEval.hpp +++ b/include/steam/evaluator/samples/TransformErrorEval.hpp @@ -19,8 +19,8 @@ class TransformErrorEval : public ErrorEvaluator<6,6>::type public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor - error is difference between 'T' and identity (in Lie algebra space) diff --git a/include/steam/evaluator/samples/VectorSpaceErrorEval.hpp b/include/steam/evaluator/samples/VectorSpaceErrorEval.hpp index 9e4fcab..96dbe54 100644 --- a/include/steam/evaluator/samples/VectorSpaceErrorEval.hpp +++ b/include/steam/evaluator/samples/VectorSpaceErrorEval.hpp @@ -20,8 +20,8 @@ class VectorSpaceErrorEval : public ErrorEvaluator::type public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/problem/CostTermBase.hpp b/include/steam/problem/CostTermBase.hpp index 9b46a86..0446ecb 100644 --- a/include/steam/problem/CostTermBase.hpp +++ b/include/steam/problem/CostTermBase.hpp @@ -7,8 +7,6 @@ #ifndef STEAM_COST_TERM_BASE_HPP #define STEAM_COST_TERM_BASE_HPP -#include - #include #include #include @@ -25,8 +23,8 @@ class CostTermBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/problem/NoiseModel-inl.hpp b/include/steam/problem/NoiseModel-inl.hpp index a82d8c7..9c2e3b7 100644 --- a/include/steam/problem/NoiseModel-inl.hpp +++ b/include/steam/problem/NoiseModel-inl.hpp @@ -15,7 +15,7 @@ namespace steam { template -DynamicNoiseModel::DynamicNoiseModel(boost::shared_ptr> eval) : +DynamicNoiseModel::DynamicNoiseModel(std::shared_ptr> eval) : eval_(eval) { this->setByCovariance(eval_->evaluateCovariance()); } @@ -141,7 +141,7 @@ void BaseNoiseModel::assertPositiveDefiniteMatrix( ////////////////////////////////////////////////////////////////////////////////////////////// template StaticNoiseModel::StaticNoiseModel(const Eigen::Matrix& matrix, - MatrixType type) + MatrixType type) : BaseNoiseModel::BaseNoiseModel(matrix,type) { } diff --git a/include/steam/problem/NoiseModel.hpp b/include/steam/problem/NoiseModel.hpp index 4611861..02cf4f7 100644 --- a/include/steam/problem/NoiseModel.hpp +++ b/include/steam/problem/NoiseModel.hpp @@ -8,7 +8,6 @@ #define STEAM_NOISE_MODEL_HPP #include -#include #include namespace steam { @@ -18,8 +17,8 @@ template class NoiseEvaluator { public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; /// \brief Default constructor. NoiseEvaluator()=default; @@ -64,8 +63,8 @@ class BaseNoiseModel virtual ~BaseNoiseModel() = default; /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Set by covariance matrix @@ -125,8 +124,8 @@ class StaticNoiseModel : public BaseNoiseModel public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Default constructor @@ -168,7 +167,7 @@ class DynamicNoiseModel : public BaseNoiseModel public: /// \brief Constructor /// \param eval a pointer to a noise evaluator. - DynamicNoiseModel(boost::shared_ptr> eval); + DynamicNoiseModel(std::shared_ptr> eval); /// \brief Deault destructor. ~DynamicNoiseModel()=default; @@ -191,7 +190,7 @@ class DynamicNoiseModel : public BaseNoiseModel private: /// \brief A pointer to a noise evaluator. - boost::shared_ptr> eval_; + std::shared_ptr> eval_; }; ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/steam/problem/ParallelizedCostTermCollection.hpp b/include/steam/problem/ParallelizedCostTermCollection.hpp index e203848..8d55859 100644 --- a/include/steam/problem/ParallelizedCostTermCollection.hpp +++ b/include/steam/problem/ParallelizedCostTermCollection.hpp @@ -7,8 +7,6 @@ #ifndef STEAM_PARALLELIZED_COST_TERM_COLLECTION_HPP #define STEAM_PARALLELIZED_COST_TERM_COLLECTION_HPP -#include - #include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -35,8 +33,8 @@ class ParallelizedCostTermCollection : public CostTermBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/problem/WeightedLeastSqCostTerm.hpp b/include/steam/problem/WeightedLeastSqCostTerm.hpp index 9da327d..10dda9c 100644 --- a/include/steam/problem/WeightedLeastSqCostTerm.hpp +++ b/include/steam/problem/WeightedLeastSqCostTerm.hpp @@ -7,8 +7,6 @@ #ifndef STEAM_WEIGHTED_LSQ_COST_TERM_HPP #define STEAM_WEIGHTED_LSQ_COST_TERM_HPP -#include - #include #include @@ -27,8 +25,8 @@ class WeightedLeastSqCostTerm : public CostTermBase public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/problem/lossfunc/CauchyLossFunc.hpp b/include/steam/problem/lossfunc/CauchyLossFunc.hpp index cbda3bd..6fddb1e 100644 --- a/include/steam/problem/lossfunc/CauchyLossFunc.hpp +++ b/include/steam/problem/lossfunc/CauchyLossFunc.hpp @@ -8,7 +8,6 @@ #define STEAM_CAUCHY_LOSS_FUNCTION_HPP #include -#include #include @@ -22,8 +21,8 @@ class CauchyLossFunc : public LossFunctionBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical) diff --git a/include/steam/problem/lossfunc/DcsLossFunc.hpp b/include/steam/problem/lossfunc/DcsLossFunc.hpp index e2b05af..f4acc70 100644 --- a/include/steam/problem/lossfunc/DcsLossFunc.hpp +++ b/include/steam/problem/lossfunc/DcsLossFunc.hpp @@ -8,7 +8,6 @@ #define STEAM_DCS_LOSS_FUNCTION_HPP #include -#include #include @@ -22,8 +21,8 @@ class DcsLossFunc : public LossFunctionBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical) diff --git a/include/steam/problem/lossfunc/GemanMcClureLossFunc.hpp b/include/steam/problem/lossfunc/GemanMcClureLossFunc.hpp index 9bdab14..1bb6c1a 100644 --- a/include/steam/problem/lossfunc/GemanMcClureLossFunc.hpp +++ b/include/steam/problem/lossfunc/GemanMcClureLossFunc.hpp @@ -8,7 +8,6 @@ #define STEAM_GM_LOSS_FUNCTION_HPP #include -#include #include @@ -22,8 +21,8 @@ class GemanMcClureLossFunc : public LossFunctionBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical) diff --git a/include/steam/problem/lossfunc/HuberLossFunc.hpp b/include/steam/problem/lossfunc/HuberLossFunc.hpp index 0f6f06d..c1e7e19 100644 --- a/include/steam/problem/lossfunc/HuberLossFunc.hpp +++ b/include/steam/problem/lossfunc/HuberLossFunc.hpp @@ -8,7 +8,6 @@ #define STEAM_HUBER_LOSS_FUNCTION_HPP #include -#include #include @@ -22,8 +21,8 @@ class HuberLossFunc : public LossFunctionBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical) diff --git a/include/steam/problem/lossfunc/L2LossFunc.hpp b/include/steam/problem/lossfunc/L2LossFunc.hpp index 1093c43..1da8087 100644 --- a/include/steam/problem/lossfunc/L2LossFunc.hpp +++ b/include/steam/problem/lossfunc/L2LossFunc.hpp @@ -8,7 +8,6 @@ #define STEAM_L2_LOSS_FUNCTION_HPP #include -#include #include @@ -22,8 +21,8 @@ class L2LossFunc : public LossFunctionBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/problem/lossfunc/LossFunctionBase.hpp b/include/steam/problem/lossfunc/LossFunctionBase.hpp index b75219f..ac724f9 100644 --- a/include/steam/problem/lossfunc/LossFunctionBase.hpp +++ b/include/steam/problem/lossfunc/LossFunctionBase.hpp @@ -7,8 +7,9 @@ #ifndef STEAM_LOSS_FUNCTION_BASE_HPP #define STEAM_LOSS_FUNCTION_BASE_HPP +#include + #include -#include namespace steam { @@ -22,8 +23,8 @@ class LossFunctionBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/solver/DoglegGaussNewtonSolver.hpp b/include/steam/solver/DoglegGaussNewtonSolver.hpp index 062502c..92157b6 100644 --- a/include/steam/solver/DoglegGaussNewtonSolver.hpp +++ b/include/steam/solver/DoglegGaussNewtonSolver.hpp @@ -8,7 +8,6 @@ #define STEAM_DOGLEG_GAUSS_NEWTON_SOLVER_HPP #include -#include #include diff --git a/include/steam/solver/GaussNewtonSolverBase.hpp b/include/steam/solver/GaussNewtonSolverBase.hpp index fbbf654..f5e6d7b 100644 --- a/include/steam/solver/GaussNewtonSolverBase.hpp +++ b/include/steam/solver/GaussNewtonSolverBase.hpp @@ -8,7 +8,6 @@ #define STEAM_GAUSS_NEWTON_SOLVER_HPP #include -#include #include diff --git a/include/steam/solver/LevMarqGaussNewtonSolver.hpp b/include/steam/solver/LevMarqGaussNewtonSolver.hpp index c74a84b..106fa10 100644 --- a/include/steam/solver/LevMarqGaussNewtonSolver.hpp +++ b/include/steam/solver/LevMarqGaussNewtonSolver.hpp @@ -8,7 +8,6 @@ #define STEAM_LEVMARQ_GAUSS_NEWTON_SOLVER_HPP #include -#include #include diff --git a/include/steam/solver/LineSearchGaussNewtonSolver.hpp b/include/steam/solver/LineSearchGaussNewtonSolver.hpp index c378954..c85f43a 100644 --- a/include/steam/solver/LineSearchGaussNewtonSolver.hpp +++ b/include/steam/solver/LineSearchGaussNewtonSolver.hpp @@ -8,7 +8,6 @@ #define STEAM_LINE_SEARCH_GAUSS_NEWTON_SOLVER_HPP #include -#include #include diff --git a/include/steam/solver/SolverBase.hpp b/include/steam/solver/SolverBase.hpp index 79f0f1a..4b560f2 100644 --- a/include/steam/solver/SolverBase.hpp +++ b/include/steam/solver/SolverBase.hpp @@ -7,11 +7,10 @@ #ifndef STEAM_SOLVER_BASE_HPP #define STEAM_SOLVER_BASE_HPP -#include -#include -#include #include +#include + #include namespace steam { diff --git a/include/steam/solver/VanillaGaussNewtonSolver.hpp b/include/steam/solver/VanillaGaussNewtonSolver.hpp index 4fa9fae..e9ffa67 100644 --- a/include/steam/solver/VanillaGaussNewtonSolver.hpp +++ b/include/steam/solver/VanillaGaussNewtonSolver.hpp @@ -8,7 +8,6 @@ #define STEAM_VANILLA_GAUSS_NEWTON_SOLVER_HPP #include -#include #include diff --git a/include/steam/state/LandmarkStateVar.hpp b/include/steam/state/LandmarkStateVar.hpp index 03691ec..0c5a28c 100644 --- a/include/steam/state/LandmarkStateVar.hpp +++ b/include/steam/state/LandmarkStateVar.hpp @@ -21,8 +21,8 @@ class LandmarkStateVar : public StateVariable public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor from a global 3D point diff --git a/include/steam/state/LieGroupStateVar.hpp b/include/steam/state/LieGroupStateVar.hpp index 68cf34c..8cbe1a9 100644 --- a/include/steam/state/LieGroupStateVar.hpp +++ b/include/steam/state/LieGroupStateVar.hpp @@ -22,8 +22,8 @@ class LieGroupStateVar : public StateVariable public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ///////////////////////////////////////////////////////////////////////////////////////////// /// \brief Default constructor diff --git a/include/steam/state/StateVariable-inl.hpp b/include/steam/state/StateVariable-inl.hpp index 6beb612..1df432a 100644 --- a/include/steam/state/StateVariable-inl.hpp +++ b/include/steam/state/StateVariable-inl.hpp @@ -46,7 +46,7 @@ void StateVariable::setFromCopy(const StateVariableBase::ConstPtr& other) if (!this->getKey().equals(other->getKey())) { throw std::invalid_argument("State variable keys did not match in setFromCopy()"); } - StateVariable::ConstPtr p = boost::static_pointer_cast >(other); + StateVariable::ConstPtr p = std::static_pointer_cast >(other); value_ = p->value_; } diff --git a/include/steam/state/StateVariable.hpp b/include/steam/state/StateVariable.hpp index a89f381..d153381 100644 --- a/include/steam/state/StateVariable.hpp +++ b/include/steam/state/StateVariable.hpp @@ -21,8 +21,8 @@ class StateVariable : public StateVariableBase public: /// Convenience typedefs - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef std::shared_ptr > Ptr; + typedef std::shared_ptr > ConstPtr; ///////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor based on perturbation dimension diff --git a/include/steam/state/StateVariableBase.hpp b/include/steam/state/StateVariableBase.hpp index ce0923b..6efa418 100644 --- a/include/steam/state/StateVariableBase.hpp +++ b/include/steam/state/StateVariableBase.hpp @@ -7,9 +7,10 @@ #ifndef STEAM_STATE_VARIABLE_BASE_HPP #define STEAM_STATE_VARIABLE_BASE_HPP +#include +#include + #include -#include -#include namespace steam { @@ -28,8 +29,8 @@ class IdGenerator ///////////////////////////////////////////////////////////////////////////////////////////// static StateID getNextId() { static StateID nextId = 0; - static boost::mutex idMutex; - boost::lock_guard guard(idMutex); + static std::mutex idMutex; + std::lock_guard guard(idMutex); return ++nextId; } }; @@ -73,8 +74,8 @@ class StateVariableBase public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ///////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/state/StateVector.hpp b/include/steam/state/StateVector.hpp index 2bbaabf..bf4d4ca 100644 --- a/include/steam/state/StateVector.hpp +++ b/include/steam/state/StateVector.hpp @@ -7,10 +7,11 @@ #ifndef STEAM_STATE_VECTOR_HPP #define STEAM_STATE_VECTOR_HPP -#include -#include +#include #include +#include + namespace steam { ////////////////////////////////////////////////////////////////////////////////////////////// @@ -94,7 +95,7 @@ class StateVector ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Main container for state variables ////////////////////////////////////////////////////////////////////////////////////////////// - boost::unordered_map states_; + std::unordered_map states_; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Block size of the related linear system diff --git a/include/steam/state/VectorSpaceStateVar.hpp b/include/steam/state/VectorSpaceStateVar.hpp index 36cf074..d3225c2 100644 --- a/include/steam/state/VectorSpaceStateVar.hpp +++ b/include/steam/state/VectorSpaceStateVar.hpp @@ -19,8 +19,8 @@ class VectorSpaceStateVar : public StateVariable public: /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ///////////////////////////////////////////////////////////////////////////////////////////// /// \brief Construct from Eigen vector (perturbation dimension assumed to match vector) diff --git a/include/steam/trajectory/SteamCATrajInterface.hpp b/include/steam/trajectory/SteamCATrajInterface.hpp index dbc22db..f2f0e14 100644 --- a/include/steam/trajectory/SteamCATrajInterface.hpp +++ b/include/steam/trajectory/SteamCATrajInterface.hpp @@ -34,11 +34,11 @@ class SteamCATrajInterface : public SteamTrajInterface /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration); void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration, const Eigen::Matrix cov); diff --git a/include/steam/trajectory/SteamCATrajPriorFactor.hpp b/include/steam/trajectory/SteamCATrajPriorFactor.hpp index 7738e0a..e23dac6 100644 --- a/include/steam/trajectory/SteamCATrajPriorFactor.hpp +++ b/include/steam/trajectory/SteamCATrajPriorFactor.hpp @@ -23,8 +23,8 @@ class SteamCATrajPriorFactor : public ErrorEvaluatorX public: /// Shared pointer typedefs for readability - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/trajectory/SteamCATrajVar.hpp b/include/steam/trajectory/SteamCATrajVar.hpp index 1de5d9d..1d0f6d4 100644 --- a/include/steam/trajectory/SteamCATrajVar.hpp +++ b/include/steam/trajectory/SteamCATrajVar.hpp @@ -27,18 +27,18 @@ class SteamCATrajVar : public SteamTrajVar public: /// Shared pointer typedefs for readability - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration); SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration, const Eigen::Matrix cov); diff --git a/include/steam/trajectory/SteamTrajInterface.hpp b/include/steam/trajectory/SteamTrajInterface.hpp index 540f72f..0f226fb 100644 --- a/include/steam/trajectory/SteamTrajInterface.hpp +++ b/include/steam/trajectory/SteamTrajInterface.hpp @@ -50,21 +50,21 @@ class SteamTrajInterface /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity); + const VectorSpaceStateVar::Ptr& w_0k_ink); void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const Eigen::Matrix cov); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration); virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration, const Eigen::Matrix cov); @@ -82,14 +82,14 @@ class SteamTrajInterface /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior /// should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// - void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, + void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& T_k0, const Eigen::Matrix& cov); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity /// prior should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// - void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, + void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& w_0k_ink, const Eigen::Matrix& cov); ////////////////////////////////////////////////////////////////////////////////////////////// @@ -148,7 +148,7 @@ class SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Ordered map of knots ////////////////////////////////////////////////////////////////////////////////////////////// - std::map knotMap_; + std::map knotMap_; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Solver used, we use this to query covariances diff --git a/include/steam/trajectory/SteamTrajPriorFactor.hpp b/include/steam/trajectory/SteamTrajPriorFactor.hpp index 3f26c90..61977d3 100644 --- a/include/steam/trajectory/SteamTrajPriorFactor.hpp +++ b/include/steam/trajectory/SteamTrajPriorFactor.hpp @@ -23,8 +23,8 @@ class SteamTrajPriorFactor : public ErrorEvaluatorX public: /// Shared pointer typedefs for readability - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor diff --git a/include/steam/trajectory/SteamTrajVar.hpp b/include/steam/trajectory/SteamTrajVar.hpp index 154de09..4642438 100644 --- a/include/steam/trajectory/SteamTrajVar.hpp +++ b/include/steam/trajectory/SteamTrajVar.hpp @@ -25,17 +25,17 @@ class SteamTrajVar public: /// Shared pointer typedefs for readability - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity); + const VectorSpaceStateVar::Ptr& w_0k_ink); SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, const Eigen::Matrix cov); + const VectorSpaceStateVar::Ptr& w_0k_ink, const Eigen::Matrix cov); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get pose evaluator @@ -76,7 +76,7 @@ class SteamTrajVar ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Generalized 6D velocity state variable ////////////////////////////////////////////////////////////////////////////////////////////// - VectorSpaceStateVar::Ptr velocity_; + VectorSpaceStateVar::Ptr w_0k_ink_; protected: Eigen::MatrixXd cov_; diff --git a/package.xml b/package.xml index e565284..95e8705 100644 --- a/package.xml +++ b/package.xml @@ -12,7 +12,6 @@ ament_cmake eigen3_cmake_module - Boost eigen OpenMP lgmath diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index fb4c42b..6e2e30f 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -6,8 +6,7 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) add_compile_options(-march=native -O3 -Wall -pedantic -Wno-unused-function) -# Boost and OpenMP -find_package(Boost 1.71.0 REQUIRED system) +# OpenMP find_package(OpenMP REQUIRED) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") diff --git a/samples/SimplePointCloudAlignment.cpp b/samples/SimplePointCloudAlignment.cpp new file mode 100644 index 0000000..8c3d9ab --- /dev/null +++ b/samples/SimplePointCloudAlignment.cpp @@ -0,0 +1,76 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SimplePointCloudAlignment.cpp +/// \brief +/// +/// \author Yuchen Wu, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include + +int main(int argc, char **argv) { + /// Reference points to align against + Eigen::Matrix ref_pts; + // clang-format off + ref_pts << 0, 1, 1, 0, -1, -1, -1, 0, 1, + 0, 0, 1, 1, 1, 0, -1, -1, -1, + 0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0, + 1, 1, 1, 1, 1, 1, 1, 1, 1; + // clang-format on + + /// Ground truth pose + Eigen::Matrix T_mq_vec; + T_mq_vec << 1, 1, 1, 1, 1, 1; + lgmath::se3::Transformation T_mq(T_mq_vec); + + Eigen::Matrix qry_pts = T_mq.inverse().matrix() * ref_pts; + + // state and evaluator + steam::se3::TransformStateVar::Ptr T_mq_var( + new steam::se3::TransformStateVar()); + steam::se3::TransformStateEvaluator::ConstPtr T_mq_eval = + steam::se3::TransformStateEvaluator::MakeShared(T_mq_var); + + // shared noise and loss functions + steam::BaseNoiseModel<3>::Ptr noise_model( + new steam::StaticNoiseModel<3>(Eigen::MatrixXd::Identity(3, 3))); + steam::L2LossFunc::Ptr loss_func(new steam::L2LossFunc()); + + // cost terms + steam::ParallelizedCostTermCollection::Ptr cost_terms( + new steam::ParallelizedCostTermCollection()); + for (int i = 0; i < ref_pts.cols(); i++) { + // Construct error function + steam::PointToPointErrorEval2::Ptr error_func( + new steam::PointToPointErrorEval2(T_mq_eval, ref_pts.block<3, 1>(0, i), + qry_pts.block<3, 1>(0, i))); + + // Create cost term and add to problem + steam::WeightedLeastSqCostTerm<3, 6>::Ptr cost( + new steam::WeightedLeastSqCostTerm<3, 6>(error_func, noise_model, + loss_func)); + cost_terms->add(cost); + } + + // Initialize problem + steam::OptimizationProblem problem; + problem.addStateVariable(T_mq_var); + problem.addCostTerm(cost_terms); + + typedef steam::VanillaGaussNewtonSolver SolverType; + SolverType::Params params; + params.verbose = true; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + solver.optimize(); + + std::cout << "true T_mq:\n" << T_mq << std::endl; + std::cout << "estimated T_mq:\n" << T_mq_var->getValue() << std::endl; + + return 0; +} \ No newline at end of file diff --git a/samples/TrustRegionExample.cpp b/samples/TrustRegionExample.cpp index 980df75..7b7ad40 100644 --- a/samples/TrustRegionExample.cpp +++ b/samples/TrustRegionExample.cpp @@ -23,8 +23,8 @@ class DivergenceErrorEval : public steam::ErrorEvaluatorX ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Shared pointer typedefs for readability ////////////////////////////////////////////////////////////////////////////////////////////// - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Construct from a 1D state vector diff --git a/src/evaluator/samples/PointToPointErrorEval2.cpp b/src/evaluator/samples/PointToPointErrorEval2.cpp new file mode 100644 index 0000000..5c3b347 --- /dev/null +++ b/src/evaluator/samples/PointToPointErrorEval2.cpp @@ -0,0 +1,72 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file PointToPointErrorEval2.cpp +/// +/// \author Yuchen Wu, ASRL +/// \brief This evaluator was developed in the context of ICP (Iterative +/// Closest Point) implementation. It is different from +/// PointToPointErrorEval in that it takes points in cartesian +/// coordinates instead of homogeneous coordinates. +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +PointToPointErrorEval2::PointToPointErrorEval2( + const se3::TransformEvaluator::ConstPtr &T_rq, + const Eigen::Vector3d &reference, const Eigen::Vector3d &query) + : T_rq_(T_rq) { + D_.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); + reference_.block<3, 1>(0, 0) = reference; + query_.block<3, 1>(0, 0) = query; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool PointToPointErrorEval2::isActive() const { return T_rq_->isActive(); } + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the 3-d measurement error (x, y, z) +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Matrix PointToPointErrorEval2::evaluate() const { + return D_ * (reference_ - T_rq_->evaluate() * query_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the 3-d measurement error (x, y, z) and Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Matrix PointToPointErrorEval2::evaluate( + const Eigen::Matrix &lhs, + std::vector> *jacs) const { + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument( + "Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Get evaluation tree + EvalTreeHandle blkAutoEvalPosOfTransformDiff = + T_rq_->getBlockAutomaticEvaluation(); + + // Get evaluation from tree + const lgmath::se3::Transformation T_rq = + blkAutoEvalPosOfTransformDiff.getValue(); + Eigen::Matrix error = D_ * (reference_ - T_rq * query_); + + // Get Jacobians + const Eigen::Matrix Tq = (T_rq * query_).block<3, 1>(0, 0); + const Eigen::Matrix new_lhs = + -lhs * D_ * lgmath::se3::point2fs(Tq); + T_rq_->appendBlockAutomaticJacobians( + new_lhs, blkAutoEvalPosOfTransformDiff.getRoot(), jacs); + + // Return evaluation + return error; +} + +} // namespace steam \ No newline at end of file diff --git a/src/solver/DoglegGaussNewtonSolver.cpp b/src/solver/DoglegGaussNewtonSolver.cpp index 269ff80..14ff293 100644 --- a/src/solver/DoglegGaussNewtonSolver.cpp +++ b/src/solver/DoglegGaussNewtonSolver.cpp @@ -6,6 +6,7 @@ #include +#include #include #include diff --git a/src/solver/LevMarqGaussNewtonSolver.cpp b/src/solver/LevMarqGaussNewtonSolver.cpp index 7beb2cb..da4cc18 100644 --- a/src/solver/LevMarqGaussNewtonSolver.cpp +++ b/src/solver/LevMarqGaussNewtonSolver.cpp @@ -6,6 +6,7 @@ #include +#include #include #include diff --git a/src/solver/LineSearchGaussNewtonSolver.cpp b/src/solver/LineSearchGaussNewtonSolver.cpp index 380beb6..2b4cf10 100644 --- a/src/solver/LineSearchGaussNewtonSolver.cpp +++ b/src/solver/LineSearchGaussNewtonSolver.cpp @@ -6,6 +6,7 @@ #include +#include #include #include diff --git a/src/solver/VanillaGaussNewtonSolver.cpp b/src/solver/VanillaGaussNewtonSolver.cpp index 4d8af40..7695b77 100644 --- a/src/solver/VanillaGaussNewtonSolver.cpp +++ b/src/solver/VanillaGaussNewtonSolver.cpp @@ -6,6 +6,7 @@ #include +#include #include #include diff --git a/src/state/StateVector.cpp b/src/state/StateVector.cpp index b316999..e2bafc3 100644 --- a/src/state/StateVector.cpp +++ b/src/state/StateVector.cpp @@ -24,7 +24,7 @@ StateVector::StateVector(const StateVector& other) : states_(other.states_), // Map is copied in initialization list to avoid re-hashing all the entries, // now we go through the entries and perform a deep copy - boost::unordered_map::iterator it = states_.begin(); + std::unordered_map::iterator it = states_.begin(); for(; it != states_.end(); ++it) { it->second.state = it->second.state->clone(); } @@ -59,11 +59,11 @@ void StateVector::copyValues(const StateVector& other) { // Keeping the original pointers is important as they are shared in other places, and we // want to update the shared memory. // todo: can we avoid a 'find' here? - boost::unordered_map::iterator it = states_.begin(); + std::unordered_map::iterator it = states_.begin(); for(; it != states_.end(); ++it) { // Find matching state by ID - boost::unordered_map::const_iterator itOther = other.states_.find(it->second.state->getKey().getID()); + std::unordered_map::const_iterator itOther = other.states_.find(it->second.state->getKey().getID()); // Check that matching state was found and has the same structure if (itOther == other.states_.end() || @@ -112,7 +112,7 @@ void StateVector::addStateVariable(const StateVariableBase::Ptr& state) { bool StateVector::hasStateVariable(const StateKey& key) const { // Find the StateContainer for key - boost::unordered_map::const_iterator it = states_.find(key.getID()); + std::unordered_map::const_iterator it = states_.find(key.getID()); // Return if found return it != states_.end(); @@ -124,7 +124,7 @@ bool StateVector::hasStateVariable(const StateKey& key) const { StateVariableBase::ConstPtr StateVector::getStateVariable(const StateKey& key) const { // Find the StateContainer for key - boost::unordered_map::const_iterator it = states_.find(key.getID()); + std::unordered_map::const_iterator it = states_.find(key.getID()); // Check that it was found if (it == states_.end()) { @@ -148,7 +148,7 @@ unsigned int StateVector::getNumberOfStates() const { int StateVector::getStateBlockIndex(const StateKey& key) const { // Find the StateContainer for key - boost::unordered_map::const_iterator it = states_.find(key.getID()); + std::unordered_map::const_iterator it = states_.find(key.getID()); // Check that the state exists in the state vector // **Note the likely causes that this occurs: @@ -174,7 +174,7 @@ std::vector StateVector::getStateBlockSizes() const { result.resize(states_.size()); // Iterate over states and populate result - for (boost::unordered_map::const_iterator it = states_.begin(); + for (std::unordered_map::const_iterator it = states_.begin(); it != states_.end(); ++it ) { // Check that the local block index is in a valid range @@ -199,7 +199,7 @@ void StateVector::update(const Eigen::VectorXd& perturbation) { BlockVector blkPerturb(this->getStateBlockSizes(), perturbation); // Iterate over states and update each - for ( boost::unordered_map::const_iterator it = states_.begin(); it != states_.end(); ++it ) { + for ( std::unordered_map::const_iterator it = states_.begin(); it != states_.end(); ++it ) { // Check for valid index if (it->second.localBlockIndex < 0) { diff --git a/src/trajectory/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp index abcc53b..71cebd1 100644 --- a/src/trajectory/SteamCATrajInterface.cpp +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -24,11 +24,11 @@ namespace se3 { ////////////////////////////////////////////////////////////////////////////////////////////// void SteamCATrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration) { // Check velocity input - if (velocity->getPerturbDim() != 6) { + if (w_0k_ink->getPerturbDim() != 6) { throw std::invalid_argument("invalid velocity size"); } @@ -40,21 +40,21 @@ void SteamCATrajInterface::add(const steam::Time& time, // Todo, check that time does not already exist in map? // Make knot - SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, velocity, acceleration)); + SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, w_0k_ink, acceleration)); // Insert in map knotMap_.insert(knotMap_.end(), - std::pair(time.nanosecs(), newEntry)); + std::pair(time.nanosecs(), newEntry)); } void SteamCATrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration, const Eigen::Matrix cov) { // Check velocity input - if (velocity->getPerturbDim() != 6) { + if (w_0k_ink->getPerturbDim() != 6) { throw std::invalid_argument("invalid velocity size"); } @@ -66,11 +66,11 @@ void SteamCATrajInterface::add(const steam::Time& time, // Todo, check that time does not already exist in map? // Make knot - SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, velocity, acceleration, cov)); + SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, w_0k_ink, acceleration, cov)); // Insert in map knotMap_.insert(knotMap_.end(), - std::pair(time.nanosecs(), newEntry)); + std::pair(time.nanosecs(), newEntry)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -83,7 +83,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam } // Get iterator to first element with time equal to or great than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -127,7 +127,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam } // Get iterators bounding the time interval - std::map::const_iterator it2 = it1; --it1; + std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); @@ -144,7 +144,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { } // Get iterator to first element with time equal to or greater than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -185,7 +185,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { } // Get iterators bounding the time interval - std::map::const_iterator it2 = it1; --it1; + std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); @@ -279,7 +279,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { } // Get iterator to first element with time equal to or greater than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -328,7 +328,7 @@ void SteamCATrajInterface::addAccelerationPrior(const steam::Time& time, } // Try to find knot at same time - std::map::const_iterator it = knotMap_.find(time.nanosecs()); + std::map::const_iterator it = knotMap_.find(time.nanosecs()); if (it == knotMap_.end()) { throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); } @@ -378,13 +378,13 @@ void SteamCATrajInterface::appendPriorCostTerms( // steam::GemanMcClureLossFunc::Ptr sharedLossFunc(new steam::GemanMcClureLossFunc(1)); // Initialize first iterator - std::map::const_iterator it1 = knotMap_.begin(); + std::map::const_iterator it1 = knotMap_.begin(); if (it1 == knotMap_.end()) { throw std::runtime_error("No knots..."); } // Iterate through all states.. if any are unlocked, supply a prior term - std::map::const_iterator it2 = it1; ++it2; + std::map::const_iterator it2 = it1; ++it2; for (; it2 != knotMap_.end(); ++it1, ++it2) { // Get knots @@ -436,7 +436,7 @@ Eigen::MatrixXd SteamCATrajInterface::getCovariance(const steam::Time& time) con } // Get iterator to first element with time equal to or great than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry diff --git a/src/trajectory/SteamCATrajVar.cpp b/src/trajectory/SteamCATrajVar.cpp index 4d692c8..9e55f26 100644 --- a/src/trajectory/SteamCATrajVar.cpp +++ b/src/trajectory/SteamCATrajVar.cpp @@ -16,12 +16,12 @@ namespace se3 { ////////////////////////////////////////////////////////////////////////////////////////////// SteamCATrajVar::SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration) - : SteamTrajVar(time, T_k0, velocity), acceleration_(acceleration) { + : SteamTrajVar(time, T_k0, w_0k_ink), acceleration_(acceleration) { // Check velocity input - if (velocity->getPerturbDim() != 6) { + if (w_0k_ink->getPerturbDim() != 6) { throw std::invalid_argument("invalid velocity size"); } @@ -33,10 +33,10 @@ SteamCATrajVar::SteamCATrajVar(const steam::Time& time, SteamCATrajVar::SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration, const Eigen::Matrix cov) - : SteamCATrajVar(time, T_k0, velocity, acceleration) { cov_set_=true; cov_=cov; } + : SteamCATrajVar(time, T_k0, w_0k_ink, acceleration) { cov_set_=true; cov_=cov; } ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get acceleration state variable diff --git a/src/trajectory/SteamTrajInterface.cpp b/src/trajectory/SteamTrajInterface.cpp index 26714a3..2165c76 100644 --- a/src/trajectory/SteamTrajInterface.cpp +++ b/src/trajectory/SteamTrajInterface.cpp @@ -66,7 +66,7 @@ void SteamTrajInterface::add(const SteamTrajVar::Ptr& knot) { // Insert in map knotMap_.insert(knotMap_.end(), - std::pair(knot->getTime().nanosecs(), knot)); + std::pair(knot->getTime().nanosecs(), knot)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -74,57 +74,57 @@ void SteamTrajInterface::add(const SteamTrajVar::Ptr& knot) { ////////////////////////////////////////////////////////////////////////////////////////////// void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity) { + const VectorSpaceStateVar::Ptr& w_0k_ink) { // Check velocity input - if (velocity->getPerturbDim() != 6) { + if (w_0k_ink->getPerturbDim() != 6) { throw std::invalid_argument("invalid velocity size"); } // Todo, check that time does not already exist in map? // Make knot - SteamTrajVar::Ptr newEntry(new SteamTrajVar(time, T_k0, velocity)); + SteamTrajVar::Ptr newEntry(new SteamTrajVar(time, T_k0, w_0k_ink)); // Insert in map knotMap_.insert(knotMap_.end(), - std::pair(time.nanosecs(), newEntry)); + std::pair(time.nanosecs(), newEntry)); } void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const Eigen::Matrix cov) { // Check velocity input - if (velocity->getPerturbDim() != 6) { + if (w_0k_ink->getPerturbDim() != 6) { throw std::invalid_argument("invalid velocity size"); } // Todo, check that time does not already exist in map? // Make knot - SteamTrajVar::Ptr newEntry(new SteamTrajVar(time, T_k0, velocity, cov)); + SteamTrajVar::Ptr newEntry(new SteamTrajVar(time, T_k0, w_0k_ink, cov)); // Insert in map knotMap_.insert(knotMap_.end(), - std::pair(time.nanosecs(), newEntry)); + std::pair(time.nanosecs(), newEntry)); } ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration) { - add(time, T_k0, velocity); + add(time, T_k0, w_0k_ink); } void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const VectorSpaceStateVar::Ptr& acceleration, const Eigen::Matrix cov) { - add(time, T_k0, velocity, cov.topLeftCorner<12,12>()); + add(time, T_k0, w_0k_ink, cov.topLeftCorner<12,12>()); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -136,7 +136,7 @@ TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam:: throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); } // Get iterator to first element with time equal to or greater than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry if (it1 == knotMap_.end()) { @@ -174,7 +174,7 @@ TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam:: } } // Get iterators bounding the time interval - std::map::const_iterator it2 = it1; --it1; + std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); @@ -190,7 +190,7 @@ Eigen::VectorXd SteamTrajInterface::getVelocity(const steam::Time& time) { } // Get iterator to first element with time equal to or greater than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -225,7 +225,7 @@ Eigen::VectorXd SteamTrajInterface::getVelocity(const steam::Time& time) { } // Get iterators bounding the time interval - std::map::const_iterator it2 = it1; --it1; + std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); @@ -286,7 +286,7 @@ Eigen::VectorXd SteamTrajInterface::getVelocity(const steam::Time& time) { /// should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// void SteamTrajInterface::addPosePrior(const steam::Time& time, - const lgmath::se3::Transformation& pose, + const lgmath::se3::Transformation& T_k0, const Eigen::Matrix& cov) { // Check that map is not empty @@ -295,7 +295,7 @@ void SteamTrajInterface::addPosePrior(const steam::Time& time, } // Try to find knot at same time - std::map::const_iterator it = knotMap_.find(time.nanosecs()); + std::map::const_iterator it = knotMap_.find(time.nanosecs()); if (it == knotMap_.end()) { throw std::runtime_error("[GpTrajectory][addPosePrior] no knot at provided time."); } @@ -311,7 +311,7 @@ void SteamTrajInterface::addPosePrior(const steam::Time& time, // Set up loss function, noise model, and error function steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); - steam::TransformErrorEval::Ptr errorfunc(new steam::TransformErrorEval(pose, knotRef->getPose())); + steam::TransformErrorEval::Ptr errorfunc(new steam::TransformErrorEval(T_k0, knotRef->getPose())); // Create cost term posePriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( @@ -323,7 +323,7 @@ void SteamTrajInterface::addPosePrior(const steam::Time& time, /// prior should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// void SteamTrajInterface::addVelocityPrior(const steam::Time& time, - const Eigen::Matrix& velocity, + const Eigen::Matrix& w_0k_ink, const Eigen::Matrix& cov) { // Check that map is not empty @@ -332,7 +332,7 @@ void SteamTrajInterface::addVelocityPrior(const steam::Time& time, } // Try to find knot at same time - std::map::const_iterator it = knotMap_.find(time.nanosecs()); + std::map::const_iterator it = knotMap_.find(time.nanosecs()); if (it == knotMap_.end()) { throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); } @@ -348,7 +348,7 @@ void SteamTrajInterface::addVelocityPrior(const steam::Time& time, // Set up loss function, noise model, and error function steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); - steam::VectorSpaceErrorEval<6,6>::Ptr errorfunc(new steam::VectorSpaceErrorEval<6,6>(velocity, knotRef->getVelocity())); + steam::VectorSpaceErrorEval<6,6>::Ptr errorfunc(new steam::VectorSpaceErrorEval<6,6>(w_0k_ink, knotRef->getVelocity())); // Create cost term velocityPriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( @@ -378,13 +378,13 @@ void SteamTrajInterface::appendPriorCostTerms( steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); // Initialize first iterator - std::map::const_iterator it1 = knotMap_.begin(); + std::map::const_iterator it1 = knotMap_.begin(); if (it1 == knotMap_.end()) { throw std::runtime_error("No knots..."); } // Iterate through all states.. if any are unlocked, supply a prior term - std::map::const_iterator it2 = it1; ++it2; + std::map::const_iterator it2 = it1; ++it2; for (; it2 != knotMap_.end(); ++it1, ++it2) { // Get knots @@ -434,7 +434,7 @@ Eigen::MatrixXd SteamTrajInterface::getCovariance(const steam::Time& time) const } // Get iterator to first element with time equal to or great than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -508,7 +508,7 @@ void SteamTrajInterface::getActiveStateVariables( std::map* outStates) const { // Iterate over trajectory - std::map::const_iterator it; + std::map::const_iterator it; for (it = knotMap_.begin(); it != knotMap_.end(); ++it) { // Append active states in transform evaluator diff --git a/src/trajectory/SteamTrajVar.cpp b/src/trajectory/SteamTrajVar.cpp index 6c33c18..3494b73 100644 --- a/src/trajectory/SteamTrajVar.cpp +++ b/src/trajectory/SteamTrajVar.cpp @@ -16,20 +16,20 @@ namespace se3 { ////////////////////////////////////////////////////////////////////////////////////////////// SteamTrajVar::SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity) - : time_(time), T_k0_(T_k0), velocity_(velocity) { + const VectorSpaceStateVar::Ptr& w_0k_ink) + : time_(time), T_k0_(T_k0), w_0k_ink_(w_0k_ink) { // Check velocity input - if (velocity->getPerturbDim() != 6) { + if (w_0k_ink->getPerturbDim() != 6) { throw std::invalid_argument("invalid velocity size"); } } SteamTrajVar::SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, - const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& w_0k_ink, const Eigen::Matrix cov) - : SteamTrajVar(time, T_k0, velocity) { cov_set_=true; cov_=cov; } + : SteamTrajVar(time, T_k0, w_0k_ink) { cov_set_=true; cov_=cov; } ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get pose evaluator @@ -42,7 +42,7 @@ const se3::TransformEvaluator::Ptr& SteamTrajVar::getPose() const { /// \brief Get velocity state variable ////////////////////////////////////////////////////////////////////////////////////////////// const VectorSpaceStateVar::Ptr& SteamTrajVar::getVelocity() const { - return velocity_; + return w_0k_ink_; } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/tests/time_test.cpp b/tests/time_test.cpp index 1f91ebc..ca0abd3 100644 --- a/tests/time_test.cpp +++ b/tests/time_test.cpp @@ -7,7 +7,7 @@ TEST(Steam, Time) { EXPECT_EQ(time.nanosecs(), 0); // example nanoseconds since epoch - boost::int64_t epoch64 = 1500000000123456789; + int64_t epoch64 = 1500000000123456789; // ^ sec ^^ nano ^ // test epoch @@ -15,7 +15,7 @@ TEST(Steam, Time) { EXPECT_EQ(epoch.nanosecs(), 1500000000123456789); // test epoch - boost::int64_t epoch_secs64 = 1500000000e9; + int64_t epoch_secs64 = 1500000000e9; steam::Time epoch_secs = steam::Time(epoch_secs64); steam::Time epoch_nsecs = epoch - epoch_secs; EXPECT_EQ(epoch_nsecs.nanosecs(), 123456789); @@ -29,8 +29,8 @@ TEST(Steam, Time) { EXPECT_EQ(nano_back_to_time.nanosecs(), 123456789); // two 32-bits to 64-bit (e.g. ros::Time) - boost::int32_t secs32 = 1500000000; - boost::int32_t nsecs32 = 123456789; + int32_t secs32 = 1500000000; + int32_t nsecs32 = 123456789; EXPECT_EQ(epoch.nanosecs(), steam::Time(secs32, nsecs32).nanosecs()); }