diff --git a/.gitignore b/.gitignore index 1a57960199ca6913030d90819f96848498a900e9..d3c36fd6eacd20e7ab33c93d779abd1a10cf12f3 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ src/examples/map_polyline_example_write.yaml /CMakeCache.txt *.dat .DS_Store +laser.found *.graffle /Default/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 19ff2105a0d8891aa3dec67ba614fbc42b739232..2aff3964dc1f960767780c9e9571995b0cdac205 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON) # ============ DEPENDENCIES ============ FIND_PACKAGE(wolfcore REQUIRED CONFIG) +# message(STATUS ${wolfcore_DIR}) FIND_PACKAGE(laser_scan_utils REQUIRED CONFIG) # found only to set the XXX_FOUND variables # -> It would be better to look for them in the the laser_scan_utils target @@ -83,6 +84,12 @@ FIND_PACKAGE(falkolib QUIET) find_package(PkgConfig) pkg_check_modules(csm QUIET csm) # link_directories(${csm_LIBRARY_DIRS}) +find_package(PCL 1.10 COMPONENTS common registration io) +if (PCL_FOUND) +include_directories(${PCL_INCLUDE_DIRS} ) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) +endif(PCL_FOUND) # ============ CONFIG.H ============ set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) @@ -213,6 +220,44 @@ if(csm_FOUND) ) endif(csm_FOUND) +# PCL +if(PCL_FOUND) + # put here all files, HEADERS and SOURCES + # message("Found PCL. Compiling some extra classes.") + # message(STATUS "PCL DIRS:" ${PCL_DIR}) + # message(STATUS "PCL include DIRS:" ${PCL_INCLUDE_DIRS}) + SET(HDRS_CAPTURE + include/${PROJECT_NAME}/capture/capture_laser_3d.h + ) + SET(HDRS_SENSOR + include/${PROJECT_NAME}/sensor/sensor_laser_3d.h + ) + SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} + include/${PROJECT_NAME}/processor/processor_odom_icp_3d.h + ) + SET(SRCS_CAPTURE + src/capture/capture_laser_3d.cpp + ) + SET(SRCS_CAPTURE + src/capture/capture_laser_3d.cpp + ) + + +# SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} +# include/${PROJECT_NAME}/processor/processor_loop_closure_falko.h +# ) +# SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} +# src/processor/processor_loop_closure_falko.cpp +# ) +# SET(HDRS_FEATURE ${HDRS_FEATURE} +# include/${PROJECT_NAME}/feature/feature_scene_falko.h +# ) +# SET(SRCS_FEATURE ${SRCS_FEATURE} +# src/feature/feature_scene_falko.cpp +# ) +endif(PCL_FOUND) + + # create the shared library ADD_LIBRARY(${PLUGIN_NAME} SHARED @@ -244,6 +289,9 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} laser_scan_utils) IF (falkolib_FOUND) TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${falkolib_LIBRARY}) ENDIF(falkolib_FOUND) +IF (PCL_FOUND) + TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${PCL_LIBRARIES}) +ENDIF(PCL_FOUND) #Build tests #===============EXAMPLE========================= diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h index 5a8a4e38f1e453174cc32f35e820829435c3bd7c..a82c35e0d620a92b29cedd300908eb625df24f4c 100644 --- a/include/laser/capture/capture_laser_3d.h +++ b/include/laser/capture/capture_laser_3d.h @@ -18,14 +18,14 @@ typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; class CaptureLaser3d : public CaptureBase { public: - CaptureLaser3d(TimeStamp _timestamp, PointCloud::Ptr _point_cloud); + CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, PointCloud::Ptr _point_cloud); ~CaptureLaser3d(); PointCloud::Ptr getPointCloud() const; - + private: PointCloud::Ptr point_cloud_; -} +}; } // namespace laser } // namespace wolf diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h new file mode 100644 index 0000000000000000000000000000000000000000..d0eed4511121a98d4dfaf3588c43377242c373f8 --- /dev/null +++ b/include/laser/processor/processor_odom_icp_3d.h @@ -0,0 +1,134 @@ + +#ifndef SRC_PROCESSOR_ODOM_ICP_3D_H_ +#define SRC_PROCESSOR_ODOM_ICP_3D_H_ + +/************************** + * WOLF includes * + **************************/ +// #include "laser/processor/params_icp.h" //vsainz: parameters not specified yet +#include "laser/sensor/sensor_laser_3d.h" + +#include "core/common/wolf.h" +#include "core/processor/processor_tracker.h" +#include "core/processor/motion_provider.h" + +/************************** + * PCL includes * + **************************/ +#include <pcl/point_types.h> +#include <pcl/point_cloud.h> +#include <pcl/io/pcd_io.h> +#include <pcl/filters/voxel_grid.h> +#include <pcl/registration/icp.h> +#include <pcl/registration/icp_nl.h> +#include <pcl/registration/transforms.h> + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d); + +struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider +{ +}; // vsainz: empty, for now + +class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider +{ + protected: + // Useful sensor stuff + SensorLaser3dPtr sensor_laser_; // casted pointer to parent + + // ICP algorithm + // wip + + // temporary and carry-on transformations + + Eigen::VectorXd odom_origin_; + Eigen::VectorXd odom_last_; + Eigen::VectorXd odom_incoming_; + + Eigen::Isometry3d rl_T_sl_, ro_T_so_; + + public: + ParamsProcessorOdomIcp3dPtr params_odom_icp_; + + public: + ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params); + WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d); + ~ProcessorOdomIcp3d() override; + void configure(SensorBasePtr _sensor) override; + + VectorComposite getState(const StateStructure& _structure = "") const override; + TimeStamp getTimeStamp() const override; + VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override; + void setProblem(ProblemPtr _problem_ptr) override; + + Eigen::Vector3d getOriginLastTransform(double& dt) const; + + protected: + void preProcess() override; + + /** \brief Compute transform from origin to incoming. + * + * The transform is the result of aligning the scans in origin and incoming. + * This alignement takes as prior the current transform from origin to last. + * + * \return the number of features tracked. This is always zero since this processor is not tracking features. + */ + unsigned int processKnown() override; + /** \brief Compute transform from last to incoming. + * + * The transform is the result of aligning the scans in last and incoming. + * This alignement takes as prior the identity transform. + * + * \param _max_features number of features to extract. This is ignored since this processor is not extracting features. + * + * \return the number of features extracted. This is always zero since this processor is not extracting features. + */ + unsigned int processNew(const int& _max_features) override; + + bool voteForKeyFrame() const override; + + /** \brief advance pointers and update internal values + * + * We do several things: + * - Advance derived pointers + * - Advance isometries + * - Update extrinsic isometries in case they have changed + * - Compute odometry + */ + void advanceDerived() override; + /** \brief advance pointers and update internal values + * + * We do several things: + * - Reset derived pointers + * - Reset isometries + * - Update extrinsic isometries in case they have changed + * - Compute odometry + */ + void resetDerived() override; + + void establishFactors() override; + FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser); + FactorBasePtr emplaceFactor(FeatureBasePtr _feature); + + bool voteForKeyFrameDistance() const; + bool voteForKeyFrameAngle() const; + bool voteForKeyFrameTime() const; + bool voteForKeyFrameMatchQuality() const; + + template <typename D1, typename D2> + void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& p3, Eigen::QuaternionBase<D2>& q3) const; + + template <typename D1> + void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& x3) const; + Eigen::Isometry2d computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2) const; + + void updateExtrinsicsIsometries(); +}; + +} // namespace wolf + + +#endif // SRC_PROCESSOR_ODOM_ICP_3D_H_ diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h new file mode 100644 index 0000000000000000000000000000000000000000..496f5466b9a1283194f9bfefe0088061e370b8a3 --- /dev/null +++ b/include/laser/sensor/sensor_laser_3d.h @@ -0,0 +1,36 @@ +#ifndef SENSOR_LASER_3d_H_ +#define SENSOR_LASER_3d_H_ + +// WOLF includes +#include <core/sensor/sensor_base.h> + + +namespace wolf +{ +namespace laser +{ + +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d); + +struct ParamsSensorLaser3d : public ParamsSensorBase +{}; // Not yet defined + +WOLF_PTR_TYPEDEFS(SensorLaser3d); + + +class SensorLaser3d : public SensorBase +{ + public: + SensorLaser3d(StateBlockPtr p_ptr, StateBlockPtr _o_ptr); + ~SensorLaser3d(); +}; + + + +WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 0); + + +} // namespace laser +} // namespace wolf + +#endif // SENSOR_LASER_3d_H_ \ No newline at end of file diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h new file mode 100644 index 0000000000000000000000000000000000000000..5c82a908f0b51c75a31ca297f04187ab5172a8dc --- /dev/null +++ b/include/laser/utils/laser3d_tools.h @@ -0,0 +1,80 @@ + + +#include <pcl/point_types.h> +#include <pcl/point_cloud.h> +#include <pcl/common/transforms.h> +#include <pcl/io/pcd_io.h> + +#include <pcl/filters/voxel_grid.h> + +#include <pcl/registration/icp.h> +#include <pcl/registration/icp_nl.h> +#include <pcl/registration/transforms.h> + + +using PointCloud = pcl::PointCloud<pcl::PointXYZ>; +typedef boost::shared_ptr<PointCloud> PointCloudBoostPtr; +typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr; + +namespace wolf +{ +namespace laser +{ + +// convenient typedefs + +// _cloud_ref: first PointCloud +// _cloud_otherref: first PointCloud +inline void pairAlign(const PointCloudBoostConstPtr _cloud_ref, + const PointCloudBoostConstPtr _cloud_other, + const Eigen::Isometry3d &_transform_guess, + Eigen::Isometry3d &_transform_final, + pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> &_registration_solver, + bool _downsample = false) +{ + // Internal PointCloud pointers (boost::shared_ptr) + PointCloudBoostPtr cloud_ref = boost::make_shared<PointCloud>(); // std::make_shared<PoinCloud> cloud_ref; + PointCloudBoostPtr cloud_other = + boost::make_shared<PointCloud>(); // std::make_shared<PoinCloud> cloud_other(new PointCloud); + + // Downsample for consistency and speed + // \note enable this for large datasets + pcl::VoxelGrid<pcl::PointXYZ> grid; // downsample for performance + if (_downsample) + { + grid.setLeafSize(0.05, 0.05, 0.05); + // grid.setInputCloud(boost::static_pointer_cast<PointCloud>(_cloud_ref)); + grid.setInputCloud(_cloud_ref); + grid.filter(*cloud_ref); + + grid.setInputCloud(_cloud_other); + grid.filter(*cloud_other); + } + else + { + *cloud_ref = *_cloud_ref; + *cloud_other = *_cloud_other; + } + + // Set input clouds + _registration_solver.setInputSource(cloud_ref); + _registration_solver.setInputTarget(cloud_other); + + // Declare variables + Eigen::Matrix4f transform_pcl; + + // Change to float + Eigen::Matrix4f transform_guess = _transform_guess.matrix().cast<float>(); + + // Alignment + _registration_solver.align(*cloud_ref, transform_guess); + transform_pcl = _registration_solver.getFinalTransformation(); + + // Output the transformation as Isometry of double(s) + _transform_final = Eigen::Isometry3d(transform_pcl.cast<double>()); + + // _transform_final = _transform_final.inverse(); // Maybe we messed up +} + +} // namespace laser +} // namespace wolf diff --git a/src/sensor/sensor_laser_3d.cpp b/src/sensor/sensor_laser_3d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e70bcd0b4785d421c9b9b4677a3ff6c25ce255bb --- /dev/null +++ b/src/sensor/sensor_laser_3d.cpp @@ -0,0 +1,15 @@ +#include "laser/sensor/sensor_laser_3d.h" + +namespace wolf +{ +namespace laser +{ + +SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : +SensorBase("SensorLaser3d", _p_ptr, _o_ptr, nullptr, 0) {} +// vsainz: Parameters should be set somewhere, not yet!! + +SensorLaser3d::~SensorLaser3d() {} + +} // namespace laser +} // namespace wolf \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1d3f41289deb112f091202b31669f289598b7832..c64c1d39019e0dde7e7ff9e6dac6e544c649f7b4 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -24,3 +24,7 @@ if(falkolib_FOUND) wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp) endif(csm_FOUND) endif(falkolib_FOUND) + +if(PCL_FOUND) + wolf_add_gtest(gtest_laser3d_tools gtest_laser3d_tools.cpp) +endif(PCL_FOUND) \ No newline at end of file diff --git a/test/data/1.pcd b/test/data/1.pcd new file mode 100644 index 0000000000000000000000000000000000000000..9b79b908a6430311c645e014e0b8174088601cdd Binary files /dev/null and b/test/data/1.pcd differ diff --git a/test/data/2.pcd b/test/data/2.pcd new file mode 100644 index 0000000000000000000000000000000000000000..575b487f86c7234f2370fc4e3c210cdca9f732ad Binary files /dev/null and b/test/data/2.pcd differ diff --git a/test/gtest_laser3d_tools.cpp b/test/gtest_laser3d_tools.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eb9c5f2d8792f16eebf1c27bffda629f433b9fdf --- /dev/null +++ b/test/gtest_laser3d_tools.cpp @@ -0,0 +1,129 @@ + +// wolf laser +#include "laser/utils/laser3d_tools.h" +#include "laser/internal/config.h" + +// //Wolf +#include "core/common/wolf.h" +// #include "core/math/rotations.h" +#include "core/utils/utils_gtest.h" + +// Eigen +#include <Eigen/Geometry> + +// //std +#include <iostream> +#include <string> +#include <filesystem> +#include <unistd.h> +// #include <iostream> +// #include <fstream> +// #include <iomanip> + +//#define write_results + +////// vsainz: i have to write a test + +// using namespace wolf + +// using namespace Eigenç + +std::string laser_root_dir = _WOLF_LASER_ROOT_DIR; + + + +void loadData(std::string fname, PointCloud& cloud) +{ + pcl::io::loadPCDFile(fname, cloud); + // remove NAN points from the cloud + pcl::Indices indices; + pcl::removeNaNFromPointCloud(cloud, cloud, indices); +}; + +TEST(laser3d_tools, pairAlign_identity) +{ + // typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; + using PointCloud = pcl::PointCloud<pcl::PointXYZ>; + + // Load data + PointCloudBoostPtr pcl_ref = boost::make_shared<PointCloud>(); + loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref); + + PointCloudBoostPtr pcl_other = pcl_ref; + // loadData("data/2.pcd", pcl_other); + + // Guess + Eigen::Isometry3d transformation_guess = Eigen::Translation3d(1,1,1) * Eigen::AngleAxisd(0.1,Eigen::Vector3d(1,2,3).normalized()); + WOLF_INFO("Initial guess transform: \n", transformation_guess.matrix()); + + // final transform + Eigen::Isometry3d transformation_final; + + // Solver configuration + pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver; + registration_solver.setTransformationEpsilon(1e-6); + // Set the maximum distance between two correspondences (src<->tgt) to 10cm + // Note: adjust this based on the size of your datasets + registration_solver.setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm + // Set the point representation + // reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation)); + registration_solver.setMaximumIterations(200); + + // Alignment + wolf::laser::pairAlign(pcl_ref, pcl_other, transformation_guess, transformation_final, registration_solver, true); //false); + + WOLF_INFO("Final transform: \n", transformation_final.matrix()); + + ASSERT_MATRIX_APPROX(transformation_final.matrix(), Eigen::Matrix4d::Identity(), 1e-2); +} + +TEST(laser3d_tools, pairAlign_known_transformation) +{ + // typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; + using PointCloud = pcl::PointCloud<pcl::PointXYZ>; + + // Load data + PointCloudBoostPtr pcl_ref = boost::make_shared<PointCloud>(); + loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref); + + // known transform + Eigen::Affine3d transformation_known = Eigen::Affine3d::Identity(); + transformation_known.translation() << 1.0, 0.5, 0.2; + transformation_known.rotate (Eigen::AngleAxisd (0.3, Eigen::Vector3d::UnitZ())); + + // Move point cloud + PointCloudBoostPtr pcl_other = boost::make_shared<PointCloud>(); + pcl::transformPointCloud (*pcl_ref, *pcl_other, transformation_known); + + // Guess + Eigen::Isometry3d transformation_guess = Eigen::Isometry3d::Identity(); + + // final transform + Eigen::Isometry3d transformation_final; + WOLF_INFO("Applied transformation: \n", transformation_known.matrix()); + + // Solver configuration + pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver; + registration_solver.setTransformationEpsilon(1e-6); + // Set the maximum distance between two correspondences (src<->tgt) to 10cm + // Note: adjust this based on the size of your datasets + registration_solver.setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm + // Set the point representation + // reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation)); + registration_solver.setMaximumIterations(200); + + // Alignment + wolf::laser::pairAlign(pcl_ref, pcl_other, transformation_guess, transformation_final, registration_solver, true); //false); + + WOLF_INFO("Final transform: \n", transformation_final.matrix()); + + ASSERT_MATRIX_APPROX(transformation_final.matrix(), transformation_known.matrix(), 1e-2); +} + + +int main(int argc, char **argv) + { + testing::InitGoogleTest(&argc, argv); + // ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q"; + return RUN_ALL_TESTS(); +}