add plane constraint

CMakeLists.txt

@@ -12,5 +12,5 @@ include_directories(${OpenCV_INCLUDE_DIRS})
 find_package(Ceres)
 include_directories(${CERES_INCLUDE_DIRS})
 
-add_executable(gnss_odom_optimizer gnss_odom_optimizer.cc pose_local_parameterization.cc io.cc)
-target_link_libraries(gnss_odom_optimizer ${OpenCV_LIBS} ${CERES_LIBRARIES})
+add_executable(gnss_odom_optimizer gnss_odom_optimizer.cc pose_local_parameterization.cc io.cc regress_plane.cc)
+target_link_libraries(gnss_odom_optimizer ${CERES_LIBRARIES})

data/zero_utm

@@ -0,0 +1 @@
+442644.940007 4428711.144783 40.834135

gnss_data_factor.h

@@ -28,6 +28,7 @@ class GNSSDataFactor : public ceres::SizedCostFunction<2, 7>
                     jacobian_pose_i.setZero();
                     jacobian_pose_i.block<2, 3>(0, 0) = -Eigen::Matrix3d::Identity().block<2, 3>(0, 0);
                     jacobian_pose_i.block<2, 3>(0, 3) = (Ri * skewSymmetric(sensor_gnss_to_body_)).block<2, 3>(0, 0);
+                    jacobian_pose_i = sqrt_info * jacobian_pose_i;
                 } 
             }
             return true;

gnss_odom_optimizer.cc

@@ -5,14 +5,16 @@
 #include "io.h"
 #include "relative_pose_factor.h"
 #include "gnss_data_factor.h"
+#include "height_factor.h"
+#include "regress_plane.h"
 
 int main() {
 
     std::vector<std::pair<double, Eigen::Matrix<double, 3, 4> > > gt_poses;
     std::vector<std::pair<double, Eigen::Vector3d> > gnss_data;
 
-    std::string pose_file = "/data/cs55/mapping/2021-11-29/bag5/data/global_camera_pose.csv";
-    std::string gnss_file = "/data/cs55/mapping/2021-11-29/bag5/data/gnss_measure.csv";
+    std::string pose_file = "data/global_camera_pose.csv";
+    std::string gnss_file = "data/gnss_measure.csv";
 
     load_gtposes(pose_file, gt_poses);
     load_gnss_observations(gnss_file, gnss_data);
@@ -45,6 +47,20 @@ int main() {
         pose_graph.add_observation_factor(pose_id, factor);
     }
 
+    HDMapDataBase hdmap_database("data/hdmap");
+
+    double sensor_plane_to_body = 1.7;   
+    std::vector<std::pair<int, double> > pose_timestamps = pose_graph.pose_timestamps();
+    for (const auto & pose_timestamp : pose_timestamps) {
+        int id = pose_timestamp.first;
+        Eigen::Matrix<double,3,4> pose = pose_graph.get_pose(id);
+        Eigen::Vector3d xyz = pose.block<3,1>(0,3);
+        double height;
+        hdmap_database.construct_plane_height_constraint(xyz, 50., height);
+        HeightFactor * factor = new HeightFactor(height, sensor_plane_to_body);
+        pose_graph.add_observation_factor(id, factor);
+    } 
+
     pose_graph.solve();
 
     pose_graph.dump("solved_poses.csv");

height_factor.h

@@ -0,0 +1,38 @@
+#include <ceres/ceres.h>
+#include <Eigen/Dense>
+#include "util.h"
+
+class HeightFactor : public ceres::SizedCostFunction<1, 7>
+{
+    public:
+        HeightFactor(const double &measure, const double & sensor_plane_to_body) : 
+            measure_(measure), sensor_plane_to_body_(sensor_plane_to_body) {
+            sqrt_info = 100.0;
+        }
+
+        virtual bool Evaluate(double const *const *parameteres, double *residuals, double **jacobians) const {
+
+            Eigen::Vector3d Pi = Eigen::Vector3d(parameteres[0][0], parameteres[0][1], parameteres[0][2]);
+            Eigen::Quaterniond Qi = Eigen::Quaterniond(parameteres[0][6], parameteres[0][3], parameteres[0][4], parameteres[0][5]);
+
+            double Pi_est = Pi.z() - sensor_plane_to_body_;
+
+            residuals[0] = sqrt_info * (measure_ - Pi_est);
+
+            if (jacobians) {
+                Eigen::Matrix3d Ri = Qi.toRotationMatrix();
+
+                if (jacobians[0]) {
+                    Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_i(jacobians[0]);
+                    jacobian_pose_i.setZero();
+                    jacobian_pose_i(0, 2) = -1;
+                    jacobian_pose_i = sqrt_info * jacobian_pose_i;
+                } 
+            }
+            return true;
+        }
+
+        double measure_;
+        double sensor_plane_to_body_;
+        double sqrt_info;
+};

io.cc

@@ -60,7 +60,7 @@ void load_gnss_observations(const std::string & file_path, std::vector<std::pair
     std::vector<std::string> csv_coord;
     while (getline(gnsss_file, line))
     {
-        csv_coord = split(line, " ");
+        csv_coord = split(line, ",");
         // double qw, qx, qy, qz, tx, ty, tz;
         double timestamp = stod(csv_coord[0]);
         double gnss_x = std::stod(csv_coord[1]);

pose_graph.h

@@ -25,6 +25,25 @@ class PoseGraph {
         problem_.AddParameterBlock(para_poses_.at(pose_id).data(), 7, local_parameterization);
     }
 
+    Eigen::Matrix<double, 3, 4> get_pose(int pose_id) {
+        Eigen::Matrix<double, 3, 4> pose;
+        const std::vector<double> & para_pose = para_poses_.at(pose_id);
+        Eigen::Map<const Eigen::Vector3d> t(para_pose.data());
+        pose.block<3,1>(0,3) = t;
+
+        Eigen::Map<const Eigen::Quaterniond> q(para_pose.data() + 3);
+        pose.block<3,3>(0,0) = q.toRotationMatrix();
+        return pose;
+    }
+
+    std::vector<std::pair<int, double> > pose_timestamps() {
+        return pose_timestamps_;
+    }
+
+    size_t size() {
+        return pose_timestamps_.size(); 
+    }
+
     void add_relative_pose_factor(int pose_id, int pose_id_second, const Eigen::Matrix<double,3,4> & rel_pose) {
 
         RelativePoseFactor * factor = new RelativePoseFactor(rel_pose);
@@ -57,14 +76,14 @@ class PoseGraph {
         ceres::Solve(options, &problem_, &summary);
     }
 
-    void dump(std::string dump_path) {
+    void dump(std::string dump_path, char delim = ',') {
         std::fstream fout(dump_path, std::ios::out);
         for (int i = 0; i < pose_timestamps_.size(); ++i) {
             int pose_id = pose_timestamps_.at(i).first;
             const std::vector<double> & para_pose = para_poses_.at(pose_id);
             fout << std::fixed << pose_timestamps_.at(i).second;
             for (int k = 0; k < 7; ++k) {
-                fout << " " << para_pose.at(k);
+                fout << delim << para_pose.at(k);
             }
             fout << std::endl;
         }

regress_plane.cc

@@ -1,35 +1,8 @@
-#include <iostream>
-#include <fstream>
-#include <string>
-#include <cstring>
-#include <map>
-#include <vector>
-#include <cmath>
-#include <iomanip>
-#include </usr/include/eigen3/Eigen/Dense>
-#include <math.h>
+#include "regress_plane.h"
 
-using namespace std;
-using namespace Eigen;
-
-
-typedef struct
+std::vector<std::string> string_split(const std::string &str, const char *delim)
 {
-    int id;
-    vector<Vector3d> xyz;
-} Lane;
-
-typedef struct 
-{
-    double timestamp;
-    Vector3d xyz;
-    Matrix3d rot;
-} Pose;
-
-
-vector<string> string_split(const string &str, const char *delim)
-{
-	vector <std::string> strlist;
+	std::vector <std::string> strlist;
 	int size = str.size();
 	char *input = new char[size+1];
 	strcpy(input, str.c_str());
@@ -42,137 +15,89 @@ vector<string> string_split(const string &str, const char *delim)
 	return strlist;
 }
 
-void load_lane_from_hdmap(string txt_path, vector<Lane> *lane_dict)
+HDMapDataBase::HDMapDataBase(std::string txt_path) {
+    load_lane_from_hdmap(txt_path);
+}
+
+void HDMapDataBase::load_lane_from_hdmap(std::string txt_path)
 {   
-    ifstream inFile(txt_path, ios::in);
-    string x;
+    std::ifstream inFile(txt_path, std::ios::in);
+    std::string x;
     while (getline(inFile, x))
     {
         Lane lane;
-        vector<string> line = string_split(x, ",");
+        std::vector<std::string> line = string_split(x, ",");
         if (line[0] == "fid(T)")
             continue; // skip head
         lane.id = stoi(line[0]);
-        vector<string> xyz_str = string_split(line[1], " ");
+        std::vector<std::string> xyz_str = string_split(line[1], " ");
         for (int i=0; i<xyz_str.size(); i+=3){
             // cout << stod(xyz_str[i]) << " " << stod(xyz_str[i+1]) << " " << stod(xyz_str[i+2]) << endl;
-            Vector3d xyz;
-            xyz << stod(xyz_str[i]), stod(xyz_str[i+1]), stod(xyz_str[i+2]); 
+            Eigen::Vector3d xyz;
+            xyz << std::stod(xyz_str[i]), std::stod(xyz_str[i+1]), std::stod(xyz_str[i+2]); 
             lane.xyz.push_back(xyz);
         }
         // cout << lane.id << " " << lane.xyz.size() << endl;
-        lane_dict->push_back(lane);
+        lane_dict_.push_back(lane);
     }
 }
 
-void get_xyz_from_lane(vector<Lane> lane_dict, vector<Vector3d> *xyz)
+void HDMapDataBase::get_xyz_from_lane(std::vector<Eigen::Vector3d> & xyz)
 {
-    for (int i=0; i<lane_dict.size(); i++)
-        for (int j=0; j<lane_dict[i].xyz.size(); j++)
+    xyz.clear();
+    for (int i=0; i<lane_dict_.size(); i++)
+        for (int j=0; j<lane_dict_[i].xyz.size(); j++)
         {
-            xyz->push_back(lane_dict[i].xyz[j]);
+            xyz.push_back(lane_dict_[i].xyz[j]);
         }
 }
 
-vector<Vector3d> find_neighbors(Vector3d camera_xyz, vector<Vector3d> lane_xyz, double radius)
+std::vector<Eigen::Vector3d> HDMapDataBase::find_neighbors(Eigen::Vector3d camera_xyz, double radius)
 {
-    vector<Vector3d> near_xyz;
-    for (int i=0; i<lane_xyz.size(); i++)
-    {
-        double dst = (camera_xyz - lane_xyz[i]).norm();
-        if (dst <= radius)
-        {
-            near_xyz.push_back(lane_xyz[i]);
+    std::vector<Eigen::Vector3d> near_xyz;
+    for (int i=0; i<lane_dict_.size(); i++) {
+        for (int j=0; j<lane_dict_[i].xyz.size(); j++) {
+            double dst = (camera_xyz - lane_dict_[i].xyz[j]).norm();
+            if (dst <= radius) {
+                near_xyz.push_back(lane_dict_[i].xyz[j]);
+            }
         }
     }
     return near_xyz;
 }
 
+double calc_dst_point_to_plane(Eigen::Vector3d camera_xyz, Eigen::Vector4d coeff)
+{
+	double d = fabs((camera_xyz.dot(coeff.topRows(3))+ coeff[3]) / coeff.topRows(3).norm());
+	return d;
+}
+
 // https://www.licc.tech/article?id=72
 // ax+by+cz+d=0
-Vector4d plane_fitting(const vector<Vector3d> & plane_pts) 
+Eigen::Vector4d plane_fitting(const std::vector<Eigen::Vector3d> & plane_pts) 
 {
-    Vector3d center = Eigen::Vector3d::Zero();
+    Eigen::Vector3d center = Eigen::Vector3d::Zero();
     for (const auto & pt : plane_pts) center += pt;
     center /= plane_pts.size();
 
-    MatrixXd A(plane_pts.size(), 3);
+    Eigen::MatrixXd A(plane_pts.size(), 3);
     for (int i = 0; i < plane_pts.size(); i++) {
         A(i, 0) = plane_pts[i][0] - center[0];
         A(i, 1) = plane_pts[i][1] - center[1];
         A(i, 2) = plane_pts[i][2] - center[2];
     }
 
-    JacobiSVD<MatrixXd> svd(A, ComputeThinV);
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinV);
     const float a = svd.matrixV()(0, 2);
     const float b = svd.matrixV()(1, 2);
     const float c = svd.matrixV()(2, 2);
     const float d = -(a * center[0] + b * center[1] + c * center[2]);
-    return Vector4d(a, b, c, d);
-}
-
-double calc_dst_point_to_plane(Vector3d camera_xyz, Vector4d coeff)
-{
-	double d = fabs((camera_xyz.dot(coeff.topRows(3))+ coeff[3]) / coeff.topRows(3).norm());
-	return d;
+    return Eigen::Vector4d(a, b, c, d);
 }
 
-
-void transform_from_lidar_to_camera(Pose lidar_pose, Pose * camera_pose)
-{
-    Matrix4d camera_to_lidar;
-    camera_to_lidar << 0.0264, -0.0089, -0.9996, -0.50,
-                    0.9995, -0.0176, 0.0266, 0.06,
-                    -0.0178, -0.9998, 0.0085, 0,
-                    0, 0, 0, 1;
-    camera_pose->xyz << lidar_pose.rot * camera_to_lidar.col(3).topRows(3) + lidar_pose.xyz;
-}
-
-
-int main()
-{
-    vector<Lane> lane_dict;
-    vector<Vector3d> all_xyz, near_xyz;
-    string lane_hdmap_txt = "/home/jinyu/Documents/HDMapProjector/data/map/100101-bxdp_line.utm.txt";
-    double radius = 250.0;
-
-    load_lane_from_hdmap(lane_hdmap_txt, &lane_dict);
-    cout << "Loaded " << lane_dict.size() << " lane from " << lane_hdmap_txt << endl;
-    get_xyz_from_lane(lane_dict, &all_xyz);
-    cout << "Loaded " << all_xyz.size() << " Points from " << lane_hdmap_txt << endl;
-
-    ifstream inFile("/home/jinyu/Documents/HDMapProjector/data/poses/global_lidar_pose.csv", ios::in);
-    string x;
-    while (getline(inFile, x))
-    {
-        Pose lidar_pose, camera_pose;
-        vector<string> line = string_split(x, ",");
-        lidar_pose.timestamp = stod(line[0]);
-        camera_pose.timestamp = stod(line[0]);
-        lidar_pose.xyz << stod(line[1]), stod(line[2]), stod(line[3]);
-        Quaterniond quat(stod(line[4]), stod(line[5]), stod(line[6]), stod(line[7]));
-        lidar_pose.rot = quat.toRotationMatrix();
-        // cout << "TimeStamp: " << camera_pose.timestamp << ", Position: (" << lidar_pose.xyz.transpose() << ")" << endl;
-        // cout << lidar_pose.rot << endl;
-
-        transform_from_lidar_to_camera(lidar_pose, &camera_pose);
-        cout << "TimeStamp: " << camera_pose.timestamp << ", Position: (" << camera_pose.xyz.transpose() << ")" << endl;
-
-        near_xyz = find_neighbors(camera_pose.xyz, all_xyz, radius);
-        if (near_xyz.size() <= 20)
-            continue;
-        else
-            cout << near_xyz.size() << " neighbors are found." << endl;
-
-        Vector4d coefficient;
-        coefficient = plane_fitting(near_xyz);
-        for (int i=0; i<4; i++)
-            cout << coefficient[i] << " ";
-        cout << endl;
-        double hd = calc_dst_point_to_plane(camera_pose.xyz, coefficient);
-        cout << hd << endl << endl;
-
-    }
-
-    return 0;
+bool HDMapDataBase::construct_plane_height_constraint(Eigen::Vector3d camera_xyz, double radius, double & camera_height) {
+    std::vector<Eigen::Vector3d> near_xyz = find_neighbors(camera_xyz, radius);
+    Eigen::Vector4d coeff = plane_fitting(near_xyz);
+    camera_height = calc_dst_point_to_plane(camera_xyz, coeff);
+    return true;
 }