add hdmap visualization tools

gnss_odom_optimizer.cc

@@ -33,10 +33,11 @@ int main() {
                 gt_poses.at(id - 1).second.block<3,3>(0,0).transpose() * (pose.block<3,1>(0,3) - gt_poses.at(id - 1).second.block<3,1>(0,3));
 
             pose_graph.add_relative_pose_factor(id - 1, id, rel_pose);
+
         }   
     }     
 
-    Eigen::Vector3d sensor_gps_to_body = Eigen::Vector3d(0, 0, 0);   
+    Eigen::Vector3d sensor_gps_to_body = Eigen::Vector3d(0, 0.2770, -0.8861);   
 
     for (int idx = 0; idx < gnss_data.size(); ++idx) {
         double timestamp = gnss_data.at(idx).first;
@@ -50,16 +51,19 @@ int main() {
     // Uncomment to enable the plane constraint.
     HDMapDataBase hdmap_database("../hdmap/tsinghua/100101-bxdp_line.utm.txt");
 
-    double sensor_plane_to_body = 1.7;   
+    double sensor_plane_to_body = 1.89;   
+    double search_radius = 50.;
     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);
+        if (hdmap_database.construct_plane_height_constraint(xyz, search_radius, height)) {
+            std::cout << height + sensor_plane_to_body << std::endl;
+            HeightFactor * factor = new HeightFactor(height, sensor_plane_to_body);
+            pose_graph.add_observation_factor(id, factor);
+        }
     } 
 
     // Solve the pose graph.

height_factor.h

@@ -7,7 +7,7 @@ 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;
+            sqrt_info = 1000.0;
         }
 
         virtual bool Evaluate(double const *const *parameteres, double *residuals, double **jacobians) const {

io.cc

@@ -37,10 +37,10 @@ void load_gtposes(const std::string & file_path, std::vector<std::pair<double, E
         double gt_x = std::stod(csv_coord[1]);
         double gt_y = std::stod(csv_coord[2]);
         double gt_height = std::stod(csv_coord[3]);
-        double gt_qw = std::stod(csv_coord[4]);
-        double gt_qx = std::stod(csv_coord[5]);
-        double gt_qy = std::stod(csv_coord[6]);
-        double gt_qz = std::stod(csv_coord[7]);
+        double gt_qx = std::stod(csv_coord[4]);
+        double gt_qy = std::stod(csv_coord[5]);
+        double gt_qz = std::stod(csv_coord[6]);
+        double gt_qw = std::stod(csv_coord[7]);
 
         Eigen::Quaterniond G_R_V_gt(gt_qw, gt_qx, gt_qy, gt_qz);
 

pose_graph.h

@@ -67,13 +67,33 @@ class PoseGraph {
     }
 
     void solve() {
+
+        // reset the orientation to fix the orientation degree of freedom
+        int pose_id = pose_timestamps_.front().first;
+        Eigen::Map<const Eigen::Quaterniond> q(para_poses_.at(pose_id).data() + 3);
+        Eigen::Quaterniond qinit = q;
+        std::cout << qinit.toRotationMatrix() << std::endl;
+
         ceres::Solver::Options options;
         options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
         options.minimizer_progress_to_stdout = true;
         options.max_num_iterations = 50;
 
         ceres::Solver::Summary summary;
         ceres::Solve(options, &problem_, &summary);
+
+        Eigen::Map<const Eigen::Quaterniond> q0(para_poses_.at(pose_id).data() + 3);
+        std::cout << q0.toRotationMatrix() << std::endl;
+
+        Eigen::Quaterniond qdiff = q0.inverse() * qinit;
+        std::cout << qdiff.toRotationMatrix() << std::endl;
+
+        for (int i = 0; i < pose_timestamps_.size(); ++i) {
+            int pose_id = pose_timestamps_.at(i).first;
+            std::vector<double> & para_pose = para_poses_[pose_id];
+            Eigen::Map<Eigen::Quaterniond> q(para_pose.data() + 3);
+            q = q * qdiff;
+        }
     }
 
     void dump(std::string dump_path, char delim = ',') {

regress_plane.cc

@@ -85,12 +85,21 @@ bool plane_fitting(const std::vector<Eigen::Vector3d> & plane_pts,
     }
 
     Eigen::MatrixXd AtA = A.transpose() * A;
-    Eigen::JacobiSVD<Eigen::MatrixXd> svd(AtA, Eigen::ComputeThinU | Eigen::ComputeThinV);
-    Eigen::LLT<Eigen::MatrixXd> cholSolver(AtA);
-    if (cholSolver.info() != Eigen::Success) 
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(AtA);
+    if (eigen_solver.info() != Eigen::Success) {
+        std::cout << "SelfAdjointEigenSolver fails." << std::endl;
         return false;
-
-    plane_coeff = AtA.llt().solve(A.transpose() * b);
+    }
+    const Eigen::Vector3d& eigen_values = eigen_solver.eigenvalues();
+    if (eigen_values(0) < 1e-6 || std::abs(eigen_values(2) / eigen_values(0)) > 1000) {
+        std::cout << "Too big eigen value ratio." << std::endl;
+        return false;
+    } 
+    Eigen::LLT<Eigen::MatrixXd> cholSolver = AtA.llt();
+    if (cholSolver.info() != Eigen::Success) {
+        return false;
+    }
+    plane_coeff = cholSolver.solve(A.transpose() * b);
     return true;
 }
 
@@ -101,5 +110,5 @@ bool HDMapDataBase::construct_plane_height_constraint(Eigen::Vector3d camera_xyz
         road_height = coeff.z();
         return true;
     } 
-    return true;
+    return false;
 }

relative_pose_factor.h

@@ -11,21 +11,21 @@ class RelativePoseFactor : public ceres::SizedCostFunction<6, 7, 7>
         RelativePoseFactor(const Eigen::Matrix<double, 3, 4> & rel_pose) {
             rel_t_ = rel_pose.block<3, 1>(0, 3);
             rel_rot_ = Eigen::Quaterniond(rel_pose.block<3, 3>(0, 0));
-            sqrt_info = 100.0;
+            sqrt_info << 100.0, 100, 100, 1000, 1000, 1000;
         }
 
-        virtual bool Evaluate(double const *const *parameteres, double *residuals, double **jacobians) const {
+        virtual bool Evaluate(double const *const *parameters, 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]);
-            Eigen::Vector3d Pj = Eigen::Vector3d(parameteres[1][0], parameteres[1][1], parameteres[1][2]);
-            Eigen::Quaterniond Qj = Eigen::Quaterniond(parameteres[1][6], parameteres[1][3], parameteres[1][4], parameteres[1][5]);
+            Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
+            Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
+            Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
+            Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
 
             // Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err.  (Ti*Rel_pose)^-1 * Tj*[1, 1/2\theta] = 2[0 I]((Ti*Rel_pose)^-1 * Tj)_L * [0, 1/2I]
             // Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err.  (Ti*[1,1/2\theta]*Rel_pose)^-1 * Tj 
             // = -[0, I] * ((Tj*-1 * Ti*[1,1/2\theta]*Rel_pose)) = -2[0 I] * (Tj*-1 * Ti)_L * (Rel_pose)R * [0, 1/2I] 
             Eigen::Quaterniond Qj_est = Qi * rel_rot_;
-            Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi;
+            Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi; // FIXME: remove the rotation part.
 
             Eigen::Quaterniond err = Qj_est.inverse() * Qj;
 
@@ -36,7 +36,7 @@ class RelativePoseFactor : public ceres::SizedCostFunction<6, 7, 7>
             else
                 residual.block<3, 1>(3, 0) = -2.0 * err.vec();
 
-            residual = sqrt_info * residual;
+            residual = sqrt_info.asDiagonal() * residual;
 
             if (jacobians) {
                 Eigen::Matrix3d Ri = Qi.toRotationMatrix();
@@ -47,24 +47,96 @@ class RelativePoseFactor : public ceres::SizedCostFunction<6, 7, 7>
                     jacobian_pose_i.setZero();
                     jacobian_pose_i.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
                     jacobian_pose_i.block<3, 3>(0, 3) = Ri * skewSymmetric(rel_t_);
-                    jacobian_pose_i.block<3, 3>(3, 3) = Qleft(Qj_est).block<3,3>(1,1);
-                    jacobian_pose_i = sqrt_info * jacobian_pose_i;
+                    jacobian_pose_i.block<3, 3>(3, 3) = -(Qleft(Qj.inverse() * Qi) * Qright(rel_rot_)).block<3,3>(1,1);
+                    jacobian_pose_i = sqrt_info.asDiagonal() * jacobian_pose_i;
                 } 
                 if (jacobians[1]) {
                     Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > jacobian_pose_j(jacobians[1]); 
                     jacobian_pose_j.setZero();
                     jacobian_pose_j.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
                     jacobian_pose_j.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity();
-                    jacobian_pose_j.block<3, 3>(3, 3) = -(Qleft(Qj.inverse() * Qi) * Qright(rel_rot_)).block<3,3>(1,1);
-                    jacobian_pose_j = sqrt_info * jacobian_pose_j;
+                    jacobian_pose_j.block<3, 3>(3, 3) = Qleft(err).block<3,3>(1,1);
+                    jacobian_pose_j = sqrt_info.asDiagonal() * jacobian_pose_j;
                 }
             }
             return true;
         }
 
+        void check(double const *const *parameters) {
+
+            double * res = new double [6];
+            double ** jaco = new double * [2];
+            jaco[0] = new double [6 * 7];
+            jaco[1] = new double [6 * 7];
+            Evaluate(parameters, res, jaco);
+
+            std::cout << "my" << std::endl;
+            std::cout << Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> >(jaco[0]) << std::endl;
+            std::cout << Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> >(jaco[1]) << std::endl;
+
+            Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
+            Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
+            Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
+            Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
+
+            // Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err.  (Ti*Rel_pose)^-1 * Tj*[1, 1/2\theta] = 2[0 I]((Ti*Rel_pose)^-1 * Tj)_L * [0, 1/2I]
+            // Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err.  (Ti*[1,1/2\theta]*Rel_pose)^-1 * Tj 
+            // = -[0, I] * ((Tj*-1 * Ti*[1,1/2\theta]*Rel_pose)) = -2[0 I] * (Tj*-1 * Ti)_L * (Rel_pose)R * [0, 1/2I] 
+            Eigen::Quaterniond Qj_est = Qi * rel_rot_;
+            Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi;
+
+            Eigen::Quaterniond err = Qj_est.inverse() * Qj;
+
+            Eigen::Matrix<double, 6, 1> residual;
+            residual.block<3, 1>(0, 0) = Pj - Pj_est;
+            if (err.w() > 0)
+                residual.block<3, 1>(3, 0) = 2.0 * err.vec();
+            else
+                residual.block<3, 1>(3, 0) = -2.0 * err.vec();
+
+            residual = sqrt_info.asDiagonal() * residual;
+
+            const double eps = 1e-6;
+            Eigen::Matrix<double, 6, 12> num_jacobian;
+            for (int k = 0; k < 12; ++k) {
+                Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
+                Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
+                Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
+                Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
+
+                int a = k / 3, b = k % 3;
+                Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;
+                if (a == 0) 
+                    Pi += delta;
+                else if (a == 1)
+                    Qi = Qi * deltaQ(delta);
+                else if (a == 2)
+                    Pj += delta;
+                else if (a == 3)
+                    Qj = Qj * deltaQ(delta);
+
+                Eigen::Quaterniond Qj_est = Qi * rel_rot_;
+                Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi;
+
+                Eigen::Quaterniond err = Qj_est.inverse() * Qj;
+
+                Eigen::Matrix<double, 6, 1> tmp_residual;
+                tmp_residual.block<3, 1>(0, 0) = Pj - Pj_est;
+                if (err.w() > 0)
+                    tmp_residual.block<3, 1>(3, 0) = 2.0 * err.vec();
+                else
+                    tmp_residual.block<3, 1>(3, 0) = -2.0 * err.vec();
+
+                tmp_residual = sqrt_info.asDiagonal() * tmp_residual;
+                num_jacobian.col(k) = (tmp_residual - residual) / eps;
+            }
+            std::cout << "num" << std::endl;
+            std::cout << num_jacobian << std::endl;
+        }
+
         Eigen::Vector3d rel_t_;
         Eigen::Quaterniond rel_rot_;
-        double sqrt_info;
+        Eigen::Vector<double, 6, 1> sqrt_info;
 };
 
 #endif

tools/gen_image_list.py

@@ -0,0 +1,11 @@
+import os
+import glob
+
+image_dir = "./data/images/"
+frames = os.listdir(image_dir)
+frames.sort()
+frames = [f.replace(".jpg", "") + '\n' for f in frames]
+# labels.sort()
+
+with open("list.txt", "w") as f:
+    f.writelines(frames)

tools/plot_poses.py

@@ -41,6 +41,7 @@ def show_in_3d_map(self):
         ax.set_xlabel('X Axis')
         ax.set_ylabel('Y Axis')
         ax.set_zlabel('Z Axis')
+        # ax.axis('equal')
         plt.show()
 
     def show_in_2d_map(self):
@@ -76,7 +77,8 @@ def find_neighbors(query, sources, radius):
 
 
 if __name__ == '__main__':
-    plotter = PosePlotter('/home/jinyu/Documents/pose_graph/data/global_camera_pose.csv', '/home/jinyu/Documents/pose_graph/data/gnss_measure.csv')
+    # plotter = PosePlotter('../data/global_camera_pose.csv', '../data/gnss_measure.csv')
+    plotter = PosePlotter('../build/solved_poses.csv', '../data/gnss_measure.csv')
     plotter.show_in_3d_map() # show all the ground-truth trajectory and GNSS observations in 3D map
     plotter.show_in_2d_map() # show all the ground-truth trajectory and GNSS observations in 2D map 
     plotter.track_gnss_measurement_in_2d_map() # show one GNSS observation and its nearby ground-truth trajectory in 2D map

tools/pose_database.py

@@ -0,0 +1,47 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+from scipy.spatial.transform import Slerp
+from scipy import interpolate
+
+class PoseDataBase:
+    def __init__(self, gt_poses_file):
+        self.load_gt_poses(gt_poses_file)
+
+
+    def load_gt_poses(self, gt_poses_path):
+
+        with open(gt_poses_path, "r") as f:
+            poses = f.readlines()
+            poses = [p.strip() for p in poses]
+
+        qs, ts, key_times = [], [], []
+        for p in poses:
+            p = p.split(' ')
+            timestamp = float(p[0])
+            qs.append(np.array([float(x) for x in p[4:8]]))
+            ts.append(np.array([float(x) for x in p[1:4]]))
+            key_times.append(timestamp)
+
+        qs = np.array(qs)
+        self.rots = Rotation.from_quat(qs)
+        self.ts = np.array(ts)
+        self.key_times = np.array(key_times)
+        self.rot_slerp = Slerp(key_times, self.rots)
+        self.t_interp = interpolate.interp1d(key_times, self.ts, axis=0)
+
+    def query_poses(self, timestamps, extrinsic=None):
+        interp_rots = self.rot_slerp(timestamps)
+        interp_ts = self.t_interp(timestamps).reshape([-1, 3, 1])
+        interp_rots = interp_rots.as_matrix()
+        if extrinsic is None:
+            return np.concatenate([interp_rots, interp_ts], axis=2)
+        else:
+            return np.concatenate([interp_rots, interp_ts], axis=2).dot(extrinsic)
+
+    def filter_timestamps(self, timestamps, timestamps_str):
+        max_t = np.max(self.key_times)
+        min_t = np.min(self.key_times)
+        timestamps = timestamps[(timestamps > min_t) & (timestamps < max_t)]
+        index = (timestamps > min_t) & (timestamps < max_t)
+        timestamps_str = [timestamps_str[i] for i in range(len(index)) if index[i]]
+        return timestamps, timestamps_str