Skip to content

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

处理坐标转换的bug

parent 130f0b45
Hide whitespace changes
Inline Side-by-side
Showing with 173 additions and 155 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,170 +34,186 @@ namespace jf {
}
bool Track::init(const std::string& track_file) {
ifstream ifs(track_file);
if(!ifs) {
cout << "can not init track from file!" << endl;
return false;
}
bool ret = false;
string line;
double lat = 0, lng = 0, h = 0;
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) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> lng;
}
if(line.find("height:") != string::npos) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> h;
station = new Station(lat, lng, h);
//cout << "lat, lon, h : " << lat << " " << lng << " " << h << endl;
}
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) {
LOG(ERROR) << "track file has unsupported field structure...";
return false;
}
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") {
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)
continue;
TrackPoint track_point;
std::string str = line;
//istringstream iss(line);
bool Track::init(const std::string &track_file) {
ifstream ifs(track_file);
if (!ifs) {
cout << "can not init track from file!" << endl;
return false;
}
bool ret = false;
string line;
double lat = 0, lng = 0, h = 0;
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) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> lng;
}
if (line.find("height:") != string::npos) {
istringstream iss(line);
string tmp_str;
iss >> tmp_str >> h;
station = new Station(lat, lng, h);
//cout << "lat, lon, h : " << lat << " " << lng << " " << h << endl;
}
if (line.find("track_points") != string::npos) {
ret = true;
point_list.clear();
std::string fields = line;
std::vector<std::string> vec_fields;
Utility::SplitString(str, vec_fields, " ");
if(vec_fields.size() != 7)
continue;
track_point.lat = std::atof(vec_fields[0].c_str());
track_point.lng = std::atof(vec_fields[1].c_str());
track_point.height = std::atof(vec_fields[2].c_str());
track_point.yaw = std::atof(vec_fields[3].c_str());
track_point.pitch = std::atof(vec_fields[4].c_str());
track_point.roll = std::atof(vec_fields[5].c_str());
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;
}
}
}
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;
station->TransFromEpsg4326(&n, &e, &u);
coord_list.emplace_back(std::move(
LocalCoordinate(e, n, u)
));
} else {
coord_list.push_back(LocalCoordinate(track_point.lat, track_point.lng, track_point.height));
}
}
return true;
}
void Track::getSegCoordLists() {
seg_coord_lists.clear();
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) {
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) {
seg_lc_list.push_back(coord_list[i]);
Utility::SplitString(fields, vec_fields, " ");
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") {
is_track_latlon = true;
} 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)
continue;
TrackPoint track_point;
std::string str = line;
//istringstream iss(line);
std::vector<std::string> vec_fields;
Utility::SplitString(str, vec_fields, " ");
if (vec_fields.size() != 7)
continue;
track_point.lat = std::atof(vec_fields[0].c_str());
track_point.lng = std::atof(vec_fields[1].c_str());
track_point.height = std::atof(vec_fields[2].c_str());
track_point.yaw = std::atof(vec_fields[3].c_str());
track_point.pitch = std::atof(vec_fields[4].c_str());
track_point.roll = std::atof(vec_fields[5].c_str());
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;
}
}
}
else if(!seg_lc_list.empty()){
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;
station->TransFromEpsg4326(&n, &e, &u);
coord_list.emplace_back(std::move(
LocalCoordinate(e, n, u)
));
} else {
coord_list.push_back(LocalCoordinate(track_point.lat, track_point.lng, track_point.height));
}
}
return true;
}
void Track::getSegCoordLists() {
seg_coord_lists.clear();
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) {
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) {
seg_lc_list.push_back(coord_list[i]);
} 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]);
}
last_lc = coord_list[i];
}
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]);
}
last_lc = coord_list[i];
}
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) {
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::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);
return true;
}
bool Track::LocalXYZ2UTM(StationInfo stationInfo) {
OGRSpatialReference* resutm51 = new OGRSpatialReference();
OGRErr err = resutm51->SetUTM(stationInfo.srid, 1);
if(err != OGRERR_NONE) {
LOG(ERROR) << CPLGetLastErrorMsg();
}
OGRCoordinateTransformation* trans2UTM = nullptr;
OGRSpatialReference* dst_wgs84 = OGRSpatialReference::GetWGS84SRS();
trans2UTM = OGRCreateCoordinateTransformation(dst_wgs84, resutm51);
if(nullptr == trans2UTM) {
LOG(ERROR) << "create coordinate transformation object failed!";
return false;
}
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;
trans2UTM->Transform(1, &utm_x, &utm_y, &utm_z);
bool Track::LocalXYZ2UTM(StationInfo stationInfo) {
OGRSpatialReference *resutm51 = new OGRSpatialReference();
OGRErr err = resutm51->SetUTM(stationInfo.srid, 1);
if (err != OGRERR_NONE) {
LOG(ERROR) << CPLGetLastErrorMsg();
}
OGRCoordinateTransformation *trans2UTM = nullptr;
OGRSpatialReference *dst_wgs84 = OGRSpatialReference::GetWGS84SRS();
trans2UTM = OGRCreateCoordinateTransformation(dst_wgs84, resutm51);
if (nullptr == trans2UTM) {
LOG(ERROR) << "create coordinate transformation object failed!";
return false;
}
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;
trans2UTM->Transform(1, &utm_x, &utm_y, &utm_z);
utm_x -= stationInfo.utm_offset_x;
utm_y -= stationInfo.utm_offset_y;
utm_z -= stationInfo.utm_offset_z;
utm_coord_list.push_back(LocalCoordinate(utm_x, utm_y, utm_z));
}
coord_list = utm_coord_list;
return true;
utm_coord_list.push_back(LocalCoordinate(utm_x, utm_y, utm_z));
}
coord_list = utm_coord_list;
return true;
}
} // namespace jf
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