From 1545c14bc3fa74a87f69bec9dd2f1d990c4a8631 Mon Sep 17 00:00:00 2001 From: Yuchen Wu Date: Fri, 25 Jun 2021 22:53:51 -0400 Subject: [PATCH 1/4] USE_AMENT default to ON for vtr --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fe837ff..a08e288 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.16) project(steam) -option(USE_AMENT "Use ament_cmake to build lgmath for ROS2." OFF) +option(USE_AMENT "Use ament_cmake to build lgmath for ROS2." ON) # Compiler setup (assumed to be GNU) set(CMAKE_CXX_STANDARD 17) From 3c4c94fa7696d57c63ee78d8144839a7c97d1e7e Mon Sep 17 00:00:00 2001 From: Yuchen Wu Date: Wed, 30 Jun 2021 15:39:30 -0400 Subject: [PATCH 2/4] vtr lidar point 2 point error eval --- include/steam.hpp | 1 + .../samples/PointToPointErrorEval2.hpp | 69 ++++++++++++++++++ .../samples/PointToPointErrorEval2.cpp | 72 +++++++++++++++++++ 3 files changed, 142 insertions(+) create mode 100644 include/steam/evaluator/samples/PointToPointErrorEval2.hpp create mode 100644 src/evaluator/samples/PointToPointErrorEval2.cpp 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/evaluator/samples/PointToPointErrorEval2.hpp b/include/steam/evaluator/samples/PointToPointErrorEval2.hpp new file mode 100644 index 0000000..da4e197 --- /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 boost::shared_ptr Ptr; + typedef boost::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/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 From 6ea5aa7903b2010b6c48a445d987c2f3b6682c35 Mon Sep 17 00:00:00 2001 From: Yuchen Wu Date: Fri, 2 Jul 2021 02:56:41 -0400 Subject: [PATCH 3/4] add a pc alignment example --- CMakeLists.txt | 32 +--------- README.md | 2 + include/steam/problem/NoiseModel-inl.hpp | 2 +- samples/SimplePointCloudAlignment.cpp | 76 ++++++++++++++++++++++++ 4 files changed, 82 insertions(+), 30 deletions(-) create mode 100644 samples/SimplePointCloudAlignment.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a08e288..8efe867 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,47 +136,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}) - -install( - DIRECTORY include/ - DESTINATION include -) - +add_executable(SimplePointCloudAlignment samples/SimplePointCloudAlignment.cpp) if (USE_AMENT) install( @@ -194,6 +167,7 @@ install( TrustRegionExample MotionDistortedP2PandCATrajPrior SimpleP2PandCATrajPrior + SimplePointCloudAlignment LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} diff --git a/README.md b/README.md index 36efbc3..ab3a8f5 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,8 @@ STEAM (Simultaneous Trajectory Estimation and Mapping) Engine is an optimization [1] Barfoot, T. D. and Furgale, P. T., “_Associating Uncertainty with Three-Dimensional Poses for use in Estimation Problems_,” IEEE Transactions on Robotics, 2014. +**Note**: This branch is for VTR3, and the `USE_AMENT` flag is `ON` by default. + ## Installation ### Hardware and Software Requirements diff --git a/include/steam/problem/NoiseModel-inl.hpp b/include/steam/problem/NoiseModel-inl.hpp index a82d8c7..cfa91cb 100644 --- a/include/steam/problem/NoiseModel-inl.hpp +++ b/include/steam/problem/NoiseModel-inl.hpp @@ -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/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 From 0015f6cd45bca21f53c3777883a0264bdffb21eb Mon Sep 17 00:00:00 2001 From: Yuchen Wu Date: Thu, 12 Aug 2021 01:01:32 -0400 Subject: [PATCH 4/4] cmakelist ament export dependencies includes eigen --- CMakeLists.txt | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8efe867..e6fa8f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -107,15 +107,9 @@ target_include_directories(${PROJECT_NAME} $ $) - 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(Boost OpenMP Eigen3 lgmath) install( DIRECTORY include/ @@ -123,9 +117,7 @@ install( ) install( - TARGETS - # Libraries - ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib