-
Notifications
You must be signed in to change notification settings - Fork 14
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #58 from utiasASRL/ros2-dev
Get latest changes from ros2-vtr3 branch
- Loading branch information
Showing
7 changed files
with
228 additions
and
34 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
69 changes: 69 additions & 0 deletions
69
include/steam/evaluator/samples/PointToPointErrorEval2.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <steam.hpp> | ||
|
|
||
| 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<PointToPointErrorEval2> Ptr; | ||
| typedef boost::shared_ptr<const PointToPointErrorEval2> 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<double, 3, 1> evaluate() const override; | ||
|
|
||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| /// \brief Evaluate the 3-d measurement error (x, y, z) and Jacobians | ||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| virtual Eigen::Matrix<double, 3, 1> evaluate( | ||
| const Eigen::Matrix<double, 3, 3> &lhs, | ||
| std::vector<Jacobian<3, 6>> *jacs) const; | ||
|
|
||
| private: | ||
| se3::TransformEvaluator::ConstPtr T_rq_; | ||
|
|
||
| Eigen::Matrix<double, 3, 4> D_ = Eigen::Matrix<double, 3, 4>::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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,76 @@ | ||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| /// \file SimplePointCloudAlignment.cpp | ||
| /// \brief | ||
| /// | ||
| /// \author Yuchen Wu, ASRL | ||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
|
|
||
| #include <iostream> | ||
|
|
||
| #include <lgmath.hpp> | ||
| #include <steam.hpp> | ||
|
|
||
| int main(int argc, char **argv) { | ||
| /// Reference points to align against | ||
| Eigen::Matrix<double, 4, 9> 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<double, 6, 1> T_mq_vec; | ||
| T_mq_vec << 1, 1, 1, 1, 1, 1; | ||
| lgmath::se3::Transformation T_mq(T_mq_vec); | ||
|
|
||
| Eigen::Matrix<double, 4, 9> 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; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <steam/evaluator/samples/PointToPointErrorEval2.hpp> | ||
|
|
||
| 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<double, 3, 1> PointToPointErrorEval2::evaluate() const { | ||
| return D_ * (reference_ - T_rq_->evaluate() * query_); | ||
| } | ||
|
|
||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| /// \brief Evaluate the 3-d measurement error (x, y, z) and Jacobians | ||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| Eigen::Matrix<double, 3, 1> PointToPointErrorEval2::evaluate( | ||
| const Eigen::Matrix<double, 3, 3> &lhs, | ||
| std::vector<Jacobian<3, 6>> *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<lgmath::se3::Transformation> blkAutoEvalPosOfTransformDiff = | ||
| T_rq_->getBlockAutomaticEvaluation(); | ||
|
|
||
| // Get evaluation from tree | ||
| const lgmath::se3::Transformation T_rq = | ||
| blkAutoEvalPosOfTransformDiff.getValue(); | ||
| Eigen::Matrix<double, 3, 1> error = D_ * (reference_ - T_rq * query_); | ||
|
|
||
| // Get Jacobians | ||
| const Eigen::Matrix<double, 3, 1> Tq = (T_rq * query_).block<3, 1>(0, 0); | ||
| const Eigen::Matrix<double, 3, 6> new_lhs = | ||
| -lhs * D_ * lgmath::se3::point2fs(Tq); | ||
| T_rq_->appendBlockAutomaticJacobians( | ||
| new_lhs, blkAutoEvalPosOfTransformDiff.getRoot(), jacs); | ||
|
|
||
| // Return evaluation | ||
| return error; | ||
| } | ||
|
|
||
| } // namespace steam |