Egomotion from lidar main source code. More...
#include "lidar_egomotion.h"#include <iostream>#include <fstream>#include <features.h>#include <bits/wordsize.h>#include <gnu/stubs-32.h>#include <stddef.h>#include <bits/types.h>#include <time.h>#include <string>#include <cmath>#include <stdexcept>#include "duration.h"#include <cstddef>#include <utility>#include <boost/config.hpp>#include <limits>#include <limits.h>#include <algorithm>#include <climits>#include <iomanip>#include <cerrno>#include <boost/config/no_tr1/cmath.hpp>#include <boost/math/tools/config.hpp>#include <boost/detail/workaround.hpp>#include <boost/mpl/aux_/config/gcc.hpp>#include <boost/mpl/aux_/config/workaround.hpp>#include <boost/mpl/aux_/adl_barrier.hpp>#include <boost/mpl/aux_/config/static_constant.hpp>#include <boost/mpl/aux_/config/msvc.hpp>#include <boost/preprocessor/config/config.hpp>#include <boost/mpl/aux_/config/ctps.hpp>#include <boost/mpl/aux_/config/preprocessor.hpp>#include <boost/preprocessor/cat.hpp>#include <boost/preprocessor/stringize.hpp>#include <boost/mpl/bool.hpp>#include <boost/mpl/has_xxx.hpp>#include <boost/mpl/aux_/na.hpp>#include <boost/mpl/aux_/config/ttp.hpp>#include <boost/mpl/aux_/nttp_decl.hpp>#include <boost/mpl/integral_c_tag.hpp>#include <boost/mpl/int.hpp>#include <boost/mpl/aux_/config/dtp.hpp>#include <boost/mpl/aux_/template_arity_fwd.hpp>#include <boost/preprocessor/logical/bool.hpp>#include <boost/preprocessor/facilities/empty.hpp>#include <boost/mpl/aux_/lambda_arity_param.hpp>#include <boost/mpl/aux_/config/lambda.hpp>#include <boost/mpl/aux_/config/overload_resolution.hpp>#include <boost/mpl/aux_/yes_no.hpp>#include <boost/mpl/aux_/config/eti.hpp>#include <boost/mpl/contains_fwd.hpp>#include <boost/mpl/begin_end_fwd.hpp>#include <boost/mpl/sequence_tag_fwd.hpp>#include <boost/mpl/void_fwd.hpp>#include <boost/mpl/aux_/na_spec.hpp>#include <boost/mpl/aux_/static_cast.hpp>#include <boost/mpl/aux_/config/integral.hpp>#include <boost/mpl/aux_/lambda_support.hpp>#include <boost/mpl/aux_/has_begin.hpp>#include <boost/mpl/void.hpp>#include <boost/mpl/aux_/preprocessor/params.hpp>#include <boost/mpl/sequence_tag.hpp>#include <boost/mpl/aux_/config/use_preprocessed.hpp>#include <boost/mpl/aux_/include_preprocessed.hpp>#include <boost/mpl/aux_/arity.hpp>#include <boost/mpl/aux_/config/has_xxx.hpp>#include <boost/mpl/aux_/value_wknd.hpp>#include <boost/mpl/aux_/nested_type_wknd.hpp>#include <boost/mpl/aux_/config/nttp.hpp>#include <boost/mpl/limits/arity.hpp>#include <boost/mpl/lambda_fwd.hpp>#include <boost/mpl/placeholders.hpp>#include <boost/mpl/apply_wrap.hpp>#include <boost/mpl/aux_/arity_spec.hpp>#include <boost/mpl/aux_/type_wrapper.hpp>#include <boost/mpl/aux_/config/bind.hpp>#include <boost/mpl/bind_fwd.hpp>#include <boost/mpl/protect.hpp>#include <boost/mpl/arg.hpp>#include <boost/mpl/int_fwd.hpp>#include <boost/mpl/aux_/is_msvc_eti_arg.hpp>#include <boost/mpl/not.hpp>#include <boost/mpl/begin_end.hpp>#include <boost/mpl/aux_/preprocessor/def_params_tail.hpp>#include <boost/mpl/eval_if.hpp>#include <boost/mpl/if.hpp>#include <boost/mpl/apply.hpp>#include <boost/mpl/next.hpp>#include <boost/mpl/aux_/integral_wrapper.hpp>#include <boost/mpl/aux_/common_name_wknd.hpp>#include <boost/mpl/aux_/config/forwarding.hpp>#include <boost/type_traits/is_same.hpp>#include <boost/mpl/aux_/traits_lambda_spec.hpp>#include <boost/mpl/find_if.hpp>#include <boost/mpl/O1_size_fwd.hpp>#include <boost/mpl/next_prior.hpp>#include <boost/mpl/deref.hpp>#include <boost/mpl/O1_size.hpp>#include <boost/mpl/identity.hpp>#include <boost/mpl/lambda.hpp>#include <boost/mpl/push_back_fwd.hpp>#include <boost/mpl/assert.hpp>#include <boost/mpl/aux_/has_type.hpp>#include <boost/mpl/push_front_fwd.hpp>#include <boost/mpl/inserter.hpp>#include <boost/mpl/push_back.hpp>#include <boost/mpl/push_front.hpp>#include <boost/mpl/back_inserter.hpp>#include <boost/mpl/front_inserter.hpp>#include <boost/mpl/clear_fwd.hpp>#include <boost/preprocessor/inc.hpp>#include <boost/mpl/aux_/has_tag.hpp>#include <boost/mpl/numeric_cast.hpp>#include <boost/mpl/aux_/msvc_eti_base.hpp>#include <boost/mpl/integral_c.hpp>#include <boost/mpl/long.hpp>#include <boost/mpl/tag.hpp>#include <boost/mpl/size_fwd.hpp>#include <boost/mpl/aux_/comparison_op.hpp>#include <boost/mpl/less.hpp>#include <stdlib.h>#include <endian.h>#include <bits/mathcalls.h>#include <boost/limits.hpp>#include <boost/assert.hpp>#include <boost/static_assert.hpp>#include <boost/math/policies/policy.hpp>#include <math.h>#include <boost/cstdint.hpp>#include <vector>#include <locale>#include <streambuf>#include <ostream>#include <iosfwd>#include <boost/format/detail/compat_workarounds.hpp>#include <boost/format/format_fwd.hpp>#include <new>#include <boost/type_traits/config.hpp>#include <boost/type_traits/detail/template_arity_spec.hpp>#include <boost/type_traits/integral_constant.hpp>#include <boost/preprocessor/arithmetic/inc.hpp>#include <boost/preprocessor/control/iif.hpp>#include <boost/preprocessor/tuple/eat.hpp>#include <boost/preprocessor/debug/error.hpp>#include <boost/preprocessor/detail/auto_rec.hpp>#include <boost/preprocessor/control/while.hpp>#include <boost/preprocessor/control/expr_iif.hpp>#include <boost/preprocessor/list/adt.hpp>#include <boost/preprocessor/list/fold_right.hpp>#include <boost/preprocessor/logical/bitand.hpp>#include <boost/preprocessor/list/fold_left.hpp>#include <boost/preprocessor/tuple/elem.hpp>#include <boost/preprocessor/tuple/rem.hpp>#include <boost/type_traits/alignment_of.hpp>#include <boost/type_traits/detail/bool_trait_def.hpp>#include <boost/type_traits/detail/bool_trait_undef.hpp>#include <boost/type_traits/intrinsics.hpp>#include <boost/type_traits/is_arithmetic.hpp>#include <boost/type_traits/is_reference.hpp>#include <boost/type_traits/add_reference.hpp>#include <boost/type_traits/detail/yes_no_type.hpp>#include <boost/type_traits/detail/ice_or.hpp>#include <boost/type_traits/is_void.hpp>#include <boost/type_traits/detail/type_trait_def.hpp>#include <boost/type_traits/detail/type_trait_undef.hpp>#include <boost/type_traits/detail/ice_and.hpp>#include <boost/type_traits/detail/ice_not.hpp>#include <boost/type_traits/is_array.hpp>#include <boost/type_traits/is_class.hpp>#include <boost/type_traits/is_union.hpp>#include <boost/type_traits/remove_cv.hpp>#include <boost/type_traits/is_member_pointer.hpp>#include <boost/type_traits/broken_compiler_spec.hpp>#include <boost/type_traits/detail/cv_traits_impl.hpp>#include <boost/type_traits/is_pod.hpp>#include <functional>#include <boost/preprocessor/punctuation/comma_if.hpp>#include <boost/preprocessor/repetition/repeat.hpp>#include <boost/preprocessor/arithmetic/dec.hpp>#include <memory>#include <exception>#include <boost/checked_delete.hpp>#include <boost/throw_exception.hpp>#include <typeinfo>#include <boost/smart_ptr/detail/sp_counted_base.hpp>#include <boost/config/no_tr1/memory.hpp>#include <boost/smart_ptr/detail/sp_has_sync.hpp>#include <boost/optional.hpp>#include <boost/format/internals_fwd.hpp>#include <boost/format/internals.hpp>#include <boost/format/alt_sstream.hpp>#include <boost/format/format_class.hpp>#include <boost/format/group.hpp>#include <boost/format/exceptions.hpp>#include <boost/math/special_functions/detail/round_fwd.hpp>#include <boost/type_traits/is_floating_point.hpp>#include <boost/type_traits/is_integral.hpp>#include <boost/type_traits/is_convertible.hpp>#include <boost/mpl/and.hpp>#include <boost/mpl/or.hpp>#include <boost/mpl/comparison.hpp>#include <complex>#include <cstring>#include "rostime_decl.h"#include <bits/time.h>#include <sys/select.h>#include "ros/time.h"#include <cstdio>#include <sstream>#include <cstdarg>#include <log4cxx/logstring.h>#include <log4cxx/log4cxx.h>#include <log4cxx/helpers/objectptr.h>#include <map>#include <log4cxx/helpers/object.h>#include <log4cxx/helpers/objectimpl.h>#include <log4cxx/helpers/pool.h>#include <log4cxx/helpers/mutex.h>#include <log4cxx/appender.h>#include <log4cxx/logger.h>#include <log4cxx/level.h>#include "ros/console.h"#include <ros/platform.h>#include <assert.h>#include "ros/assert.h"#include <set>#include <list>#include <boost/shared_ptr.hpp>#include <boost/smart_ptr/detail/shared_count.hpp>#include <boost/smart_ptr/shared_ptr.hpp>#include <boost/preprocessor/array/size.hpp>#include <cassert>#include <stdarg.h>#include <bits/wchar.h>#include <boost/type_traits/has_trivial_copy.hpp>#include <boost/type_traits/is_volatile.hpp>#include <boost/type_traits/is_enum.hpp>#include <boost/type_traits/is_member_function_pointer.hpp>#include <boost/type_traits/is_pointer.hpp>#include <boost/type_traits/ice.hpp>#include <boost/preprocessor/repetition/enum_params.hpp>#include <ros/macros.h>#include "roscpp_serialization_macros.h"#include <boost/smart_ptr/detail/operator_bool.hpp>#include <boost/shared_array.hpp>#include <stdint.h>#include "ros/forwards.h"#include "ros/common.h"#include <xlocale.h>#include <iterator>#include <ros/types.h>#include "serialized_message.h"#include "message_forward.h"#include <boost/utility/enable_if.hpp>#include <boost/type_traits/remove_reference.hpp>#include "message_traits.h"#include "ros/exception.h"#include <boost/array.hpp>#include <boost/ref.hpp>#include <boost/mem_fn.hpp>#include <boost/type.hpp>#include <boost/is_placeholder.hpp>#include <boost/bind/arg.hpp>#include "common.h"#include <ros/message_traits.h>#include <boost/type_traits/is_const.hpp>#include <boost/type_traits/remove_const.hpp>#include <boost/function.hpp>#include <boost/type_traits/type_with_alignment.hpp>#include <ros/static_assert.h>#include <boost/type_traits/add_const.hpp>#include "ros/builtin_message_traits.h"#include "ros/serialization.h"#include "ros/message_event.h"#include <boost/type_traits/is_base_of.hpp>#include <boost/make_shared.hpp>#include "forwards.h"#include "timer_options.h"#include "ros/rate.h"#include "wall_timer_options.h"#include "ros/message.h"#include "ros/service_traits.h"#include <istream>#include <boost/call_traits.hpp>#include <ios>#include <boost/integer_traits.hpp>#include <boost/type_traits/is_abstract.hpp>#include "subscription_callback_helper.h"#include "ros/spinner.h"#include <boost/bind.hpp>#include "ros/publisher.h"#include <boost/utility/addressof.hpp>#include <boost/utility/base_from_member.hpp>#include <boost/preprocessor/facilities/identity.hpp>#include <boost/preprocessor/control/if.hpp>#include <boost/preprocessor/seq/seq.hpp>#include <boost/preprocessor/seq/size.hpp>#include <boost/preprocessor/seq/fold_left.hpp>#include <boost/preprocessor/arithmetic/sub.hpp>#include <boost/preprocessor/logical/compl.hpp>#include "ros/service_server.h"#include "ros/subscriber.h"#include "ros/node_handle.h"#include "ros/init.h"#include "XmlRpcValue.h"#include "node_handle.h"#include "ros/names.h"#include <boost/type_traits/is_base_and_derived.hpp>#include <boost/mpl/equal_to.hpp>#include <boost/mpl/greater.hpp>#include <boost/serialization/level.hpp>#include <boost/serialization/level_enum.hpp>#include <boost/serialization/tracking_enum.hpp>#include <boost/serialization/access.hpp>#include <boost/serialization/force_include.hpp>#include <boost/serialization/traits.hpp>#include <boost/serialization/split_member.hpp>#include <boost/serialization/wrapper.hpp>#include <boost/mpl/always.hpp>#include <boost/serialization/nvp.hpp>#include <boost/noncopyable.hpp>#include <boost/numeric/ublas/detail/config.hpp>#include <boost/numeric/ublas/exception.hpp>#include <boost/mpl/at_fwd.hpp>#include <boost/mpl/aux_/config/typeof.hpp>#include <boost/mpl/vector/aux_/at.hpp>#include <boost/mpl/vector/aux_/tag.hpp>#include <boost/mpl/aux_/numeric_op.hpp>#include <boost/mpl/aux_/arithmetic_op.hpp>#include <boost/mpl/advance_fwd.hpp>#include <boost/mpl/distance_fwd.hpp>#include <boost/mpl/prior.hpp>#include <boost/mpl/minus.hpp>#include <boost/mpl/vector/aux_/O1_size.hpp>#include <boost/mpl/vector/aux_/iterator.hpp>#include <boost/mpl/vector/aux_/vector0.hpp>#include <boost/mpl/vector/aux_/include_preprocessed.hpp>#include <boost/mpl/at.hpp>#include "boost/type_traits/add_volatile.hpp"#include "boost/type_traits/has_nothrow_copy.hpp"#include <boost/type_traits/has_trivial_destructor.hpp>#include "boost/type_traits/has_trivial_assign.hpp"#include "boost/type_traits/has_trivial_constructor.hpp"#include "boost/type_traits/is_signed.hpp"#include "boost/type_traits/is_unsigned.hpp"#include <boost/type_traits/is_fundamental.hpp>#include "boost/type_traits/is_float.hpp"#include <boost/type_traits/is_function.hpp>#include "boost/type_traits/is_polymorphic.hpp"#include "boost/type_traits/is_scalar.hpp"#include <boost/type_traits/is_empty.hpp>#include <boost/type_traits/detail/size_t_trait_def.hpp>#include <boost/type_traits/detail/size_t_trait_undef.hpp>#include <boost/type_traits/add_pointer.hpp>#include <boost/type_traits/integral_promotion.hpp>#include <boost/type_traits/floating_point_promotion.hpp>#include <boost/type_traits/make_unsigned.hpp>#include <boost/type_traits/remove_bounds.hpp>#include <boost/numeric/ublas/detail/iterator.hpp>#include <boost/numeric/ublas/traits.hpp>#include <boost/numeric/ublas/detail/definitions.hpp>#include <boost/numeric/ublas/functional.hpp>#include <boost/serialization/collection_size_type.hpp>#include <boost/numeric/ublas/vector_expression.hpp>#include <boost/serialization/array.hpp>#include <boost/thread/mutex.hpp>#include "ros/message_operations.h"#include "std_msgs/Header.h"#include "geometry_msgs/Vector3.h"#include "geometry_msgs/Quaternion.h"#include "geometry_msgs/Point.h"#include "btMatrix3x3.h"#include "tf/exceptions.h"#include "LinearMath/btTransform.h"#include <boost/functional/hash/hash_fwd.hpp>#include <boost/functional/hash/detail/float_functions.hpp>#include <boost/integer/static_log2.hpp>#include <boost/functional/hash/detail/limits.hpp>#include <boost/functional/hash/hash.hpp>#include <boost/iterator.hpp>#include <boost/detail/iterator.hpp>#include <boost/type_traits/aligned_storage.hpp>#include <boost/utility/swap.hpp>#include <boost/preprocessor/repetition/enum_binary_params.hpp>#include <boost/preprocessor/repetition/repeat_from_to.hpp>#include <boost/unordered/detail/hash_table_impl.hpp>#include <boost/unordered/detail/config.hpp>#include <initializer_list>#include <boost/type_traits/function_traits.hpp>#include <boost/weak_ptr.hpp>#include <boost/smart_ptr/detail/sp_convertible.hpp>#include <boost/config/no_tr1/functional.hpp>#include <boost/smart_ptr/weak_ptr.hpp>#include <boost/operators.hpp>#include <boost/utility.hpp>#include <boost/signals/detail/signals_common.hpp>#include <boost/signals/connection.hpp>#include <boost/type_traits.hpp>#include <boost/visit_each.hpp>#include <boost/signals/detail/config.hpp>#include <boost/function/detail/prologue.hpp>#include <boost/iterator/detail/config_def.hpp>#include <boost/iterator/detail/config_undef.hpp>#include <boost/iterator/iterator_categories.hpp>#include <boost/type_traits/remove_pointer.hpp>#include <boost/signals/trackable.hpp>#include <boost/signals/slot.hpp>#include <boost/smart_ptr.hpp>#include <boost/function/function2.hpp>#include <boost/iterator/iterator_facade.hpp>#include <boost/signals/signal_template.hpp>#include "sensor_msgs/PointCloud.h"#include "src/Core/util/DisableStupidWarnings.h"#include "src/Core/util/Macros.h"#include <cstdlib>#include "src/Core/util/Constants.h"#include "src/Core/util/ForwardDeclarations.h"#include "src/Core/util/Meta.h"#include "src/Core/util/XprHelper.h"#include "src/Core/util/StaticAssert.h"#include "src/Core/util/Memory.h"#include "src/Core/NumTraits.h"#include "src/Core/MathFunctions.h"#include "src/Core/GenericPacketMath.h"#include "src/Core/arch/Default/Settings.h"#include "src/Core/Functors.h"#include "src/Core/DenseCoeffsBase.h"#include "src/Core/DenseBase.h"#include "src/Core/MatrixBase.h"#include "src/Core/EigenBase.h"#include "src/Core/Assign.h"#include "src/Core/util/BlasUtil.h"#include "src/Core/DenseStorage.h"#include "src/Core/NestByValue.h"#include "src/Core/ForceAlignedAccess.h"#include "src/Core/ReturnByValue.h"#include "src/Core/NoAlias.h"#include "src/Core/PlainObjectBase.h"#include "src/Core/Matrix.h"#include "src/Core/Array.h"#include "src/Core/CwiseBinaryOp.h"#include "src/Core/CwiseUnaryOp.h"#include "src/Core/CwiseNullaryOp.h"#include "src/Core/CwiseUnaryView.h"#include "src/Core/SelfCwiseBinaryOp.h"#include "src/Core/Dot.h"#include "src/Core/StableNorm.h"#include "src/Core/MapBase.h"#include "src/Core/Stride.h"#include "src/Core/Map.h"#include "src/Core/Block.h"#include "src/Core/VectorBlock.h"#include "src/Core/Transpose.h"#include "src/Core/DiagonalMatrix.h"#include "src/Core/Diagonal.h"#include "src/Core/DiagonalProduct.h"#include "src/Core/PermutationMatrix.h"#include "src/Core/Transpositions.h"#include "src/Core/Redux.h"#include "src/Core/Visitor.h"#include "src/Core/Fuzzy.h"#include "src/Core/IO.h"#include "src/Core/Swap.h"#include "src/Core/CommaInitializer.h"#include "src/Core/Flagged.h"#include "src/Core/ProductBase.h"#include "src/Core/Product.h"#include "src/Core/TriangularMatrix.h"#include "src/Core/SelfAdjointView.h"#include "src/Core/SolveTriangular.h"#include "src/Core/products/Parallelizer.h"#include "src/Core/products/CoeffBasedProduct.h"#include "src/Core/products/GeneralBlockPanelKernel.h"#include "src/Core/products/GeneralMatrixVector.h"#include "src/Core/products/GeneralMatrixMatrix.h"#include "src/Core/products/GeneralMatrixMatrixTriangular.h"#include "src/Core/products/SelfadjointMatrixVector.h"#include "src/Core/products/SelfadjointMatrixMatrix.h"#include "src/Core/products/SelfadjointProduct.h"#include "src/Core/products/SelfadjointRank2Update.h"#include "src/Core/products/TriangularMatrixVector.h"#include "src/Core/products/TriangularMatrixMatrix.h"#include "src/Core/products/TriangularSolverMatrix.h"#include "src/Core/products/TriangularSolverVector.h"#include "src/Core/BandMatrix.h"#include "src/Core/BooleanRedux.h"#include "src/Core/Select.h"#include "src/Core/VectorwiseOp.h"#include "src/Core/Random.h"#include "src/Core/Replicate.h"#include "src/Core/Reverse.h"#include "src/Core/ArrayBase.h"#include "src/Core/ArrayWrapper.h"#include "src/Core/GlobalFunctions.h"#include "src/Core/util/ReenableStupidWarnings.h"#include <sensor_msgs/LaserScan.h>#include <sensor_msgs/PointCloud2.h>#include "geometry_msgs/TransformStamped.h"#include "tf/tf.h"#include "ros/ros.h"#include <boost/thread/detail/platform.hpp>#include "platform.hpp"#include <boost/config/auto_link.hpp>#include <deque>#include <boost/thread/detail/config.hpp>#include <boost/config/abi_prefix.hpp>#include <boost/config/abi_suffix.hpp>#include <ctime>#include <boost/date_time/compiler_config.hpp>#include "boost/date_time/c_time.hpp"#include "boost/date_time/time_clock.hpp"#include "boost/date_time/microsec_time_clock.hpp"#include <boost/date_time/time_defs.hpp>#include "boost/date_time/special_defs.hpp"#include "boost/date_time/locale_config.hpp"#include "boost/date_time/constrained_value.hpp"#include "boost/date_time/year_month_day.hpp"#include "boost/date_time/date_defs.hpp"#include <cctype>#include "boost/date_time/int_adapter.hpp"#include <boost/date_time/date_duration.hpp>#include <boost/date_time/date.hpp>#include <boost/date_time/gregorian/greg_calendar.hpp>#include <boost/date_time/gregorian/greg_duration.hpp>#include <boost/date_time/date_duration_types.hpp>#include "boost/date_time/gregorian/greg_date.hpp"#include "boost/date_time/adjust_functors.hpp"#include <boost/date_time/wrapping_int.hpp>#include "boost/date_time/gregorian/greg_duration_types.hpp"#include "boost/date_time/posix_time/ptime.hpp"#include "boost/date_time/posix_time/posix_time_config.hpp"#include "boost/date_time/posix_time/posix_time_system.hpp"#include "boost/date_time/period.hpp"#include "boost/date_time/posix_time/posix_time_duration.hpp"#include "boost/date_time/date_generators.hpp"#include "boost/date_time/filetime_functions.hpp"#include <boost/date_time/gregorian/gregorian_types.hpp>#include <boost/thread/detail/thread_heap_alloc.hpp>#include <boost/thread/condition_variable.hpp>#include <boost/thread/exceptions.hpp>#include <boost/thread/tss.hpp>#include <boost/thread/thread_time.hpp>#include <boost/thread/detail/move.hpp>#include <boost/thread/shared_mutex.hpp>#include <stdio.h>#include "Core"#include "src/StlSupport/StdVector.h"#include "SVD"#include "LU"#include "src/Geometry/OrthoMethods.h"#include "src/Geometry/EulerAngles.h"#include "src/Geometry/Homogeneous.h"#include "src/Geometry/RotationBase.h"#include "src/Geometry/Rotation2D.h"#include "src/Geometry/Quaternion.h"#include "src/Geometry/AngleAxis.h"#include "src/Geometry/Transform.h"#include "src/Geometry/Translation.h"#include "src/Geometry/Scaling.h"#include "src/Geometry/Hyperplane.h"#include "src/Geometry/ParametrizedLine.h"#include "src/Geometry/AlignedBox.h"#include "src/Geometry/Umeyama.h"#include <pcl/pcl_macros.h>#include <Eigen/Core>#include <bitset>#include <sensor_msgs/PointField.h>#include <boost/type_traits/remove_all_extents.hpp>#include <boost/mpl/vector.hpp>#include <boost/aligned_storage.hpp>#include <boost/type_traits/add_cv.hpp>#include <boost/type_traits/remove_volatile.hpp>#include <boost/swap.hpp>#include <boost/preprocessor/seq/enum.hpp>#include <boost/preprocessor/repetition/for.hpp>#include <boost/preprocessor/seq/transform.hpp>#include <pcl/point_cloud.h>#include <pcl/ros/point_traits.h>#include <boost/mpl/is_sequence.hpp>#include <boost/mpl/remove_if.hpp>#include <boost/mpl/contains.hpp>#include <boost/mpl/aux_/unwrap.hpp>#include "pcl/exceptions.h"#include <boost/mpl/logical.hpp>#include <boost/range/config.hpp>#include <string.h>#include <wchar.h>#include <boost/iterator/iterator_traits.hpp>#include <boost/range/const_iterator.hpp>#include <boost/range/iterator.hpp>#include <boost/range/begin.hpp>#include <boost/iterator/detail/enable_if.hpp>#include <boost/range/end.hpp>#include <boost/range/reverse_iterator.hpp>#include <boost/mpl/size.hpp>#include <Eigen/Geometry>#include <tf/transform_datatypes.h>#include <tf/transform_listener.h>#include <pcl/ros/conversions.h>#include <pcl/point_types.h>#include <csm/csm_all.h>#include <csm/laser_data.h>#include "MbICP.h"#include "TData.h"#include <atlascar_base/AtlascarStatus.h>#include <atlascar_base/AtlascarVelocityStatus.h>#include <kfilter/ktypes.hpp>
Go to the source code of this file.
Functions | |
| void | CleanZone (ray_definition *rays, double steering_angle) |
| void | ConfigurePLICP (struct sm_params *params, LDP ref, LDP sens) |
| void | ConvertEstimatedToMeasurment (double vl, double dir, float *dx, float *dy, float *dtheta, double dt, double l, double bwa) |
| void | CreateMeasurementFromDisplacement (double dx, double dy, double dtheta, double z[2], double dt, double l, double bwa) |
| void | laser_2_Handler (const sensor_msgs::LaserScan &scan) |
| void | laser_3_Handler (const sensor_msgs::LaserScan &scan) |
| int | main (int argc, char **argv) |
| void | plcStatusHandler (const atlascar_base::AtlascarStatus &scan) |
| void | RaysToLDP (ray_config_t *cfg, ray_measurment_t *src, LDP dst, bool test_angle, double angle) |
| void | RaysToLDP (ray_definition *src, LDP dst, bool test_angle, double angle) |
| int | RaysToMbICP (ray_config_t *cfg, ray_measurment_t *src, Tpfp *dst, bool test_angle, double angle) |
| int | RaysToMbICP (ray_definition *src, Tpfp *dst) |
| void | RaysToPMScan (ray_config_t *cfg, ray_measurment_t *src, PMScan *dst) |
| void | RaysToPMScan (ray_definition *src, PMScan *dst) |
| void | SetDefaultConfiguration (void) |
| void | velocityStatusHandler (const atlascar_base::AtlascarVelocityStatus &scan) |
| pcl::PointCloud< pcl::PointXYZ > | wrapper_Laserscan2PointCloud (const sensor_msgs::LaserScan &scan) |
Variables | |
| double | bwa = 3.40 |
| ray_definition | c_rays |
| t_flag | flags |
| Tpfp * | fp1 = NULL |
| Tpfp * | fp2 = NULL |
| Tpfp * | fp3 = NULL |
| double | gl = 2.6 |
| ray_history | h_rays |
| ray_definition * | l2_rays = NULL |
| ray_definition * | l_rays = NULL |
| LDP | ldp_1 = NULL |
| LDP | ldp_2 = NULL |
| std::vector< pair< double, double > > | measurments |
| bool | new_laser_2 = false |
| bool | new_laser_3 = false |
| bool | new_plc_status = false |
| bool | new_velocity_status = false |
| std::vector< pair< double, double > > | odometry_readings |
| TSMparams | params |
| struct sm_params | params_sm |
| std::vector< t_posePtr > | path_laser |
| std::vector< t_posePtr > | path_odo |
| PMScan | pm1 |
| PMScan | pm2 |
| pcl::PointCloud< pcl::PointXYZ > | point_cloud_2 |
| pcl::PointCloud< pcl::PointXYZ > | point_cloud_3 |
| struct sm_result | results_sm |
| std::vector< double > | runtime_MbICP |
| std::vector< double > | runtime_PlICP |
| std::vector< double > | runtime_PSM |
| scan_matching_method | scan_method = MBICP |
| Tsc | sensorMotion |
| Tsc | solution |
| Tsc | solution2 |
| int | step = 2 |
| tf::TransformListener * | transform_listener |
| double | z [2] |
Egomotion from lidar main source code.
Definition in file lidar_egomotion.cpp.
| void CleanZone | ( | ray_definition * | rays, | |
| double | steering_angle | |||
| ) |
Definition at line 84 of file lidar_egomotion.cpp.
| void ConfigurePLICP | ( | struct sm_params * | params, | |
| LDP | ref, | |||
| LDP | sens | |||
| ) |
A threshold for stopping.
A threshold for stopping.
Maximum angular displacement between scans (deg)
Maximum translation between scans (m)
Noise in the scan
Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
Definition at line 667 of file lidar_egomotion.cpp.
| void ConvertEstimatedToMeasurment | ( | double | vl, | |
| double | dir, | |||
| float * | dx, | |||
| float * | dy, | |||
| float * | dtheta, | |||
| double | dt, | |||
| double | l, | |||
| double | bwa | |||
| ) |
Definition at line 243 of file lidar_egomotion.cpp.
| void CreateMeasurementFromDisplacement | ( | double | dx, | |
| double | dy, | |||
| double | dtheta, | |||
| double | z[2], | |||
| double | dt, | |||
| double | l, | |||
| double | bwa | |||
| ) |
Definition at line 535 of file lidar_egomotion.cpp.
| void laser_2_Handler | ( | const sensor_msgs::LaserScan & | scan | ) |
Definition at line 757 of file lidar_egomotion.cpp.
| void laser_3_Handler | ( | const sensor_msgs::LaserScan & | scan | ) |
Definition at line 763 of file lidar_egomotion.cpp.
| int main | ( | int | argc, | |
| char ** | argv | |||
| ) |
Definition at line 781 of file lidar_egomotion.cpp.
| void plcStatusHandler | ( | const atlascar_base::AtlascarStatus & | scan | ) |
Definition at line 769 of file lidar_egomotion.cpp.
| void RaysToLDP | ( | ray_config_t * | cfg, | |
| ray_measurment_t * | src, | |||
| LDP | dst, | |||
| bool | test_angle, | |||
| double | angle | |||
| ) |
Definition at line 353 of file lidar_egomotion.cpp.
| void RaysToLDP | ( | ray_definition * | src, | |
| LDP | dst, | |||
| bool | test_angle, | |||
| double | angle | |||
| ) |
Definition at line 330 of file lidar_egomotion.cpp.
| int RaysToMbICP | ( | ray_config_t * | cfg, | |
| ray_measurment_t * | src, | |||
| Tpfp * | dst, | |||
| bool | test_angle, | |||
| double | angle | |||
| ) |
Definition at line 442 of file lidar_egomotion.cpp.
| int RaysToMbICP | ( | ray_definition * | src, | |
| Tpfp * | dst | |||
| ) |
Definition at line 426 of file lidar_egomotion.cpp.
| void RaysToPMScan | ( | ray_config_t * | cfg, | |
| ray_measurment_t * | src, | |||
| PMScan * | dst | |||
| ) |
Definition at line 295 of file lidar_egomotion.cpp.
| void RaysToPMScan | ( | ray_definition * | src, | |
| PMScan * | dst | |||
| ) |
Definition at line 225 of file lidar_egomotion.cpp.
| void SetDefaultConfiguration | ( | void | ) |
Definition at line 611 of file lidar_egomotion.cpp.
| void velocityStatusHandler | ( | const atlascar_base::AtlascarVelocityStatus & | scan | ) |
Definition at line 775 of file lidar_egomotion.cpp.
| pcl::PointCloud<pcl::PointXYZ> wrapper_Laserscan2PointCloud | ( | const sensor_msgs::LaserScan & | scan | ) |
Definition at line 722 of file lidar_egomotion.cpp.
| double bwa = 3.40 |
Definition at line 59 of file lidar_egomotion.cpp.
Definition at line 34 of file lidar_egomotion.cpp.
Definition at line 64 of file lidar_egomotion.cpp.
Definition at line 55 of file lidar_egomotion.cpp.
Definition at line 56 of file lidar_egomotion.cpp.
Definition at line 57 of file lidar_egomotion.cpp.
| double gl = 2.6 |
Definition at line 60 of file lidar_egomotion.cpp.
Definition at line 35 of file lidar_egomotion.cpp.
| ray_definition* l2_rays = NULL |
Definition at line 37 of file lidar_egomotion.cpp.
| ray_definition* l_rays = NULL |
Definition at line 36 of file lidar_egomotion.cpp.
| LDP ldp_1 = NULL |
Definition at line 49 of file lidar_egomotion.cpp.
| LDP ldp_2 = NULL |
Definition at line 50 of file lidar_egomotion.cpp.
| std::vector<pair<double,double> > measurments |
Definition at line 74 of file lidar_egomotion.cpp.
| bool new_laser_2 = false |
Definition at line 79 of file lidar_egomotion.cpp.
| bool new_laser_3 = false |
Definition at line 80 of file lidar_egomotion.cpp.
| bool new_plc_status = false |
Definition at line 81 of file lidar_egomotion.cpp.
| bool new_velocity_status = false |
Definition at line 82 of file lidar_egomotion.cpp.
| std::vector<pair<double,double> > odometry_readings |
Definition at line 73 of file lidar_egomotion.cpp.
| struct sm_params params_sm |
Definition at line 42 of file lidar_egomotion.cpp.
| std::vector<t_posePtr> path_laser |
Definition at line 67 of file lidar_egomotion.cpp.
Definition at line 66 of file lidar_egomotion.cpp.
Definition at line 52 of file lidar_egomotion.cpp.
Definition at line 53 of file lidar_egomotion.cpp.
| pcl::PointCloud<pcl::PointXYZ> point_cloud_2 |
Definition at line 77 of file lidar_egomotion.cpp.
| pcl::PointCloud<pcl::PointXYZ> point_cloud_3 |
Definition at line 78 of file lidar_egomotion.cpp.
| struct sm_result results_sm |
Definition at line 43 of file lidar_egomotion.cpp.
| std::vector<double> runtime_MbICP |
Definition at line 70 of file lidar_egomotion.cpp.
| std::vector<double> runtime_PlICP |
Definition at line 69 of file lidar_egomotion.cpp.
| std::vector<double> runtime_PSM |
Definition at line 71 of file lidar_egomotion.cpp.
| scan_matching_method scan_method = MBICP |
Definition at line 39 of file lidar_egomotion.cpp.
Definition at line 45 of file lidar_egomotion.cpp.
Definition at line 45 of file lidar_egomotion.cpp.
Definition at line 45 of file lidar_egomotion.cpp.
| int step = 2 |
Definition at line 47 of file lidar_egomotion.cpp.
| tf::TransformListener* transform_listener |
Definition at line 76 of file lidar_egomotion.cpp.
| double z[2] |
Definition at line 62 of file lidar_egomotion.cpp.