Skip to content

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

test

parent 94b6c2a8
Hide whitespace changes
Inline Side-by-side
Showing with 58 additions and 57 deletions
apps/CMakeLists.txt
View file @ c5240fe9
......@@ -87,26 +87,26 @@ target_link_libraries(localize_test PUBLIC
# dispatch
# serialize
# glog
# gflag
# gflags
# )
###############################################
# restore_feature_map
#add_executable(restore_feature_map extract.cpp)
add_executable(restore_feature_map extract.cpp)
#target_include_directories(restore_feature_map PUBLIC
# ${PCL_INCLUDE_DIRS}
# stream_info
# )
target_include_directories(restore_feature_map PUBLIC
${PCL_INCLUDE_DIRS}
stream_info
)
#target_link_libraries(restore_feature_map PUBLIC
# ${PCL_LIBRARIES}
# ${Boost_LIBRARIES}
# ${GLOG_LIBRARIES}
# ${GFLAGS_LIBRARIES}
# stream_info
# glog
# gflag
# )
target_link_libraries(restore_feature_map PUBLIC
${PCL_LIBRARIES}
${Boost_LIBRARIES}
${GLOG_LIBRARIES}
${GFLAGS_LIBRARIES}
stream_info
glog
gflags
)
###############################################
# stream_test
#add_executable(stream_test stream_test.cpp)
......
apps/localize_test.cpp
View file @ c5240fe9
......@@ -41,14 +41,14 @@ using namespace std;
Eigen::Isometry3f final_pose;
#define init_big_map_matcher_data() \
std::string base_dir = "/home/wdw/Downloads/329546503.stream"; \
std::string base_dir = "/home/wdw/Downloads/329546499/329546499.stream"; \
PointCloudJ::Ptr raw_scan(new PointCloudJ); \
PointCloudJ::Ptr scan(new PointCloudJ); \
PointCloudJ::Ptr result(new PointCloudJ); \
pcl::io::loadPCDFile("/home/wdw/Downloads/pcd/1683806268.156870.pcd", *raw_scan); \
pcl::io::loadPCDFile("/home/wdw/Downloads/1683806268.056868.pcd", *raw_scan); \
Eigen::Isometry3d mapPose; \
mapPose.translation() = Eigen::Vector3d( 82.0294, 0370.65, 030.281); \
Eigen::Vector3d mapPoseRpy(0.00306673, 00.0165335, 0002.68098); \
mapPose.translation() = Eigen::Vector3d(64.7009, 372.594, 29.5076); \
Eigen::Vector3d mapPoseRpy(0.00618679, 0.00999828, 00002.4279); \
mapPose.linear() = Eigen::Matrix3d( \
Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ()) \
* Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY()) \
......@@ -56,7 +56,7 @@ using namespace std;
GnssPoint blh_final_pose; \
float Movement = 0.139191344380379; \
float Rotation = 0.0263764802366495; \
int Precision = 1; \
int Precision = 3; \
Eigen::Isometry3f final_pose;
void test_ceres_map_matcher()
......
libs/voxel_map/libs/area/maparea.h
View file @ c5240fe9
......@@ -197,44 +197,44 @@ public:
}
auto ret = currMesh_->GetCellValue(type, index, length);
// std::ofstream ofs;
// ofs.open("/home/juefx/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;
// gridCenter.y() = blockIndexY * 23.04 + (0.5 + gridIndexY) * 0.09;
//// 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;
}
......
libs/voxel_map/libs/area/mesharea.cpp
View file @ c5240fe9
......@@ -63,6 +63,7 @@ void MeshArea::init()
offset_.x() = header.offset_x();
offset_.y() = header.offset_y();
offset_.z() = header.offset_z();
LOG(INFO) << header.mesh_id() << " offset_: " << offset_.transpose();
meshLevel_ = header.mesh_level();
meshCenter_.x() = header.latitude();
meshCenter_.y() = header.longtitude();
......
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