refine_motion_estimate_tester.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <iostream>
00003 
00004 #include <Eigen/Geometry>
00005 
00006 #include "../libfovis/refine_motion_estimate.hpp"
00007 
00008 #define dump(v) std::cerr << #v << " : " << (v) << "\n"
00009 #define dumpT(v) std::cerr << #v << " : " << (v).transpose() << "\n"
00010 
00011 using namespace fovis;
00012 
00013 static inline Eigen::Isometry3d
00014 isometryFromXYZRollPitchYaw(const Eigen::Matrix<double, 6, 1>& params)
00015 {
00016   Eigen::Isometry3d result;
00017 
00018   double roll = params(3), pitch = params(4), yaw = params(5);
00019   double halfroll = roll / 2;
00020   double halfpitch = pitch / 2;
00021   double halfyaw = yaw / 2;
00022   double sin_r2 = sin(halfroll);
00023   double sin_p2 = sin(halfpitch);
00024   double sin_y2 = sin(halfyaw);
00025   double cos_r2 = cos(halfroll);
00026   double cos_p2 = cos(halfpitch);
00027   double cos_y2 = cos(halfyaw);
00028 
00029   Eigen::Quaterniond quat(
00030     cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2,
00031     sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2,
00032     cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2,
00033     cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2);
00034 
00035   result.setIdentity();
00036   result.translate(params.head<3>());
00037   result.rotate(quat);
00038 
00039   return result;
00040 }
00041 
00042 static inline Eigen::Vector3d
00043 isometryGetRollPitchYaw(const Eigen::Isometry3d& M)
00044 {
00045   Eigen::Quaterniond q(M.rotation());
00046   double roll_a = 2 * (q.w()*q.x() + q.y()*q.z());
00047   double roll_b = 1 - 2 * (q.x()*q.x() + q.y()*q.y());
00048   double pitch_sin = 2 * (q.w()*q.y() - q.z()*q.x());
00049   double yaw_a = 2 * (q.w()*q.z() + q.x()*q.y());
00050   double yaw_b = 1 - 2 * (q.y()*q.y() + q.z()*q.z());
00051 
00052   return Eigen::Vector3d(atan2(roll_a, roll_b),
00053       asin(pitch_sin),
00054       atan2(yaw_a, yaw_b));
00055 }
00056 
00057 int main(int argc, char** argv)
00058 {
00059 
00060   int num_points = 9;
00061 
00062   Eigen::Matrix<double, 4, Eigen::Dynamic> points_xyz(4, num_points);
00063   Eigen::Matrix<double, 4, Eigen::Dynamic> transformed_xyz(4, num_points);
00064   Eigen::Matrix<double, 2, Eigen::Dynamic> ref_projections(2, num_points);
00065 
00066   double tx    = 1.0;
00067   double ty    = 1.5;
00068   double tz    = 0.5;
00069   double roll  = 10 * (M_PI / 180);
00070   double pitch =  1 * (M_PI / 180);
00071   double yaw   =  5 * (M_PI / 180);
00072   //    tx = 0;
00073   //    ty = 0;
00074   //    tz = 0;
00075   //    roll = 0;
00076   //    pitch = 0;
00077   //    yaw = 1 * (M_PI / 180);
00078 
00079   Eigen::Matrix<double, 6, 1> params;
00080   params << tx, ty, tz, roll, pitch, yaw;
00081   Eigen::Isometry3d true_motion = isometryFromXYZRollPitchYaw(params);
00082   double fx = 528;
00083   double cx = 320;
00084   double cy = 240;
00085 
00086   Eigen::Matrix<double, Eigen::Dynamic, 2> tmp(num_points, 2);
00087   tmp << 
00088     0, 0,
00089     320, 0,
00090     640, 0,
00091     0, 240,
00092     320, 240,
00093     640, 240,
00094     0, 480,
00095     320, 480,
00096     640, 480;
00097   ref_projections = tmp.transpose();
00098   double depths[] = {
00099     1, 0.5, 0.75,
00100     100, 1, 0.75,
00101     0.75, 0.5, 1,
00102   };
00103 
00104   Eigen::Matrix<double, 3, 4> K;
00105   K << fx, 0, cx, 0,
00106     0, fx, cy, 0,
00107     0, 0, 1, 0;
00108 
00109   for(int i=0; i<num_points; i++) {
00110     points_xyz(0, i) = depths[i] * (ref_projections(0, i) - cx) / fx;
00111     points_xyz(1, i) = depths[i] * (ref_projections(1, i) - cy) / fx;
00112     points_xyz(2, i) = depths[i];
00113     points_xyz(3, i) = 1;
00114 
00115     Eigen::Vector3d uvw = K * points_xyz.col(i);
00116     uvw(0) /= uvw(2);
00117     uvw(1) /= uvw(2);
00118     uvw(2) = 1;
00119 
00120     transformed_xyz.col(i) = true_motion.inverse().matrix() * points_xyz.col(i);
00121 
00122     Eigen::Vector4d t = transformed_xyz.col(i);
00123     Eigen::Vector4d p = points_xyz.col(i);
00124 
00125     printf("%3d : %6.2f %6.2f  ->  %6.2f %6.2f %6.2f  -> %7.3f %7.3f %7.3f\n",
00126         i,
00127         uvw(0), uvw(1),
00128         p(0), p(1), p(2),
00129         t(0), t(1), t(2));
00130   }
00131   printf("=======\n");
00132 
00133 
00134   Eigen::Isometry3d initial_estimate;
00135   initial_estimate.setIdentity();
00136   Eigen::Vector3d initial_rpy = isometryGetRollPitchYaw(initial_estimate);
00137   Eigen::Vector3d initial_trans = initial_estimate.translation();
00138 
00139   Eigen::Isometry3d estimate = refineMotionEstimate(transformed_xyz,
00140       ref_projections,
00141       fx, cx, cy,
00142       initial_estimate, 6);
00143 
00144   Eigen::Matrix<double, 3, 4> P = K * estimate.matrix();
00145 
00146   Eigen::Vector3d estimated_rpy = isometryGetRollPitchYaw(estimate);
00147   Eigen::Vector3d estimated_trans = estimate.translation();
00148   printf("       Estimate   True    Initial\n"
00149       "tx:    %6.2f %6.2f %6.2f\n"
00150       "ty:    %6.2f %6.2f %6.2f\n"
00151       "tz:    %6.2f %6.2f %6.2f\n"
00152       "roll:  %6.2f %6.2f %6.2f\n"
00153       "pitch: %6.2f %6.2f %6.2f\n"
00154       "yaw:   %6.2f %6.2f %6.2f\n",
00155       estimated_trans(0), tx, initial_trans(0), 
00156       estimated_trans(1), ty, initial_trans(1), 
00157       estimated_trans(2), tz, initial_trans(2), 
00158       estimated_rpy(0) * 180 / M_PI, roll * 180 / M_PI,  initial_rpy(0) * 180 / M_PI,
00159       estimated_rpy(1) * 180 / M_PI, pitch * 180 / M_PI, initial_rpy(1) * 180 / M_PI,
00160       estimated_rpy(2) * 180 / M_PI, yaw * 180 / M_PI,   initial_rpy(2) * 180 / M_PI);
00161 
00162   // compute reprojection error
00163   for(int i=0; i<num_points; i++) {
00164     Eigen::Vector3d uvw = P * transformed_xyz.col(i);
00165     double u = uvw(0) / uvw(2);
00166     double v = uvw(1) / uvw(2);
00167     double ref_u = ref_projections(0, i);
00168     double ref_v = ref_projections(1, i);
00169     double err_u = u - ref_u;
00170     double err_v = v - ref_v;
00171 
00172     printf("%3d  %6.1f %6.1f -> %6.1f %6.1f (%6.2f %6.2f)\n", i, ref_u, ref_v, u, v, err_u, err_v);
00173   }
00174 
00175   return 0;
00176 }


libfovis
Author(s): Albert Huang, Maurice Fallon
autogenerated on Thu Jun 6 2019 20:16:12