/home/laradmin/lar/perception/lidar_egomotion/src/lidar_egomotion.cpp File Reference

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>
This graph shows which files directly or indirectly include this file:

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
Tpfpfp1 = NULL
Tpfpfp2 = NULL
Tpfpfp3 = NULL
double gl = 2.6
ray_history h_rays
ray_definitionl2_rays = NULL
ray_definitionl_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_posePtrpath_laser
std::vector< t_posePtrpath_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]

Detailed Description

Egomotion from lidar main source code.

Definition in file lidar_egomotion.cpp.


Function Documentation

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.


Variable Documentation

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.

Tpfp* fp1 = NULL

Definition at line 55 of file lidar_egomotion.cpp.

Tpfp* fp2 = NULL

Definition at line 56 of file lidar_egomotion.cpp.

Tpfp* fp3 = NULL

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.

Definition at line 37 of file lidar_egomotion.cpp.

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.

Definition at line 103 of file MbICP.c.

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.

std::vector<t_posePtr> path_odo

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.

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.

 All Classes Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


lidar_egomotion
Author(s): Jorge Almeida
autogenerated on Wed Jul 23 04:34:38 2014