Skip to content

D
denpends
Commit e638e71e authored by xueyimeng's avatar xueyimeng
Browse files
Options

处理坐标转换的bug

parent 130f0b45
Show whitespace changes
Inline Side-by-side
Showing with 61 additions and 43 deletions
cloud_common/utility/track.cpp
View file @ e638e71e
......@@ -12,7 +12,7 @@
using namespace std;
namespace jf {
Track& Track::operator=(const jf::Track& track) {
Track &Track::operator=(const jf::Track &track) {
this->start_time = track.start_time;
this->end_time = track.end_time;
this->coord_list = track.coord_list;
......@@ -23,7 +23,7 @@ namespace jf {
return *this;
}
Track::Track(const Track& track) {
Track::Track(const Track &track) {
this->start_time = track.start_time;
this->end_time = track.end_time;
this->coord_list = track.coord_list;
......@@ -34,9 +34,9 @@ namespace jf {
}
bool Track::init(const std::string& track_file) {
bool Track::init(const std::string &track_file) {
ifstream ifs(track_file);
if(!ifs) {
if (!ifs) {
cout << "can not init track from file!" << endl;
return false;
}
......@@ -44,18 +44,18 @@ bool Track::init(const std::string& track_file) {
string line;
double lat = 0, lng = 0, h = 0;
while(getline(ifs, line)) {
if(line.find("latitude") != string::npos) {
while (getline(ifs, line)) {
if (line.find("latitude") != string::npos) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> lat;
}
if(line.find("longitude") != string::npos) {
if (line.find("longitude") != string::npos) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> lng;
}
if(line.find("height:") != string::npos) {
if (line.find("height:") != string::npos) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> h;
......@@ -64,27 +64,27 @@ bool Track::init(const std::string& track_file) {
}
if(line.find("track_points") != string::npos) {
if (line.find("track_points") != string::npos) {
ret = true;
point_list.clear();
std::string fields = line;
std::vector<std::string> vec_fields;
Utility::SplitString(fields, vec_fields, " ");
if(vec_fields.size() != 8) {
if (vec_fields.size() != 8) {
LOG(ERROR) << "track file has unsupported field structure...";
return false;
}
if(vec_fields[1] == "lat" && vec_fields[2] == "lon" && vec_fields[3] == "height") {
if (vec_fields[1] == "lat" && vec_fields[2] == "lon" && vec_fields[3] == "height") {
is_track_latlon = true;
} else if(vec_fields[1] == "x" && vec_fields[2] == "y" && vec_fields[3] == "z") {
} else if (vec_fields[1] == "x" && vec_fields[2] == "y" && vec_fields[3] == "z") {
is_track_latlon = false;
} else {
LOG(WARNING) << "track file has unsupported field structure...";
is_track_latlon = false;
}
while(getline(ifs, line)) {
if(line.length() < 10)
while (getline(ifs, line)) {
if (line.length() < 10)
continue;
TrackPoint track_point;
std::string str = line;
......@@ -92,7 +92,7 @@ bool Track::init(const std::string& track_file) {
//istringstream iss(line);
std::vector<std::string> vec_fields;
Utility::SplitString(str, vec_fields, " ");
if(vec_fields.size() != 7)
if (vec_fields.size() != 7)
continue;
track_point.lat = std::atof(vec_fields[0].c_str());
......@@ -104,19 +104,19 @@ bool Track::init(const std::string& track_file) {
track_point.loc_time = std::atol(vec_fields[6].c_str());
point_list.emplace_back(std::move(track_point));
}
if(!point_list.empty()) {
start_time = point_list.front().loc_time/1000.0;
end_time = point_list.front().loc_time/1000.0;
if (!point_list.empty()) {
start_time = point_list.front().loc_time / 1000.0;
end_time = point_list.front().loc_time / 1000.0;
}
}
}
return ret;
}
}
bool Track::generateCartesian() {
for(const auto& track_point : point_list) {
if(is_track_latlon) {
double e = track_point.lat, n=track_point.lng, u=track_point.height;
bool Track::generateCartesian() {
for (const auto &track_point : point_list) {
if (is_track_latlon) {
double e = track_point.lat, n = track_point.lng, u = track_point.height;
station->TransFromEpsg4326(&n, &e, &u);
coord_list.emplace_back(std::move(
LocalCoordinate(e, n, u)
......@@ -127,23 +127,22 @@ bool Track::generateCartesian() {
}
return true;
}
}
void Track::getSegCoordLists() {
void Track::getSegCoordLists() {
seg_coord_lists.clear();
if(coord_list.empty()) return;
if (coord_list.empty()) return;
LocalCoordinate last_lc = coord_list[0];
std::vector<LocalCoordinate> seg_lc_list;
for(size_t i=0; i<coord_list.size(); ++i) {
for (size_t i = 0; i < coord_list.size(); ++i) {
Eigen::Vector3d coor_now(coord_list[i].x, coord_list[i].y, coord_list[i].z);
Eigen::Vector3d coor_last(last_lc.x, last_lc.y, last_lc.z);
double dis = (coor_now-coor_last).norm();
if(dis < TRACK_SEG_DISTANCE) {
double dis = (coor_now - coor_last).norm();
if (dis < TRACK_SEG_DISTANCE) {
seg_lc_list.push_back(coord_list[i]);
}
else if(!seg_lc_list.empty()){
} else if (!seg_lc_list.empty()) {
seg_coord_lists.push_back(seg_lc_list);
seg_lc_list.clear();
seg_lc_list.push_back(coord_list[i]);
......@@ -151,44 +150,61 @@ void Track::getSegCoordLists() {
last_lc = coord_list[i];
}
if(!seg_lc_list.empty()){
if (!seg_lc_list.empty()) {
seg_coord_lists.push_back(seg_lc_list);
seg_lc_list.clear();
}
}
}
bool Track::LocalXYZ2latlon(LocalCoordinate local_xyz, double& lat, double& lng, double& height) {
if(nullptr == station) {
bool Track::LocalXYZ2latlon(LocalCoordinate local_xyz, double &lat, double &lng, double &height) {
if (nullptr == station) {
LOG(ERROR) << "station is not set...";
return false;
}
station->TransToEpsg4326(&local_xyz.x, &local_xyz.y, &local_xyz.z);
lng = local_xyz.x;
lat = local_xyz.y;
height = local_xyz.z;
return true;
}
}
bool Track::Latlon2LocalXYZ(double lat, double lon, double height, LocalCoordinate& local_xyz) {
if (nullptr == station) {
LOG(ERROR) << "station is not set...";
return false;
}
local_xyz.x = lon;
local_xyz.y = lat;
local_xyz.z = height;
station->TransFromEpsg4326(&local_xyz.x, &local_xyz.y, &local_xyz.z);
return true;
}
bool Track::LocalXYZ2UTM(StationInfo stationInfo) {
OGRSpatialReference* resutm51 = new OGRSpatialReference();
OGRSpatialReference *resutm51 = new OGRSpatialReference();
OGRErr err = resutm51->SetUTM(stationInfo.srid, 1);
if(err != OGRERR_NONE) {
if (err != OGRERR_NONE) {
LOG(ERROR) << CPLGetLastErrorMsg();
}
OGRCoordinateTransformation* trans2UTM = nullptr;
OGRCoordinateTransformation *trans2UTM = nullptr;
OGRSpatialReference* dst_wgs84 = OGRSpatialReference::GetWGS84SRS();
OGRSpatialReference *dst_wgs84 = OGRSpatialReference::GetWGS84SRS();
trans2UTM = OGRCreateCoordinateTransformation(dst_wgs84, resutm51);
if(nullptr == trans2UTM) {
if (nullptr == trans2UTM) {
LOG(ERROR) << "create coordinate transformation object failed!";
return false;
}
for(int i=0; i<coord_list.size(); i++) {
for (int i = 0; i < coord_list.size(); i++) {
LocalCoordinate local_coord = coord_list[i];
double lat, lon, height;
LocalXYZ2latlon(local_coord, lat, lon, height);
double utm_x=lon, utm_y=lat, utm_z=height;
double utm_x = lon, utm_y = lat, utm_z = height;
trans2UTM->Transform(1, &utm_x, &utm_y, &utm_z);
utm_x -= stationInfo.utm_offset_x;
utm_y -= stationInfo.utm_offset_y;
......
cloud_common/utility/track.h
View file @ e638e71e
......@@ -49,6 +49,8 @@ public:
bool LocalXYZ2latlon(LocalCoordinate local_xyz, double& lat, double& lon, double& height);
bool Latlon2LocalXYZ(double lat, double lon, double height, LocalCoordinate& local_xyz);
bool LocalXYZ2UTM(StationInfo stationInfo);
public:
......
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