Skip to content

L
localize_for_ppk
Commit 41d1cc40 authored by wangdawei's avatar wangdawei
Browse files
Options

test

parent 435cc77c
Show whitespace changes
Inline Side-by-side
Showing with 54 additions and 52 deletions
apps/localize_test.cpp
View file @ 41d1cc40
......@@ -45,10 +45,10 @@ using namespace std;
PointCloudJ::Ptr raw_scan(new PointCloudJ); \
PointCloudJ::Ptr scan(new PointCloudJ); \
PointCloudJ::Ptr result(new PointCloudJ); \
pcl::io::loadPCDFile("/home/wdw/Downloads/test/1683806268.156870.pcd", *raw_scan); \
pcl::io::loadPCDFile("/home/wdw/Downloads/pcd(3)/1683748706.956934_base.pcd", *raw_scan); \
Eigen::Isometry3d mapPose; \
mapPose.translation() = Eigen::Vector3d(64.2653, 0372.09, 29.5085); \
Eigen::Vector3d mapPoseRpy(0.00638869, 00.0101079, 0002.42708); \
mapPose.translation() = Eigen::Vector3d(-21.0299381284723, 0266.663172243435, 029.9718818825938); \
Eigen::Vector3d mapPoseRpy(0.0154573707048036, 0.0165835359033508, 002.47772534260029); \
mapPose.linear() = Eigen::Matrix3d( \
Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ()) \
* Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY()) \
......@@ -119,9 +119,9 @@ void test_onemesh_voxel_map_matcher()
init_big_map_matcher_data();
juefx::VoxelMapMatcherOption vmap_option;
vmap_option.option.filter_resolution = 0.5;
vmap_option.option.filter_resolution = 1.2;
vmap_option.option.cloud_range = 80;
vmap_option.fast_option.accepted_score = 0.45;
vmap_option.fast_option.accepted_score = 0.35;
// vmap_option.fast_option.Yaw_search_window *= vmap_option.fast_option.init_Yaw_scale;
// vmap_option.fast_option.XY_search_window *= vmap_option.fast_option.init_XYZ_scale;
// vmap_option.fast_option.Z_search_window *= vmap_option.fast_option.init_XYZ_scale;
......
libs/localize/voxel_map/fast_voxel_matcher.cc
View file @ 41d1cc40
......@@ -377,7 +377,7 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
}
#ifdef VOXEL_MAP_FAST_DEBUG
printf("score candidate, scan %d, depth %d, offset %d %d %d, score %f, low_score %f\n",
candidate.scan->index, candidate.search_depth,
candidate.scan->yaw_index, candidate.search_depth,
candidate.offset.x(), candidate.offset.y(), candidate_xy.offset.z(),
candidate_xy.score, candidate_xy.low_resolution_score);
#endif
......@@ -396,7 +396,7 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
}
#ifdef VOXEL_MAP_FAST_DEBUG
printf("score candidate, scan %d, depth %d, offset %d %d %d, score %f/%f=%f, low_score %f\n",
candidate.scan->index, candidate.search_depth,
candidate.scan->yaw_index, candidate.search_depth,
candidate.offset.x(), candidate.offset.y(), candidate_xy.offset.z(),
above_ground_score, ground_score, candidate_xy.score,
candidate_xy.low_resolution_score);
......@@ -411,9 +411,11 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
no_map_cnt++;
}
}
// LOG(INFO) << "search_depth: " << candidates->front().search_depth
// << " grid_indexes: " << candidates->front().scan->grid_indexes.size()
// << ", no_map_cnt: " << no_map_cnt;
#ifdef VOXEL_MAP_FAST_DEBUG
LOG(INFO) << "search_depth: " << candidates->front().search_depth
<< " grid_indexes: " << candidates->front().scan->grid_indexes.size()
<< ", no_map_cnt: " << no_map_cnt;
#endif
}
if (0 != no_map_cnt) {
float multi = (float)candidates->front().scan->grid_indexes.size() /
......@@ -460,7 +462,7 @@ Candidate FastVoxelMatcher::BranchAndBound(const VoxelMap &voxel_map,
}
#ifdef VOXEL_MAP_FAST_DEBUG
printf("====>>====>> scan %d, depth %d, offset %d %d %d, score %f, low_score %f\n",
candidate.scan->index, candidate.search_depth,
candidate.scan->yaw_index, candidate.search_depth,
candidate.offset.x(), candidate.offset.y(), candidate.offset.z(),
candidate.score, candidate.low_resolution_score);
#endif
......@@ -481,7 +483,7 @@ Candidate FastVoxelMatcher::BranchAndBound(const VoxelMap &voxel_map,
}
#ifdef VOXEL_MAP_FAST_DEBUG
printf(">>>> scan %d, depth %d, offset %d %d %d, score %f, low_score %f\n",
candidate.scan->index, candidate.search_depth,
candidate.scan->yaw_index, candidate.search_depth,
candidate.offset.x(), candidate.offset.y(), candidate.offset.z(),
candidate.score, candidate.low_resolution_score);
#endif
......
libs/localize/voxel_map/voxel_map_interface.h
View file @ 41d1cc40
......@@ -3,7 +3,7 @@
//#include "voxel_map_stub.h"
#include "voxel_map/interface/voxel_map.h"
//#define VOXEL_MAP_DEBUG
//#define VOXEL_MAP_FAST_DEBUG
#define VOXEL_MAP_DEBUG
#define VOXEL_MAP_FAST_DEBUG
//#define VOXEL_MAP_CERES_DEBUG
//#define VOXEL_MAP_CERES_OUTPUT_NUM 0
libs/locate_system/adjust_ppk_by_locate/convert.cc
View file @ 41d1cc40
......@@ -826,7 +826,7 @@ void Convert::init(pandar_msgs::PandarPacket packet)
lidarmotorspeed = MOTOR_SPEED_600; //changing the speed,give enough size
}
m_iMotorSpeed = lidarmotorspeed;
printf("init mode: workermode: %x,return mode: %x,speed: %d\n",m_iWorkMode, m_iReturnMode, m_iMotorSpeed);
printf("init mode: workermode: %x,return mode: %x,speed: %d, m_iLaserNum: %d\n",m_iWorkMode, m_iReturnMode, m_iMotorSpeed, m_iLaserNum);
changeAngleSize();
changeReturnBlockSize();
......
libs/voxel_map/libs/area/maparea.h
View file @ 41d1cc40
......@@ -201,44 +201,44 @@ public:
}
auto ret = currMesh_->GetCellValue(type, index, length);
// std::ofstream ofs;
// ofs.open("/home/wdw/grid_use_" + to_string(type) + ".txt", std::ios::app);
// int32_t blockIndexX = index.x.blockId;
// int32_t blockIndexY = index.y.blockId;
// uint32_t gridIndexX = index.x.gridId;
// uint32_t gridIndexY = index.y.gridId;
// Vector2f gridCenter;
// gridCenter.x() = blockIndexX * 23.04 + (0.5 + gridIndexX) * 0.09 + currMesh_->GetOffset().x();
// gridCenter.y() = blockIndexY * 23.04 + (0.5 + gridIndexY) * 0.09 + currMesh_->GetOffset().y();
//// if(fabs(gridCenter.x() - (-0.045)) < 0.01
//// && fabs(gridCenter.y() - 9.315) < 0.01){
//// LOG(INFO) << "blockIndexX: " << blockIndexX << " blockIndexY: " << blockIndexY
//// << " gridIndexX: " << gridIndexX << " gridIndexY: " << gridIndexY
//// << " type: " << (uint16_t)type << " length: " << (uint16_t)length;
//// }
// if(ret == nullptr){
//// std::ofstream nullOfs;
//// nullOfs.open("/home/juefx/null_" + to_string(type) + ".txt", std::ios::app);
//// nullOfs << gridCenter.x() << "," << gridCenter.y() << "," << 0 << std::endl;
//// nullOfs.close();
// return nullptr;
std::ofstream ofs;
ofs.open("/home/wdw/grid_use_" + to_string(type) + ".txt", std::ios::app);
int32_t blockIndexX = index.x.blockId;
int32_t blockIndexY = index.y.blockId;
uint32_t gridIndexX = index.x.gridId;
uint32_t gridIndexY = index.y.gridId;
Vector2f gridCenter;
gridCenter.x() = blockIndexX * 23.04 + (0.5 + gridIndexX) * 0.09 + currMesh_->GetOffset().x();
gridCenter.y() = blockIndexY * 23.04 + (0.5 + gridIndexY) * 0.09 + currMesh_->GetOffset().y();
// if(fabs(gridCenter.x() - (-0.045)) < 0.01
// && fabs(gridCenter.y() - 9.315) < 0.01){
// LOG(INFO) << "blockIndexX: " << blockIndexX << " blockIndexY: " << blockIndexY
// << " gridIndexX: " << gridIndexX << " gridIndexY: " << gridIndexY
// << " type: " << (uint16_t)type << " length: " << (uint16_t)length;
// }
// for(int i = 0; i < length; i++){
// for(int32_t j = 0;
// j < (ret + i)->height / 3;
// j++){
// int32_t indexZ = j + (ret + i)->floor / 3;
// float point[4];
// point[0] = gridCenter.x();
// point[1] = gridCenter.y();
// point[2] = ((float)indexZ + 0.5) * 3 / 100;
// point[3] = (ret + i)->intensity;
// ofs << point[0] << "," << point[1] << ","
// << point[2] << "," << point[3] << std::endl;
// }
// }
// ofs.close();
if(ret == nullptr){
// std::ofstream nullOfs;
// nullOfs.open("/home/juefx/null_" + to_string(type) + ".txt", std::ios::app);
// nullOfs << gridCenter.x() << "," << gridCenter.y() << "," << 0 << std::endl;
// nullOfs.close();
return nullptr;
}
for(int i = 0; i < length; i++){
for(int32_t j = 0;
j < (ret + i)->height / 3;
j++){
int32_t indexZ = j + (ret + i)->floor / 3;
float point[4];
point[0] = gridCenter.x();
point[1] = gridCenter.y();
point[2] = ((float)indexZ + 0.5) * 3 / 100;
point[3] = (ret + i)->intensity;
ofs << point[0] << "," << point[1] << ","
<< point[2] << "," << point[3] << std::endl;
}
}
ofs.close();
return ret;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment