Skip to content
Snippets Groups Projects
Commit fe996879 authored by Víctor Sainz Ubide's avatar Víctor Sainz Ubide
Browse files

new tests for laser3d_tools.h

parent 0bf8a103
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
...@@ -23,6 +23,7 @@ src/examples/map_polyline_example_write.yaml ...@@ -23,6 +23,7 @@ src/examples/map_polyline_example_write.yaml
/CMakeCache.txt /CMakeCache.txt
*.dat *.dat
.DS_Store .DS_Store
laser.found
*.graffle *.graffle
/Default/ /Default/
......
...@@ -76,6 +76,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON) ...@@ -76,6 +76,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============ # ============ DEPENDENCIES ============
FIND_PACKAGE(wolfcore REQUIRED CONFIG) FIND_PACKAGE(wolfcore REQUIRED CONFIG)
# message(STATUS ${wolfcore_DIR})
FIND_PACKAGE(laser_scan_utils REQUIRED CONFIG) FIND_PACKAGE(laser_scan_utils REQUIRED CONFIG)
# found only to set the XXX_FOUND variables # found only to set the XXX_FOUND variables
# -> It would be better to look for them in the the laser_scan_utils target # -> It would be better to look for them in the the laser_scan_utils target
...@@ -83,6 +84,12 @@ FIND_PACKAGE(falkolib QUIET) ...@@ -83,6 +84,12 @@ FIND_PACKAGE(falkolib QUIET)
find_package(PkgConfig) find_package(PkgConfig)
pkg_check_modules(csm QUIET csm) pkg_check_modules(csm QUIET csm)
# link_directories(${csm_LIBRARY_DIRS}) # 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 ============ # ============ CONFIG.H ============
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
...@@ -213,6 +220,44 @@ if(csm_FOUND) ...@@ -213,6 +220,44 @@ if(csm_FOUND)
) )
endif(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 # create the shared library
ADD_LIBRARY(${PLUGIN_NAME} ADD_LIBRARY(${PLUGIN_NAME}
SHARED SHARED
...@@ -244,6 +289,9 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} laser_scan_utils) ...@@ -244,6 +289,9 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} laser_scan_utils)
IF (falkolib_FOUND) IF (falkolib_FOUND)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${falkolib_LIBRARY}) TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${falkolib_LIBRARY})
ENDIF(falkolib_FOUND) ENDIF(falkolib_FOUND)
IF (PCL_FOUND)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${PCL_LIBRARIES})
ENDIF(PCL_FOUND)
#Build tests #Build tests
#===============EXAMPLE========================= #===============EXAMPLE=========================
......
...@@ -18,14 +18,14 @@ typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; ...@@ -18,14 +18,14 @@ typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
class CaptureLaser3d : public CaptureBase class CaptureLaser3d : public CaptureBase
{ {
public: public:
CaptureLaser3d(TimeStamp _timestamp, PointCloud::Ptr _point_cloud); CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, PointCloud::Ptr _point_cloud);
~CaptureLaser3d(); ~CaptureLaser3d();
PointCloud::Ptr getPointCloud() const; PointCloud::Ptr getPointCloud() const;
private: private:
PointCloud::Ptr point_cloud_; PointCloud::Ptr point_cloud_;
} };
} // namespace laser } // namespace laser
} // namespace wolf } // namespace wolf
......
#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_
#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
#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
#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
...@@ -24,3 +24,7 @@ if(falkolib_FOUND) ...@@ -24,3 +24,7 @@ if(falkolib_FOUND)
wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp) wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp)
endif(csm_FOUND) endif(csm_FOUND)
endif(falkolib_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
File added
File added
// 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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment