From 47e9f27b145717a6d8e482762ae11e92cfd7fd11 Mon Sep 17 00:00:00 2001 From: "buaaxw@gmail.com" Date: Thu, 12 Nov 2020 14:52:34 +0800 Subject: [PATCH] solve some bugs and increase the precission --- launch/mapping_avia.launch | 6 +- rviz_cfg/loam_livox.rviz | 14 +- src/IMU_Processing.hpp | 7 +- src/feature_extract.cpp | 727 ++++++++++++++++--------------------- src/laserMapping.cpp | 51 ++- 5 files changed, 358 insertions(+), 447 deletions(-) diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch index 64a1deb..6f38467 100644 --- a/launch/mapping_avia.launch +++ b/launch/mapping_avia.launch @@ -20,18 +20,18 @@ - + - + - + diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index c377629..cd634ff 100644 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -217,8 +217,8 @@ Visualization Manager: - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -1.7674484252929688 - Min Value: -8.243647575378418 + Max Value: 0.32742905616760254 + Min Value: -2.179872751235962 Value: true Axis: Z Channel Name: intensity @@ -664,18 +664,18 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 6.608828544616699 - Y: 3.081573247909546 - Z: -3.704350709915161 + X: 6.49392557144165 + Y: -0.4694555699825287 + Z: -1.3836920261383057 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -0.17020323872566223 + Pitch: 0.3497962951660156 Target Frame: Value: Orbit (rviz) - Yaw: 3.0474979877471924 + Yaw: 5.425689220428467 Saved: ~ Window Geometry: Displays: diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp index 5c7e0fa..63cc2c2 100644 --- a/src/IMU_Processing.hpp +++ b/src/IMU_Processing.hpp @@ -26,7 +26,7 @@ /// *************Preconfiguration -#define MAX_INI_COUNT (50) +#define MAX_INI_COUNT (200) const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; @@ -216,7 +216,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout #ifdef DEBUG_PRINT // fout<header.stamp.toSec()<<" "<header.stamp.toSec() - head->header.stamp.toSec(); /* covariance propagation */ @@ -224,7 +224,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout Eigen::Matrix3d Exp_f = Exp(angvel_avr, dt); acc_avr_skew<(0,0) = - Exp_f; + F_x.block<3,3>(0,0) = Exp(angvel_avr, - dt); F_x.block<3,3>(0,9) = - Eye3d * dt; // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; F_x.block<3,3>(3,6) = Eye3d * dt; @@ -290,7 +290,6 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout pos_imu<pos); angvel_avr<gyr); - int i = 0; for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) { dt = it_pcl->curvature / double(1000) - head->offset_time; diff --git a/src/feature_extract.cpp b/src/feature_extract.cpp index e6b97d9..44822c7 100644 --- a/src/feature_extract.cpp +++ b/src/feature_extract.cpp @@ -3,6 +3,8 @@ #include #include +// Feature will be updated in next version + typedef pcl::PointXYZINormal PointType; using namespace std; ros::Publisher pub_full, pub_surf, pub_corn; @@ -15,12 +17,11 @@ enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; struct orgtype { - double range; // sqrt(x*x+y*y) - double dista; // 该点与后一个点的距离平方 - double angle[2]; // 前(后)一个点、该点、原点所成角度 - double intersect; // 前后点与该点的夹角 - E_jump edj[2]; // 每个点前后点的关系 - // Surround nor_dir; + double range; + double dista; + double angle[2]; + double intersect; + E_jump edj[2]; Feature ftype; orgtype() { @@ -32,7 +33,6 @@ struct orgtype } }; -// const int hor_line = 6; const double rad2deg = 180*M_1_PI; int lidar_type; @@ -63,25 +63,6 @@ int main(int argc, char **argv) ros::init(argc, argv, "feature_extract"); ros::NodeHandle n; - // lidar_type = MID; - // blind = 0.5; - // inf_bound = 10; - // // N_SCANS = 1; - // group_size = 8; - // disA = 0.01; disB = 0.1; - // p2l_ratio = 400; - // limit_maxmid = 9; - // limit_midmin = 16; - // // limit_maxmin; - // jump_up_limit = cos(175.0/180*M_PI); - // jump_down_limit = cos(5.0/180*M_PI); - // cos160 = cos(160.0/180*M_PI); - // edgea = 3; // 2 - // edgeb = 0.05; // 0.1 - // smallp_intersect = cos(170.0/180*M_PI); - // smallp_ratio = 1.3; - // point_filter_num = 4; - n.param("lidar_type", lidar_type, 0); n.param("blind", blind, 0.5); n.param("inf_bound", inf_bound, 10); @@ -107,75 +88,18 @@ int main(int argc, char **argv) cos160 = cos(cos160/180*M_PI); smallp_intersect = cos(smallp_intersect/180*M_PI); - // lidar_type = HORIZON; - // blind = 0.5; - // inf_bound = 10; - // N_SCANS = 6; - // group_size = 8; - // disA = 0.01; disB = 0.1; - // p2l_ratio = 225; - // limit_maxmid = 6.25; - // limit_midmin = 6.25; - // // limit_maxmin; - // jump_up_limit = cos(170.0/180*M_PI); - // jump_down_limit = cos(8.0/180*M_PI); - // cos160 = cos(160.0/180*M_PI); - // edgea = 2; - // edgeb = 0.1; - // smallp_intersect = cos(170.0/180*M_PI); - // smallp_ratio = 1.4; - - // lidar_type = VELO16; - // blind = 0.5; - // inf_bound = 10; - // N_SCANS = 16; - // group_size = 9; - // disA = 0.015; disB = 0.3; - // p2l_ratio = 400; - // // limit_maxmid = 9; - // // limit_midmin = 16; - // limit_maxmin = 3.24; - // jump_up_limit = cos(170.0/180*M_PI); - // jump_down_limit = cos(8.0/180*M_PI); - // cos160 = cos(160.0/180*M_PI); - // edgea = 2; // 2 - // edgeb = 0.1; // 0.1 - // smallp_intersect = cos(170.0/180*M_PI); - // smallp_ratio = 1.4; - - // lidar_type = OUST64; // lidar的种类 - // blind = 0.5; // lidar的盲区（非平方） - // inf_bound = 10; // 无穷远点最近点的距离限制（非平方） - // N_SCANS = 64; // 6, 16, 64 - // group_size = 9; // 7->8 9 - // disA = 0.015; // 0.015 // Ax+B - // disB = 0.3; // 0.3 - // p2l_ratio = 196; // 225 196 - // // limit_maxmid = 9; // 6.25 - // // limit_midmin = 16; // 6.25 - // limit_maxmin = 6.25; // 6.25 - // jump_up_limit = cos(170.0/180*M_PI); - // jump_down_limit = cos(8.0/180*M_PI); - // cos160 = cos(160.0/180*M_PI); - // edgea = 2.5; // 2 - // edgeb = 0.2; // 0.1 - // smallp_intersect = cos(170.0/180*M_PI); - // smallp_ratio = 1.4; - - ros::Subscriber sub_points; - switch(lidar_type) { case MID: - printf("MID40-70\n"); + printf("MID40\n"); sub_points = n.subscribe("/livox/lidar", 1000, mid_handler); // sub_points = n.subscribe("/livox/lidar_1LVDG1S006J5GZ3", 1000, mid_handler); break; case HORIZON: - printf("HORIZON_MID70pro\n"); + printf("HORIZON\n"); sub_points = n.subscribe("/livox/lidar", 1000, horizon_handler); break; @@ -195,9 +119,9 @@ int main(int argc, char **argv) break; } - pub_full = n.advertise("/livox_cloud", 20); - pub_surf = n.advertise("/laser_cloud_flat", 20); - pub_corn = n.advertise("/laser_cloud_sharp", 20); + pub_full = n.advertise("/laser_cloud", 100); + pub_surf = n.advertise("/laser_cloud_flat", 100); + pub_corn = n.advertise("/laser_cloud_sharp", 100); ros::spin(); return 0; @@ -218,7 +142,6 @@ void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) for(uint i=0; i> a; - // if(a == 0) - // { - // exit(0); - // } - + pub_func(pl, pub_full, msg->header.stamp); + pub_func(pl_surf, pub_surf, msg->header.stamp); + pub_func(pl_corn, pub_corn, msg->header.stamp); } void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) @@ -296,13 +211,110 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) ros::Time ct; ct.fromNSec(msg->timebase); - pub_func(pl_full, pub_full, ct); - pub_func(pl_surf, pub_surf, ct); - pub_func(pl_corn, pub_corn, ct); + pub_func(pl_full, pub_full, msg->header.stamp); + pub_func(pl_surf, pub_surf, msg->header.stamp); + pub_func(pl_corn, pub_corn, msg->header.stamp); std::cout<<"[~~~~~~~~~~~~ Feature Extract ]: time: "<< omp_get_wtime() - t1<<" "<header.stamp.toSec()< pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + uint plsize = pl_orig.size(); + + vector> pl_buff(N_SCANS); + vector> typess(N_SCANS); + pcl::PointCloud pl_corn, pl_surf, pl_full; + + int scanID; + int last_stat = -1; + int idx = 0; + + for(int i=0; i=N_SCANS || scanID<0) + { + continue; + } + + if(orders[scanID] <= last_stat) + { + idx++; + } + + pl_buff[scanID][idx].x = ap.x; + pl_buff[scanID][idx].y = ap.y; + pl_buff[scanID][idx].z = ap.z; + pl_buff[scanID][idx].intensity = ap.intensity; + typess[scanID][idx].range = leng; + last_stat = orders[scanID]; + } + + // for(int i=0; iheader.stamp); + // } + + + // for(int i=0; i<10; i++) + // { + // printf("%f %f %f\n", pl_buff[0][i].x, pl_buff[0][i].y, pl_buff[0][i].z); + // } + // cout << endl; + + idx++; + + for(int j=0; j &pl = pl_buff[j]; + vector &types = typess[j]; + pl.erase(pl.begin()+idx, pl.end()); + types.erase(types.begin()+idx, types.end()); + plsize = idx - 1; + for(uint i=0; iheader.stamp); + pub_func(pl_surf, pub_surf, msg->header.stamp); + pub_func(pl_corn, pub_corn, msg->header.stamp); + +} + + +void velo16_handler1(const sensor_msgs::PointCloud2::ConstPtr &msg) { pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); @@ -354,6 +366,41 @@ void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) stat = 0; } } + + // idx = 0; + // int last_stat = -1; + // for(uint i=0; i=N_SCANS || scanID<0) + // { + // continue; + // } + + // if(orders[scanID] <= last_stat) + // { + // idx++; + // } + + // pl_buff[scanID][idx].x = ap.x; + // pl_buff[scanID][idx].y = ap.y; + // pl_buff[scanID][idx].z = ap.z; + // pl_buff[scanID][idx].intensity = ap.intensity; + + // last_stat = orders[scanID]; + // } + + idx++; @@ -374,13 +421,13 @@ void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) } types[plsize].range = sqrt(pl[plsize].x*pl[plsize].x + pl[plsize].y*pl[plsize].y); + give_feature(pl, types, pl_corn, pl_surf); } - ros::Time ct(ros::Time::now()); - pub_func(pl_full, pub_full, ct); - pub_func(pl_surf, pub_surf, ct); - pub_func(pl_corn, pub_corn, ct); + pub_func(pl_full, pub_full, msg->header.stamp); + pub_func(pl_surf, pub_surf, msg->header.stamp); + pub_func(pl_corn, pub_corn, msg->header.stamp); } void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) @@ -398,7 +445,6 @@ void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) for(int i=0; iheader.stamp); + pub_func(pl_surf, pub_surf, msg->header.stamp); + pub_func(pl_corn, pub_corn, msg->header.stamp); } @@ -444,12 +489,13 @@ void give_feature(pcl::PointCloud &pl, vector &types, pcl::P return; } uint head = 0; + while(types[head].range < blind) { head++; } - // 平面点检测 + // Surf plsize2 = plsize - group_size; Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); @@ -457,7 +503,6 @@ void give_feature(pcl::PointCloud &pl, vector &types, pcl::P uint i_nex, i2; uint last_i = 0; uint last_i_nex = 0; - // 0:上次状态无用 1:上次为平面组 int last_state = 0; int plane_type; @@ -511,12 +556,12 @@ void give_feature(pcl::PointCloud &pl, vector &types, pcl::P { if(last_state == 1) { - uint i_nex_tem; // 临时变量 + uint i_nex_tem; uint j; for(j=last_i+1; j<=last_i_nex; j++) { uint i_nex_tem2 = i_nex_tem; - Eigen::Vector3d curr_direct2; // curr_direct临时变量 + Eigen::Vector3d curr_direct2; uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); @@ -561,100 +606,100 @@ void give_feature(pcl::PointCloud &pl, vector &types, pcl::P } plsize2 = plsize - 3; - // for(uint i=head+3; i=Real_Plane) - // { - // continue; - // } + for(uint i=head+3; i=Real_Plane) + { + continue; + } - // if(types[i-1].dista<1e-16 || types[i].dista<1e-16) - // { - // continue; - // } + if(types[i-1].dista<1e-16 || types[i].dista<1e-16) + { + continue; + } - // Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); - // Eigen::Vector3d vecs[2]; + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; - // for(int j=0; j<2; j++) - // { - // int m = -1; - // if(j == 1) - // { - // m = 1; - // } + for(int j=0; j<2; j++) + { + int m = -1; + if(j == 1) + { + m = 1; + } - // if(types[i+m].range < blind) - // { - // if(types[i].range > inf_bound) - // { - // types[i].edj[j] = Nr_inf; - // } - // else - // { - // types[i].edj[j] = Nr_blind; - // } - // continue; - // } + if(types[i+m].range < blind) + { + if(types[i].range > inf_bound) + { + types[i].edj[j] = Nr_inf; + } + else + { + types[i].edj[j] = Nr_blind; + } + continue; + } - // vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); - // vecs[j] = vecs[j] - vec_a; + vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); + vecs[j] = vecs[j] - vec_a; - // types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); - // if(types[i].angle[j] < jump_up_limit) - // { - // types[i].edj[j] = Nr_180; - // } - // else if(types[i].angle[j] > jump_down_limit) - // { - // types[i].edj[j] = Nr_zero; - // } - // } + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if(types[i].angle[j] < jump_up_limit) + { + types[i].edj[j] = Nr_180; + } + else if(types[i].angle[j] > jump_down_limit) + { + types[i].edj[j] = Nr_zero; + } + } - // types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); - // if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) - // { - // if(types[i].intersect > cos160) - // { - // if(edge_jump_judge(pl, types, i, Prev)) - // { - // types[i].ftype = Edge_Jump; - // } - // } - // } - // else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) - // { - // if(types[i].intersect > cos160) - // { - // if(edge_jump_judge(pl, types, i, Next)) - // { - // types[i].ftype = Edge_Jump; - // } - // } - // } - // else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) - // { - // if(edge_jump_judge(pl, types, i, Prev)) - // { - // types[i].ftype = Edge_Jump; - // } - // } - // else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) - // { - // if(edge_jump_judge(pl, types, i, Next)) - // { - // types[i].ftype = Edge_Jump; - // } + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } - // } - // else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) - // { - // if(types[i].ftype == Nor) - // { - // types[i].ftype = Wire; - // } - // } - // } + } + else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) + { + if(types[i].ftype == Nor) + { + types[i].ftype = Wire; + } + } + } plsize2 = plsize-1; double ratio; @@ -696,81 +741,84 @@ void give_feature(pcl::PointCloud &pl, vector &types, pcl::P } } - int last_surface = 0; - for(uint i=0; i &pl, vector &type } -int plane_judge1(const pcl::PointCloud &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct) -{ - double group_dis = 0.01*pl[i].x + 0.1; - group_dis = group_dis*group_dis; - - double two_dis; - vector disarr; - disarr.reserve(128); - - for(i_nex=i; i_nex= group_dis) - { - break; - } - disarr.push_back(types[i_nex].dista); - i_nex++; - } - - - double leng_wid = 0; - double v1[3], v2[3]; - for(uint j=i+1; j leng_wid) - { - leng_wid = lw; - } - } - - // if((two_dis*two_dis/leng_wid) <= 225) - if((two_dis*two_dis/leng_wid) <= 400) - { - curr_direct = Eigen::Vector3d::Zero(); - return 0; - } - - // 寻找中位数(不是准确的中位数，有点偏差) - for(uint32_t j=0; j=9 || dismid_min>=16) - { - curr_direct = Eigen::Vector3d::Zero(); - return 0; - } - - curr_direct << vx, vy, vz; - curr_direct.normalize(); - return 1; -} - -bool edge_jump_judge1(const pcl::PointCloud &pl, vector &types, uint i, Surround nor_dir) -{ - // if(nor_dir == 0) - // { - // if(pl[i-1].x0.05) - { - if(d1>3*d2 || (d1-d2)>0.05) - { - return false; - } - } - return true; -} - - diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 474039e..59b4f9a 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -82,6 +82,7 @@ int laserCloudValidInd[250]; int laserCloudSurroundInd[250]; int laserCloudValidNum = 0; int laserCloudSurroundNum = 0; +int laserCloudSelNum = 0; const int laserCloudWidth = 42; const int laserCloudHeight = 22; @@ -425,7 +426,6 @@ void lasermap_fov_segment() if (isInLaserFOV) { - laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudValidNum ++; @@ -533,14 +533,12 @@ int main(int argc, char** argv) ros::Subscriber sub_pcl = nh.subscribe("/laser_cloud_flat", 20000, feat_points_cbk); ros::Subscriber sub_imu = nh.subscribe("/livox/imu", 20000, imu_cbk); - // ros::Subscriber subLaserCloudSurfLast = nh.subscribe - // ("/livox_undistort", 100, featsLastHandler); ros::Publisher pubLaserCloudFullRes = nh.advertise ("/cloud_registered", 100); + ros::Publisher pubLaserCloudEffect = nh.advertise + ("/cloud_effected", 100); ros::Publisher pubLaserCloudMap = nh.advertise ("/Laser_map", 100); - // ros::Publisher pubSolvedPose6D = nh.advertise - // ("/States_updated", 100); ros::Publisher pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 10); @@ -711,7 +709,7 @@ int main(int argc, char** argv) { /** Find the closest surface/line in the map **/ kdtreeSurfFromMap->nearestKSearch(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf); - if (pointSearchSqDis_surf[NUM_MATCH_POINTS - 1] < 3.0) + if (pointSearchSqDis_surf[NUM_MATCH_POINTS - 1] < 3) { point_selected_surf[i] = true; } @@ -773,13 +771,13 @@ int main(int argc, char** argv) //if(fabs(pd2) > 0.1) continue; float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel_tmpt.x * pointSel_tmpt.x + pointSel_tmpt.y * pointSel_tmpt.y + pointSel_tmpt.z * pointSel_tmpt.z)); - if (s > 0.1) + if (s > 0.92) { point_selected_surf[i] = true; - coeffSel_tmpt->points[i].x = s * pa; - coeffSel_tmpt->points[i].y = s * pb; - coeffSel_tmpt->points[i].z = s * pc; - coeffSel_tmpt->points[i].intensity = s * pd2; + coeffSel_tmpt->points[i].x = pa; + coeffSel_tmpt->points[i].y = pb; + coeffSel_tmpt->points[i].z = pc; + coeffSel_tmpt->points[i].intensity = pd2; } else { @@ -794,7 +792,7 @@ int main(int argc, char** argv) for (int i = 0; i < coeffSel_tmpt->points.size(); i++) { float error_abs = std::abs(coeffSel_tmpt->points[i].intensity); - if (point_selected_surf[i] && (error_abs < 0.5)) + if (point_selected_surf[i]) { laserCloudOri->push_back(feats_down->points[i]); coeffSel->push_back(coeffSel_tmpt->points[i]); @@ -802,7 +800,7 @@ int main(int argc, char** argv) } } - int laserCloudSelNum = laserCloudOri->points.size(); + laserCloudSelNum = laserCloudOri->points.size(); double ave_residual = total_residual / laserCloudSelNum; // ave_res_last @@ -886,14 +884,13 @@ int main(int argc, char** argv) euler_cur = RotMtoEuler(state.rot_end); #ifdef DEBUG_PRINT - // std::cout<<"solution: "<(0,0) = K * Hsub; state.cov = (I_STATE - G) * state.cov; - // std::cout<<"propagated cov: "<clear(); *laserCloudFullRes2 = dense_map_en ? (*feats_undistort) : (* feats_down); - // *laserCloudFullRes2 = dense_map_en ? (*laserCloudFullRes) : (* feats_down); int laserCloudFullResNum = laserCloudFullRes2->points.size(); - + pcl::PointXYZRGB temp_point; laserCloudFullResColor->clear(); + { for (int i = 0; i < laserCloudFullResNum; i++) { RGBpointBodyToWorld(&laserCloudFullRes2->points[i], &temp_point); @@ -971,6 +967,23 @@ int main(int argc, char** argv) laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar); laserCloudFullRes3.header.frame_id = "/camera_init"; pubLaserCloudFullRes.publish(laserCloudFullRes3); + } + + /******* Publish Effective points *******/ + // { + // laserCloudFullResColor->clear(); + // pcl::PointXYZRGB temp_point; + // for (int i = 0; i < laserCloudSelNum; i++) + // { + // RGBpointBodyToWorld(&laserCloudOri->points[i], &temp_point); + // laserCloudFullResColor->push_back(temp_point); + // } + // sensor_msgs::PointCloud2 laserCloudFullRes3; + // pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3); + // laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar); + // laserCloudFullRes3.header.frame_id = "/camera_init"; + // pubLaserCloudEffect.publish(laserCloudFullRes3); + // } /******* Publish Maps: *******/ // sensor_msgs::PointCloud2 laserCloudMap;