//*************************************************************************** TEST(GPSData, init) { // GPS Reading 1 will be ENU origin double t1 = 84831; Point3 NED1(0, 0, 0); LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, Geocentric::WGS84); // GPS Readin 2 double t2 = 84831.5; double E, N, U; enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U); Point3 NED2(N, E, -U); // Estimate initial state Pose3 T; Vector3 nV; boost::tie(T, nV) = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796); // Check values values EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4)); EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); Point3 expectedT(2.38461, -2.31289, -0.156011); EXPECT(assert_equal(expectedT, T.translation(), 1e-5)); }
/* ************************************************************************* */ double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 6> H2) const { Matrix13 D_local_point; double r = range(pose.translation(), H1, H2 ? &D_local_point : 0); if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); return r; }
void Frustum_Filter::init_z_near_z_far_depth ( const SfM_Data & sfm_data, const double zNear, const double zFar ) { // If z_near & z_far are -1 and structure if not empty, // compute the values for each camera and the structure const bool bComputed_Z = (zNear == -1. && zFar == -1.) && !sfm_data.structure.empty(); if (bComputed_Z) // Compute the near & far planes from the structure and view observations { for (Landmarks::const_iterator itL = sfm_data.GetLandmarks().begin(); itL != sfm_data.GetLandmarks().end(); ++itL) { const Landmark & landmark = itL->second; const Vec3 & X = landmark.X; for (Observations::const_iterator iterO = landmark.obs.begin(); iterO != landmark.obs.end(); ++iterO) { const IndexT id_view = iterO->first; const View * view = sfm_data.GetViews().at(id_view).get(); if (!sfm_data.IsPoseAndIntrinsicDefined(view)) continue; const Pose3 pose = sfm_data.GetPoseOrDie(view); const double z = Depth(pose.rotation(), pose.translation(), X); NearFarPlanesT::iterator itZ = z_near_z_far_perView.find(id_view); if (itZ != z_near_z_far_perView.end()) { if ( z < itZ->second.first) itZ->second.first = z; else if ( z > itZ->second.second) itZ->second.second = z; } else z_near_z_far_perView[id_view] = {z,z}; } } } else { // Init the same near & far limit for all the valid views for (Views::const_iterator it = sfm_data.GetViews().begin(); it != sfm_data.GetViews().end(); ++it) { const View * view = it->second.get(); if (!sfm_data.IsPoseAndIntrinsicDefined(view)) continue; if (z_near_z_far_perView.find(view->id_view) == z_near_z_far_perView.end()) z_near_z_far_perView[view->id_view] = {zNear, zFar}; } } }
/* ************************************************************************* */ BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) { // Cnb = DCMnb(Att); Matrix Cnb = A.rotation().matrix().transpose(); Vector p_rel_c = Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than // azi = atan2(p_rel_c(2),p_rel_c(1)); double azimuth = atan2(p_rel_c(1),p_rel_c(0)); // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2)); double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1))); return BearingS2(azimuth, elevation); }
/* ************************************************************************* */ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { #ifdef GTSAM_POSE3_EXPMAP return Logmap(T, H); #else Matrix3 DR; Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); if (H) { *H = I_6x6; H->topLeftCorner<3,3>() = DR; } Vector6 xi; xi << omega, T.translation().vector(); return xi; #endif }
/* ************************************************************************* */ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { // Cnb = DCMnb(Att); Matrix Cnb = A.rotation().matrix().transpose(); // Cbc = [0,0,1;0,1,0;-1,0,0]; Matrix Cbc = (Matrix(3,3) << 0.,0.,1., 0.,1.,0., -1.,0.,0.).finished(); // p_rel_c = Cbc*Cnb*(PosObj - Pos); Vector p_rel_c = Cbc*Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than // azi = atan2(p_rel_c(2),p_rel_c(1)); double azimuth = atan2(p_rel_c(1),p_rel_c(0)); // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2)); double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1))); return BearingS2(azimuth, elevation); }
/* ************************************************************************* */ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { if (H) *H = LogmapDerivative(p); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { Vector6 log; log << w, T; return log; } else { Matrix3 W = skewSymmetric(w / t); // Formula from Agrawal06iros, equation (14) // simplified with Mathematica, and multiplying in T to avoid matrix math double Tan = tan(0.5 * t); Vector3 WT = W * T; Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); Vector6 log; log << w, u; return log; } }
/* OpenGL draw function & timing */ static void draw(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); { // convert opengl coordinates into the document information coordinates glPushMatrix(); glMultMatrixf((GLfloat*)m_convert); // apply view offset openMVG::Mat4 offset_w = l2w_Camera(Mat3::Identity(), Vec3(x_offset,y_offset,z_offset)); glMultMatrixd((GLdouble*)offset_w.data()); // then apply current camera transformation const View * view = sfm_data.GetViews().at(vec_cameras[current_cam]).get(); const Pose3 pose = sfm_data.GetPoseOrDie(view); const openMVG::Mat4 l2w = l2w_Camera(pose.rotation(), pose.translation()); glPushMatrix(); glMultMatrixd((GLdouble*)l2w.data()); glPointSize(3); glDisable(GL_TEXTURE_2D); glDisable(GL_LIGHTING); //Draw Structure in GREEN (as seen from the current camera) size_t nbPoint = sfm_data.GetLandmarks().size(); size_t cpt = 0; glBegin(GL_POINTS); glColor3f(0.f,1.f,0.f); for (Landmarks::const_iterator iter = sfm_data.GetLandmarks().begin(); iter != sfm_data.GetLandmarks().end(); ++iter) { const Landmark & landmark = iter->second; glVertex3d(landmark.X(0), landmark.X(1), landmark.X(2)); } glEnd(); glDisable(GL_CULL_FACE); for (int i_cam=0; i_cam < vec_cameras.size(); ++i_cam) { const View * view = sfm_data.GetViews().at(vec_cameras[i_cam]).get(); const Pose3 pose = sfm_data.GetPoseOrDie(view); const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get(); if (isPinhole(cam->getType())) { const Pinhole_Intrinsic * camPinhole = dynamic_cast<const Pinhole_Intrinsic*>(cam); // Move frame to draw the camera i_cam by applying its inverse transformation // Warning: translation has to be "fixed" to remove the current camera rotation // Fix camera_i translation with current camera rotation inverse const Vec3 trans = pose.rotation().transpose() * pose.translation(); // compute inverse transformation matrix from local to world const openMVG::Mat4 l2w_i = l2w_Camera(pose.rotation().transpose(), -trans); // stack it and use it glPushMatrix(); glMultMatrixd((GLdouble*)l2w_i.data()); // 1. Draw optical center (RED) and image center (BLUE) glPointSize(3); glDisable(GL_TEXTURE_2D); glDisable(GL_LIGHTING); glBegin(GL_POINTS); glColor3f(1.f,0.f,0.f); glVertex3f(0, 0, 0); // optical center glColor3f(0.f,0.f,1.f); glVertex3f(0, 0, normalized_focal); // image center glEnd(); // compute image corners coordinated with normalized focal (f=normalized_focal) const int w = camPinhole->w(); const int h = camPinhole->h(); const double focal = camPinhole->focal(); // use principal point to adjust image center const Vec2 pp = camPinhole->principal_point(); Vec3 c1( -pp[0]/focal * normalized_focal, (-pp[1]+h)/focal * normalized_focal, normalized_focal); Vec3 c2((-pp[0]+w)/focal * normalized_focal, (-pp[1]+h)/focal * normalized_focal, normalized_focal); Vec3 c3((-pp[0]+w)/focal * normalized_focal, -pp[1]/focal * normalized_focal, normalized_focal); Vec3 c4( -pp[0]/focal * normalized_focal, -pp[1]/focal * normalized_focal, normalized_focal); // 2. Draw thumbnail if (i_cam == current_cam) { glEnable(GL_TEXTURE_2D); glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glBindTexture(GL_TEXTURE_2D, m_image_vector[i_cam].texture); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glEnable(GL_BLEND); glDisable(GL_DEPTH_TEST); if (i_cam == current_cam) { glColor4f(0.5f,0.5f,0.5f, 0.7f); } else { glColor4f(0.5f,0.5f,0.5f, 0.5f); } glBegin(GL_QUADS); glTexCoord2d(0.0,1.0); glVertex3d(c1[0], c1[1], c1[2]); glTexCoord2d(1.0,1.0); glVertex3d(c2[0], c2[1], c2[2]); glTexCoord2d(1.0,0.0); glVertex3d(c3[0], c3[1], c3[2]); glTexCoord2d(0.0,0.0); glVertex3d(c4[0], c4[1], c4[2]); glEnd(); glDisable(GL_TEXTURE_2D); glDisable(GL_BLEND); glEnable(GL_DEPTH_TEST); } // 3. Draw camera cone if (i_cam == current_cam) { glColor3f(1.f,1.f,0.f); } else { glColor3f(1.f,0.f,0.f); } glBegin(GL_LINES); glVertex3d(0.0,0.0,0.0); glVertex3d(c1[0], c1[1], c1[2]); glVertex3d(0.0,0.0,0.0); glVertex3d(c2[0], c2[1], c2[2]); glVertex3d(0.0,0.0,0.0); glVertex3d(c3[0], c3[1], c3[2]); glVertex3d(0.0,0.0,0.0); glVertex3d(c4[0], c4[1], c4[2]); glVertex3d(c1[0], c1[1], c1[2]); glVertex3d(c2[0], c2[1], c2[2]); glVertex3d(c2[0], c2[1], c2[2]); glVertex3d(c3[0], c3[1], c3[2]); glVertex3d(c3[0], c3[1], c3[2]); glVertex3d(c4[0], c4[1], c4[2]); glVertex3d(c4[0], c4[1], c4[2]); glVertex3d(c1[0], c1[1], c1[2]); glEnd(); glPopMatrix(); // go back to current camera frame } } glPopMatrix(); // go back to (document +offset) frame glPopMatrix(); // go back to identity } }
bool CreateImageFile( const SfM_Data & sfm_data, const std::string & sImagesFilename) { /* images.txt # Image list with two lines of data per image: # IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME # POINTS2D[] as (X, Y, POINT3D_ID) # Number of images: X, mean observations per image: Y */ // Header std::ofstream images_file( sImagesFilename ); if ( ! images_file ) { std::cerr << "Cannot write file" << sImagesFilename << std::endl; return false; } images_file << "# Image list with two lines of data per image:\n"; images_file << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; images_file << "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; images_file << "# Number of images: X, mean observations per image: Y\n"; std::map< IndexT, std::vector< std::tuple<double, double, IndexT> > > viewIdToPoints2D; const Landmarks & landmarks = sfm_data.GetLandmarks(); { for ( Landmarks::const_iterator iterLandmarks = landmarks.begin(); iterLandmarks != landmarks.end(); ++iterLandmarks) { const IndexT point3d_id = iterLandmarks->first; // Tally set of feature observations const Observations & obs = iterLandmarks->second.obs; for ( Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs ) { const IndexT currentViewId = itObs->first; const Observation & ob = itObs->second; viewIdToPoints2D[currentViewId].push_back(std::make_tuple(ob.x( 0 ), ob.x( 1 ), point3d_id)); } } } { C_Progress_display my_progress_bar( sfm_data.GetViews().size(), std::cout, "\n- CREATE IMAGE FILE -\n" ); for (Views::const_iterator iter = sfm_data.GetViews().begin(); iter != sfm_data.GetViews().end(); ++iter, ++my_progress_bar) { const View * view = iter->second.get(); if ( !sfm_data.IsPoseAndIntrinsicDefined( view ) ) { continue; } const Pose3 pose = sfm_data.GetPoseOrDie( view ); const Mat3 rotation = pose.rotation(); const Vec3 translation = pose.translation(); const double Tx = translation[0]; const double Ty = translation[1]; const double Tz = translation[2]; Eigen::Quaterniond q( rotation ); const double Qx = q.x(); const double Qy = q.y(); const double Qz = q.z(); const double Qw = q.w(); const IndexT image_id = view->id_view; // Colmap's camera_ids correspond to openMVG's intrinsic ids const IndexT camera_id = view->id_intrinsic; const std::string image_name = view->s_Img_path; // first line per image //IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME images_file << image_id << " " << Qw << " " << Qx << " " << Qy << " " << Qz << " " << Tx << " " << Ty << " " << Tz << " " << camera_id << " " << image_name << " " << "\n"; // second line per image //POINTS2D[] as (X, Y, POINT3D_ID) for (auto point2D: viewIdToPoints2D[image_id]) { images_file << std::get<0>(point2D) << " " << std::get<1>(point2D) << " " << std::get<2>(point2D) << " "; } images_file << "\n"; } } return true; }
int main(int argc, char **argv) { CmdLine cmd; std::string sSfM_Data_Filename; std::string sOutDir = ""; cmd.add( make_option('i', sSfM_Data_Filename, "sfmdata") ); cmd.add( make_option('o', sOutDir, "outdir") ); try { if (argc == 1) throw std::string("Invalid command line parameter."); cmd.process(argc, argv); } catch(const std::string& s) { std::cerr << "Usage: " << argv[0] << '\n' << "[-i|--sfmdata] filename, the SfM_Data file to convert\n" << "[-o|--outdir] path.\n" << std::endl; std::cerr << s << std::endl; return EXIT_FAILURE; } std::cout << " You called : " <<std::endl << argv[0] << std::endl << "--sfmdata " << sSfM_Data_Filename << std::endl << "--outdir " << sOutDir << std::endl; bool bOneHaveDisto = false; // Create output dir if (!stlplus::folder_exists(sOutDir)) stlplus::folder_create( sOutDir ); // Read the SfM scene SfM_Data sfm_data; if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(VIEWS|INTRINSICS|EXTRINSICS))) { std::cerr << std::endl << "The input SfM_Data file \""<< sSfM_Data_Filename << "\" cannot be read." << std::endl; return EXIT_FAILURE; } for(Views::const_iterator iter = sfm_data.GetViews().begin(); iter != sfm_data.GetViews().end(); ++iter) { const View * view = iter->second.get(); if (!sfm_data.IsPoseAndIntrinsicDefined(view)) continue; // Valid view, we can ask a pose & intrinsic data const Pose3 pose = sfm_data.GetPoseOrDie(view); Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic); const IntrinsicBase * cam = iterIntrinsic->second.get(); if (!cameras::isPinhole(cam->getType())) continue; const Pinhole_Intrinsic * pinhole_cam = static_cast<const Pinhole_Intrinsic *>(cam); // Extrinsic const Vec3 t = pose.translation(); const Mat3 R = pose.rotation(); // Intrinsic const double f = pinhole_cam->focal(); const Vec2 pp = pinhole_cam->principal_point(); // Image size in px const int w = pinhole_cam->w(); const int h = pinhole_cam->h(); // We can now create the .cam file for the View in the output dir std::ofstream outfile( stlplus::create_filespec( sOutDir, stlplus::basename_part(view->s_Img_path), "cam" ).c_str() ); // See https://github.com/nmoehrle/mvs-texturing/blob/master/Arguments.cpp // for full specs const int largerDim = w > h ? w : h; outfile << t(0) << " " << t(1) << " " << t(2) << " " << R(0,0) << " " << R(0,1) << " " << R(0,2) << " " << R(1,0) << " " << R(1,1) << " " << R(1,2) << " " << R(2,0) << " " << R(2,1) << " " << R(2,2) << "\n" << f / largerDim << " 0 0 1 " << pp(0) / w << " " << pp(1) / h; outfile.close(); if(cam->have_disto()) bOneHaveDisto = true; } const string sUndistMsg = bOneHaveDisto ? "undistorded" : ""; const string sQuitMsg = std::string("Your SfM_Data file was succesfully converted!\n") + "Now you can copy your " + sUndistMsg + " images in the \"" + sOutDir + "\" directory and run MVS Texturing"; std::cout << sQuitMsg << std::endl; return EXIT_SUCCESS; }