diff --git a/ethzasl_gridmap_2d/CMakeLists.txt b/ethzasl_gridmap_2d/CMakeLists.txt index 2e056de..a508cb9 100644 --- a/ethzasl_gridmap_2d/CMakeLists.txt +++ b/ethzasl_gridmap_2d/CMakeLists.txt @@ -20,3 +20,5 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) include_directories(include ${Eigen_INCLUDE_DIRS}) add_library(ethzasl_gridmap_2d src/grid-map.cpp) +add_dependencies(${PROJECT_NAME} ethzasl_gridmap_2d) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/ethzasl_icp_mapper/CMakeLists.txt b/ethzasl_icp_mapper/CMakeLists.txt index 8f740f4..66d60b6 100644 --- a/ethzasl_icp_mapper/CMakeLists.txt +++ b/ethzasl_icp_mapper/CMakeLists.txt @@ -18,7 +18,7 @@ generate_messages(DEPENDENCIES sensor_msgs nav_msgs std_msgs geometry_msgs actio catkin_package ( CATKIN_DEPENDS roscpp rospy rosbag std_msgs geometry_msgs nav_msgs -tf tf2 tf_conversions message_filters map_msgs libpointmatcher_ros ethzasl_gridmap_2d visualization_msgs interactive_markers +tf tf2 tf_conversions message_filters map_msgs libpointmatcher_ros ethzasl_gridmap_2d visualization_msgs interactive_markers message_runtime libpointmatcher ) @@ -31,25 +31,8 @@ message_runtime libpointmatcher set(CMAKE_BUILD_TYPE RelWithDebInfo) # c++ 0x -#if("${CMAKE_CXX_COMPILER_ID}" MATCHES "clang") -# set(CMAKE_COMPILER_IS_CLANGXX) -#endif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "clang") -if(CMAKE_COMPILER_IS_GNUCXX) - execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) - if (GCC_VERSION VERSION_GREATER 4.4 OR GCC_VERSION VERSION_EQUAL 4.4) - add_definitions(-std=gnu++0x) - else (GCC_VERSION VERSION_GREATER 4.4 OR GCC_VERSION VERSION_EQUAL 4.4) - message(SEND_ERROR "You need GCC version 4.4 or greater to compile this library") - endif (GCC_VERSION VERSION_GREATER 4.4 OR GCC_VERSION VERSION_EQUAL 4.4) -else(CMAKE_COMPILER_IS_GNUCXX) - message(SEND_ERROR "You need partial C++0x support for N2351, N2672, N1984. Currently this build toolchain supports only GCC >= 4.4. If your compiler supports these specifications, please send us a patch.") -endif(CMAKE_COMPILER_IS_GNUCXX) -# well, this breaks libpointmatcher on non-unix platform, -# please contribute the define for your favorite compiler -# It must support (compiler support in parenthesis): -# - N2351: shared pointers in standard -# - N2672: Initializer lists (gcc) -# - N1984: auto-typed variables (gcc, clang) +execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) +add_definitions(-std=gnu++0x) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) @@ -85,5 +68,3 @@ target_link_libraries(dynamic_mapper ${catkin_LIBRARIES}) add_executable(dynamic_trails src/dynamic_trails.cpp ) add_dependencies(dynamic_trails ${PROJECT_NAME}_gencpp) target_link_libraries(dynamic_trails ${catkin_LIBRARIES}) - - diff --git a/ethzasl_icp_mapper/src/dynamic_trails.cpp b/ethzasl_icp_mapper/src/dynamic_trails.cpp index 0d09187..9f3bc2c 100644 --- a/ethzasl_icp_mapper/src/dynamic_trails.cpp +++ b/ethzasl_icp_mapper/src/dynamic_trails.cpp @@ -41,7 +41,7 @@ class DynamicTrails { typedef PM::TransformationParameters TP; typedef PM::Matches Matches; - + typedef typename Nabo::NearestNeighbourSearch NNS; typedef typename NNS::SearchType NNSearchType; @@ -68,19 +68,19 @@ class DynamicTrails shared_ptr lastPointCloud; shared_ptr trailPointCloud; PM::DataPoints globalMap; - + std::shared_ptr globalNNS; std::shared_ptr lastNNS; boost::mutex usingGlobalMap; - + std::deque> priors; // tf tf::TransformListener tfListener; boost::mutex publishLock; - + const float eps; ros::Time lastInputTime; @@ -93,11 +93,11 @@ class DynamicTrails const int p_windowSize; const string p_trailFilepName; int trailCount; - + public: DynamicTrails(ros::NodeHandle& n, ros::NodeHandle& pn); ~DynamicTrails(); - + protected: //void gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn, const nav_msgs::OdometryConstPtr& odom); void gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn); @@ -106,7 +106,7 @@ class DynamicTrails PM::DataPoints extractDynamicPoints(const PM::DataPoints input, const float minDynamicRatio); void computeVelocity(shared_ptr reference, shared_ptr ref_tree, shared_ptr reading, shared_ptr read_tree, const float maxRadius, const int knn, ros::Publisher debugPub); float angleWeight(const Eigen::Vector3f &normal, const Eigen::Vector3f &velocity); - + void averageWithReference(shared_ptr reference, shared_ptr reading, const float dir); }; @@ -131,7 +131,7 @@ DynamicTrails::DynamicTrails(ros::NodeHandle& n, ros::NodeHandle& pn): p_trailFilepName(getParam("trail_filename", "trailMap.vtk")), trailCount(0) { - + // memory for last velocities //priors.resize(p_windowSize); @@ -156,10 +156,10 @@ DynamicTrails::DynamicTrails(ros::NodeHandle& n, ros::NodeHandle& pn): ROS_INFO_STREAM("No input filters config file given, not using these filters"); } - + // topics and services initialization //sync.registerCallback(boost::bind(&DynamicTrails::gotCloud, this, _1, _2)); - + cloudSub = n.subscribe("cloud_in", 10, &DynamicTrails::gotCloud, this); mapSub = n.subscribe("point_map", 1, &DynamicTrails::gotMap, this); lastPub = n.advertise("last_dynamic_points", 2, true); @@ -167,13 +167,13 @@ DynamicTrails::DynamicTrails(ros::NodeHandle& n, ros::NodeHandle& pn): lastProjectedPub= n.advertise("last_projected_points", 2, true); dynProjectedPub= n.advertise("dyn_projected_points", 2, true); markerPub = n.advertise("dynamic_trail", 1); - + } DynamicTrails::~DynamicTrails() { // save point cloud - + if (trailPointCloud) { trailPointCloud->save(p_trailFilepName); @@ -188,7 +188,7 @@ void DynamicTrails::gotMap(const sensor_msgs::PointCloud2& cloudMsgIn) usingGlobalMap.lock(); // Convert pointcloud2 to DataPoint globalMap = PointMatcher_ros::rosMsgToPointMatcherCloud(cloudMsgIn); - + // Build a kd-tree globalNNS.reset( NNS::create(globalMap.features, globalMap.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS)); usingGlobalMap.unlock(); @@ -202,14 +202,14 @@ void DynamicTrails::gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn) const ros::Time currentTime = ros::Time::now(); const float currentRate = 1.0/(currentTime - lastInputTime).toSec(); - + if(currentRate < p_inputRate ) { lastInputTime = currentTime; // Convert pointcloud2 to DataPoint DP cloud(PointMatcher_ros::rosMsgToPointMatcherCloud(*cloudMsgIn)); - + // Convert odometry msg to TransformationParameters //TP TScannerToMap = PointMatcher_ros::odomMsgToEigenMatrix(*odom); TP TScannerToMap; @@ -230,7 +230,7 @@ void DynamicTrails::gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn) ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< cloudMsgIn->header.stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - cloudMsgIn->header.stamp); return; } - + } } @@ -240,36 +240,36 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) { ROS_INFO("Processing new point cloud"); - + timer t; // Print how long take the algo - - + + ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]"); - + // Correct new points using ICP result and move them in their own frame - inputPointCloud = transformation->compute(inputPointCloud, TScannerToMap); + inputPointCloud = transformation->compute(inputPointCloud, TScannerToMap); cerr << "inputPointCloud.features.cols() " << inputPointCloud.features.cols() << endl; // filter to keep only dynamic points shared_ptr dynamicPointCloud( new DP(extractDynamicPoints(inputPointCloud, p_minDynamicRatio))); - - + + // Apply filters to incoming cloud, in global coordinates! inputFilters.apply(*dynamicPointCloud); - + const int dynamicPtsCount = dynamicPointCloud->features.cols(); cerr << "dynamicPointCloud->features.cols() " << dynamicPtsCount << endl; - + // Error verification if(dynamicPtsCount == 0) { ROS_WARN_STREAM("No dynamic points could be found"); return; } - + //PM::Matrix unitVec(3,dynamicPtsCount); //unitVec.row(0) = PM::Matrix::Ones(1,dynamicPtsCount); //unitVec.bottomRows(2) = PM::Matrix::Zero(2,dynamicPtsCount); @@ -312,17 +312,17 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) return; } - + // Copy the last speed direction for temporal smoothing PM::Matrix priorOn_velocityDir = lastPointCloud->getDescriptorViewByName("velocity_dir"); PM::Matrix priorOn_velocityDt = lastPointCloud->getDescriptorViewByName("velocity_dt"); - + DP::View v_normals_input = dynamicPointCloud->getDescriptorViewByName("normals"); PM::Matrix inputEuclNormal(6, dynamicPtsCount); inputEuclNormal.topRows(3) = dynamicPointCloud->features.topRows(3); inputEuclNormal.bottomRows(3) = v_normals_input*p_normalSpaceFactor; - + // Build kd-tree of the input points std::shared_ptr inputNNS; @@ -341,7 +341,7 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) averageWithReference(lastPointCloud, dynamicPointCloud, 1); averageWithReference(dynamicPointCloud, lastPointCloud, 1); } - + DP::View v_matchId_dyn = dynamicPointCloud->getDescriptorViewByName("match_id"); //DP::View v_velocityMag_dyn = dynamicPointCloud->getDescriptorViewByName("velocity_mag"); @@ -352,10 +352,10 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) //DP::View v_velocityMag_last = lastPointCloud->getDescriptorViewByName("velocity_mag"); DP::View v_velocityDir_last = lastPointCloud->getDescriptorViewByName("velocity_dir"); DP::View v_velocityDt_last = lastPointCloud->getDescriptorViewByName("velocity_dt"); - + // Project into the future v_velocityDir_dyn *= -1; - + // Markers visualization_msgs::Marker marker; @@ -366,7 +366,7 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) //marker.id = trailCount; trailCount++; marker.type = visualization_msgs::Marker::LINE_LIST; - + marker.scale.x = 0.01; marker.scale.y = 0.1; marker.scale.z = 0.1; @@ -389,15 +389,15 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) const Eigen::Vector3f pt = dynamicPointCloud->features.col(i).head(3); const Eigen::Vector3f pt_last = lastPointCloud->features.col(id).head(3); - + const Eigen::Vector3f normal = v_normal_dyn.col(i); const float w_ang = angleWeight(normal, v_velocityDir_dyn.col(i)); - + const Eigen::Vector3f lastVelDir = priorOn_velocityDir.col(id); const Eigen::Vector3f currentVelDir = v_velocityDir_dyn.col(i); const float w_diffAngle = 1 - acos(lastVelDir.normalized().dot(currentVelDir.normalized()))/M_PI; - - + + int lastID = id; // FIXME: finish here @@ -410,11 +410,11 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) // DP::View match_id = priors[i]->getDescriptorViewByName("match_id"); // lastID = match_id(0, lastID); - // + // //} //cerr << "----------" << endl; - + const float lastDt = priorOn_velocityDt(0,id); const float delta = (pt_last - pt).norm(); const float delta_projected = (pt_last + lastVelDir*lastDt - pt).norm(); @@ -436,7 +436,7 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) p.y = dynamicPointCloud->features(1,i); p.z = dynamicPointCloud->features(2,i); marker.points.push_back(p); - + p.x += v_velocityDir_dyn(0,i); p.y += v_velocityDir_dyn(1,i); p.z += v_velocityDir_dyn(2,i); @@ -452,14 +452,14 @@ void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap) v_velocityDir_dyn.col(i) = Eigen::Vector3f::Zero(); v_velocityDt_dyn(0,i) = 0; } - + } currenttTrails.conservativeResize(currentTrailPtCount); if(currentTrailPtCount > 0) { - if(trailPointCloud == false) + if(trailPointCloud == nullptr) { trailPointCloud.reset(new DP(currenttTrails)); } @@ -503,7 +503,7 @@ PM::DataPoints DynamicTrails::extractDynamicPoints (const PM::DataPoints input, const int knn = 1; Matches::Dists dists_input(knn, inputPtsCount); Matches::Ids ids_input(knn, inputPtsCount); - + usingGlobalMap.lock(); // FIXME: this is a parameter const float maxGlobalDist = 0.5; @@ -569,7 +569,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re DP::View v_Msec_read = reading->getDescriptorViewByName("stamps_Msec"); DP::View v_sec_read = reading->getDescriptorViewByName("stamps_sec"); DP::View v_nsec_read = reading->getDescriptorViewByName("stamps_nsec"); - + // Ensure that we don't modify the reference PM::Matrix velocityDir_ref = PM::Matrix::Zero(3,refPtsCount); PM::Matrix velocityDt_ref = PM::Matrix::Zero(1,refPtsCount); @@ -577,7 +577,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re DP::View v_Msec_ref = reference->getDescriptorViewByName("stamps_Msec"); DP::View v_sec_ref = reference->getDescriptorViewByName("stamps_sec"); DP::View v_nsec_ref = reference->getDescriptorViewByName("stamps_nsec"); - + // Move points based on velocity PM::Matrix moved_read(3, readPtsCount); @@ -587,7 +587,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re //PM::Matrix static_read(6, readPtsCount); //static_read.topRows(3) = reading->features.topRows(3); //static_read.bottomRows(3) = v_normal_read; - + //if (knn == 1) // abort(); @@ -595,11 +595,11 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re { moved_read.col(i)= reading->features.col(i).head(3) + v_velocityDir_read.col(i)*v_velocityDt_read(0,i); //moved_read.col(i)= reading->features.col(i).head(3) + v_velocityDir_read.col(i); - + //moved_read.topRows(3).col(i)= reading->features.col(i).head(3) + v_velocityDir_read.col(i)*v_velocityMag_read(0,i); //moved_read.bottomRows(3).col(i) = v_normal_read.col(i); - - + + assert(!isnan(moved_read(0,i))); assert(!isnan(moved_read(1,i))); assert(!isnan(moved_read(2,i))); @@ -615,7 +615,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re ref_tree->knn(moved_read, ids, dists, knn, 0, 2.0); - + PM::Matrix sumVec_ref = PM::Matrix::Zero(3,refPtsCount); //PM::Matrix sumVec_ref(3,refPtsCount); //sumVec_ref.row(0) = PM::Matrix::Ones(1,refPtsCount); @@ -642,7 +642,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re const int id = ids(k,i); const Eigen::Vector3f pt_nn = reference->features.col(id).head(3); const double timeRef = v_Msec_ref(0,id)*1e6 + v_sec_ref(0,id) + v_nsec_ref(0,id)*1e-9; - + const Eigen::Vector3f delta = pt_nn - pt; const Eigen::Vector3f normal_read = v_normal_read.col(i); const double dt = abs(timeRead - timeRef); @@ -658,7 +658,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re assert(!isnan(w_ang)); assert(dt != 0); - + weight_ref(id) += w; sumDt_ref(id) += w*dt; sumVec_ref.col(id) += w*delta/dt; @@ -668,7 +668,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re sumDt += w*dt; sumVec += w*delta/dt; //sumVec += w*delta; - + closeId_read(i) = id; } } @@ -703,13 +703,13 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re // Weighted mean sumDt_ref(i) /= weight_ref(i); sumVec_ref.col(i) /= weight_ref(i); - + if(mag < p_maxVelocity) { // Store in the descriptors velocityDir_ref.col(i) = sumVec_ref.col(i); velocityDt_ref(0,i) = sumDt_ref(i); - } + } } } @@ -731,7 +731,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re v_velocityDt_read(0,i) = velocityDt_ref(0,id); } } - + } //------------------ @@ -744,19 +744,19 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re read_tree->knn(static_read, ids_local, dists_local, maxNN, 0, 0, maxRadius); //PM::Matrix dists_nosqrt = dists_local.cwiseSqrt(); //dists_local = dists_local.cwiseSqrt(); - + // Initialize counters - + PM::Matrix sumVec_read = PM::Matrix::Zero(3,readPtsCount); PM::Matrix weight_read = PM::Matrix::Zero(1,readPtsCount); PM::Matrix sumMag_read = PM::Matrix::Zero(1,readPtsCount); PM::Matrix sumDt_read = PM::Matrix::Zero(1,readPtsCount); - + for(int i=0; i::infinity()) @@ -780,7 +780,7 @@ void DynamicTrails::computeVelocity(shared_ptr reference, shared_ptr re } } - + //------------------ // Average points given their neighborgs for(int i=0; i reference, shared_ptr re v_velocityDt_read(0,i) = sumDt_read(i); } } - + assert(isnan(v_velocityDir_read(0,i)) == false); } @@ -838,28 +838,28 @@ void DynamicTrails::averageWithReference(shared_ptr reference, shared_ptrfeatures.cols(); const int refPtsCount = reference->features.cols(); - + //DP::View v_matchId_read = reading->getDescriptorViewByName("match_id"); DP::View v_velocityDir_read = reading->getDescriptorViewByName("velocity_dir"); DP::View v_velocityDt_read = reading->getDescriptorViewByName("velocity_dt"); DP::View v_normal_read = reading->getDescriptorViewByName("normals"); - + DP::View v_matchId_ref = reference->getDescriptorViewByName("match_id"); DP::View v_velocityDir_ref = reference->getDescriptorViewByName("velocity_dir"); DP::View v_velocityDt_ref = reference->getDescriptorViewByName("velocity_dt"); - + for(int i=0; i eps && mag_ref > eps) { v_velocityDir_read.col(id) = (w_read*dir*v_velocityDir_read.col(id) - w_ref*dir*v_velocityDir_ref.col(i))/(w_sum); - + v_velocityDt_read(0,id) = (w_read*v_velocityDt_read(0,id) + w_ref*v_velocityDt_ref(0,i))/(w_sum); } } @@ -871,14 +871,14 @@ void DynamicTrails::averageWithReference(shared_ptr reference, shared_ptr eps && mag_ref > eps) // { // assert((dir*v_velocityDir_read.col(i) - dir*v_velocityDir_ref.col(id)).norm() != 0); // v_velocityDir_read.col(i) = (w_read*dir*v_velocityDir_read.col(i) - w_ref*dir*v_velocityDir_ref.col(id))/(w_sum); - // + // // v_velocityDt_read(0,i) = (w_read*v_velocityDt_read(0,i) + w_ref*v_velocityDt_ref(0,id))/(w_sum); // } // assert(isnan(v_velocityDir_read(0,i)) == false); @@ -900,6 +900,6 @@ int main(int argc, char **argv) ros::spinOnce(); r.sleep(); } - + return 0; } diff --git a/libpointmatcher_ros/CMakeLists.txt b/libpointmatcher_ros/CMakeLists.txt index 6cc8591..770017b 100644 --- a/libpointmatcher_ros/CMakeLists.txt +++ b/libpointmatcher_ros/CMakeLists.txt @@ -26,24 +26,8 @@ libpointmatcher ) # c++ 0x -#if("${CMAKE_CXX_COMPILER_ID}" MATCHES "clang") -# set(CMAKE_COMPILER_IS_CLANGXX) -#endif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "clang") -if(CMAKE_COMPILER_IS_GNUCXX) - execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) - if (GCC_VERSION VERSION_GREATER 4.4 OR GCC_VERSION VERSION_EQUAL 4.4) - add_definitions(-std=gnu++0x) - else (GCC_VERSION VERSION_GREATER 4.4 OR GCC_VERSION VERSION_EQUAL 4.4) - message(SEND_ERROR "You need GCC version 4.4 or greater to compile this library") - endif (GCC_VERSION VERSION_GREATER 4.4 OR GCC_VERSION VERSION_EQUAL 4.4) -else(CMAKE_COMPILER_IS_GNUCXX) - message(SEND_ERROR "You need partial C++0x support for N2351, N2672, N1984. Currently this build toolchain supports only GCC >= 4.4. If your compiler supports these specifications, please send us a patch.") -endif(CMAKE_COMPILER_IS_GNUCXX) - -# well, this breaks libpointmatcher on non-unix platform, -# please contribute the define for your favorite compiler -# It must support (compiler support in parenthesis): -# - N1984: auto-typed variables (gcc, clang) +execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) +add_definitions(-std=gnu++0x) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) @@ -53,4 +37,4 @@ include_directories(include) add_library(pointmatcher_ros src/point_cloud.cpp src/transform.cpp) include_directories(${catkin_INCLUDE_DIRS}) -target_link_libraries(pointmatcher_ros ${catkin_LIBRARIES}) \ No newline at end of file +target_link_libraries(pointmatcher_ros ${catkin_LIBRARIES})