处理坐标转换的bug

cloud_common/utility/track.cpp

@@ -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

@@ -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: