diff --git a/src/asp/Camera/CsmUtils.cc b/src/asp/Camera/CsmUtils.cc index 3aecc5322..2d34a9e61 100644 --- a/src/asp/Camera/CsmUtils.cc +++ b/src/asp/Camera/CsmUtils.cc @@ -293,7 +293,7 @@ void populateCsmLinescan(double first_line_time, double dt_line, double t0_ephem, double dt_ephem, double t0_quat, double dt_quat, double focal_length, - vw::Vector2 const & detector_origin, + vw::Vector2 const & optical_center, vw::Vector2i const & image_size, vw::cartography::Datum const & datum, std::string const & sensor_id, @@ -349,11 +349,18 @@ void populateCsmLinescan(double first_line_time, double dt_line, ls_model->m_detectorLineOrigin = 0.0; ls_model->m_detectorSampleOrigin = 0.0; ls_model->m_detectorLineSumming = 1.0; - // TODO(oalexan1): Must test with non-zero detector origin[1]. - ls_model->m_startingDetectorLine = detector_origin[1]; ls_model->m_detectorSampleSumming = 1.0; - ls_model->m_startingDetectorSample = (detector_origin[0] - 0.5); - + // There is an inconsistency below, but this is what works + ls_model->m_startingDetectorLine = -optical_center[1]; + ls_model->m_startingDetectorSample = -optical_center[0] - 0.5; + + // Fix for the quirky C++ negative 0. It shows up in the produced + // .json files and looks odd. + if (std::abs(ls_model->m_startingDetectorLine) == 0.0) + ls_model->m_startingDetectorLine = 0.0; + if (std::abs(ls_model->m_startingDetectorSample) == 0.0) + ls_model->m_startingDetectorSample = 0.0; + // Set the time ls_model->m_intTimeLines.push_back(1.0); // to offset CSM's quirky 0.5 additions in places ls_model->m_intTimeStartTimes.push_back(first_line_time); diff --git a/src/asp/Camera/CsmUtils.h b/src/asp/Camera/CsmUtils.h index 58656b89d..f90de71c4 100644 --- a/src/asp/Camera/CsmUtils.h +++ b/src/asp/Camera/CsmUtils.h @@ -85,7 +85,7 @@ void populateCsmLinescan(double first_line_time, double dt_line, double t0_ephem, double dt_ephem, double t0_quat, double dt_quat, double focal_length, - vw::Vector2 const & detector_origin, + vw::Vector2 const & optical_center, vw::Vector2i const & image_size, vw::cartography::Datum const & datum, std::string const & sensor_id, diff --git a/src/asp/Camera/LinescanASTERModel.cc b/src/asp/Camera/LinescanASTERModel.cc index 6f7914b3a..788725df7 100644 --- a/src/asp/Camera/LinescanASTERModel.cc +++ b/src/asp/Camera/LinescanASTERModel.cc @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include @@ -52,29 +54,29 @@ ASTERCameraModel::ASTERCameraModel(std::vector> const& double tol = 1e-10; double d_row = double(max_row - min_row)/(num_rows - 1.0); if (std::abs(d_row - round(d_row)) > tol) { - vw::vw_throw( vw::ArgumentErr() - << "The spacing between lattice points must be integer.\n" ); + vw::vw_throw(vw::ArgumentErr() + << "The spacing between lattice points must be integer.\n"); } d_row = round(d_row); // The spacing between columns must be integer double d_col = double(max_col - min_col)/(num_cols - 1.0); if (std::abs(d_col - round(d_col)) > tol) { - vw::vw_throw( vw::ArgumentErr() - << "The spacing between lattice points must be integer.\n" ); + vw::vw_throw(vw::ArgumentErr() + << "The spacing between lattice points must be integer.\n"); } d_col = round(d_col); if ((int)m_sat_pos.size() != num_rows) - vw::vw_throw( vw::ArgumentErr() - << "The number of rows of lattice points does not " - << "agree with the number of satellite positions.\n" ); + vw::vw_throw(vw::ArgumentErr() + << "The number of rows of lattice points does not " + << "agree with the number of satellite positions.\n"); if (num_rows != (int)m_world_sight_mat.size() || num_cols != (int)m_world_sight_mat[0].size()) - vw::vw_throw( vw::ArgumentErr() - << "The number of rows or columns of lattice points does not " - << "agree with the number of sight vectors.\n" ); + vw::vw_throw(vw::ArgumentErr() + << "The number of rows or columns of lattice points does not " + << "agree with the number of sight vectors.\n" ); m_interp_sat_pos = vw::camera::LinearPiecewisePositionInterpolation(m_sat_pos, min_row, d_row); @@ -82,14 +84,73 @@ ASTERCameraModel::ASTERCameraModel(std::vector> const& m_interp_sight_mat = vw::camera::SlerpGridPointingInterpolation(m_world_sight_mat, min_row, d_row, min_col, d_col); - double focal_length; - vw::Vector2 optical_center; - std::vector> rotation_vec; - // Find the rotation matrices, focal length, and optical center, // that best fit a 2D matrix of sight vectors. Uses for ASTER. - //fitBestRotationsIntrinsics(m_world_sight_mat, m_image_size, d_col, - // focal_length, optical_center, rotation_vec); + double focal_length = 0.0; + vw::Vector2 optical_center; + std::vector> cam2world_vec; + fitBestRotationsIntrinsics(m_world_sight_mat, m_image_size, d_col, + focal_length, optical_center, cam2world_vec); + + // Assume it takes one unit of time to scan one image line + double first_line_time = 0; // time of the first scanned line + double last_line_time = m_image_size[1] - 1; + double dt_line = (last_line_time - first_line_time) / (m_image_size[1] - 1.0); + + // The image line for the first pose determines its time. The spacing + // between poses determines dt_ephem. Note that min_row is negative, + // because the first pose is measured before data starts being recorded. + double t0_ephem = min_row * dt_line; + double dt_ephem = d_row * dt_line; + double t0_quat = t0_ephem; + double dt_quat = dt_ephem; + + // We have no velocities in this context, so set them to 0 + std::vector velocities(m_sat_pos.size(), vw::Vector3(0, 0, 0)); + + vw::cartography::Datum datum("WGS84"); // ASTER is for Earth + std::string sensor_id = "ASTER"; + // Create the CSM model always, but use it only if the user asked for it. + // This is convenient in cam_test when comparing CSM vs non-CSM. + asp::populateCsmLinescan(first_line_time, dt_line, + t0_ephem, dt_ephem, t0_quat, dt_quat, + focal_length, optical_center, + m_image_size, datum, sensor_id, + m_sat_pos, velocities, cam2world_vec, + m_csm_model); // output + + if (asp::stereo_settings().aster_use_csm) + vw_out() << "Using the CSM model with ASTER cameras.\n"; + + if ((asp::stereo_settings().enable_correct_velocity_aberration || + asp::stereo_settings().enable_correct_atmospheric_refraction) && + asp::stereo_settings().aster_use_csm) + vw::vw_throw(vw::ArgumentErr() << "Cannot correct velocity aberration or " + << "atmospheric refraction with the CSM model.\n"); + +#if 0 + // Temporary Sanity check + for (int row = 0; row < num_rows; row++) { + for (int col = 0; col < num_cols; col++) { + + // Vector in sensor coordinates + vw::Vector3 in(col * d_col - optical_center[0], -optical_center[1], focal_length); + in = in/norm_2(in); + // Rotate to world coordinates + in = cam2world_vec[row] * in; + + vw::Vector3 out = world_sight_mat[row][col]; + out = out/norm_2(out); + //std::cout << "norm of diff is " << norm_2(in-out) << std::endl; + + // Create the CSM pixel + vw::Vector2 pix(col * d_col, row * d_row + min_row); + std::cout << "csm pixel is " << pix << std::endl; + vw::Vector3 dir = m_csm_model.pixel_to_vector(pix); + std::cout << "in, dir, and norm of diff are " << in << ' ' << dir << ' ' << norm_2(in-dir) << std::endl; + } + } +#endif } // Project the point onto the camera. Sometimes, but not always, seeding with the RPC @@ -97,8 +158,11 @@ ASTERCameraModel::ASTERCameraModel(std::vector> const& vw::Vector2 ASTERCameraModel::point_to_pixel(Vector3 const& point, Vector2 const& start_in) const { + if (stereo_settings().aster_use_csm) + return m_csm_model.point_to_pixel(point); + // - This method will be slower but works for more complicated geometries - vw::camera::CameraGenericLMA model( this, point ); + vw::camera::CameraGenericLMA model(this, point); int status; vw::Vector2 start = m_image_size / 2.0; // Use the center as the initial guess @@ -125,49 +189,17 @@ vw::Vector2 ASTERCameraModel::point_to_pixel(Vector3 const& point, int T = 100; // This way we'll sample about every 4-th pixel since dcol = 400 int col = m_lattice_mat.front().size()/2; for (int r = 0; r < T; r++) { - double wr = double(r)/(T-1.0); - Vector2 pt - = wr*m_lattice_mat[row+1][col] - + (1-wr)*m_lattice_mat[row][col]; - double err = norm_2(model(pt)); - if (err < min_err) { - min_err = err; - start = pt; - } + double wr = double(r)/(T-1.0); + Vector2 pt = wr*m_lattice_mat[row+1][col] + (1-wr)*m_lattice_mat[row][col]; + double err = norm_2(model(pt)); + if (err < min_err) { + min_err = err; + start = pt; + } } } } - -#if 0 - // This exhaustive search for an initial guess is an overkill - if (!has_guess) { - double min_err = norm_2(model(start)); - // No good initial guess. The method will fail to converge. - // Iterate through the lattice to find a good initial guess. - for (int row = 0; row < int(m_lattice_mat.size())-1; row++) { - for (int col = 0; col < int(m_lattice_mat.front().size())-1; col++) { - int T = 100; - for (int r = 0; r < T; r++) { - for (int c = 0; c < T; c++) { - double wr = double(r)/(T-1.0); - double wc = double(c)/(T-1.0); - Vector2 pt - = wr*wc*m_lattice_mat[row+1][col+1] - + (1-wr)*wc*m_lattice_mat[row][col+1] - + wr*(1-wc)*m_lattice_mat[row+1][col] - + (1-wr)*(1-wc)*m_lattice_mat[row][col]; - double err = norm_2(model(pt)); - if (err < min_err) { - min_err = err; - start = pt; - } - } - } - } - } - } -#endif - + // Solver constants const double ABS_TOL = 1e-16; const double REL_TOL = 1e-16; @@ -178,7 +210,8 @@ vw::Vector2 ASTERCameraModel::point_to_pixel(Vector3 const& point, // Solution with user-provided initial guess Vector3 objective(0, 0, 0); - vw::Vector2 solution1 = vw::math::levenberg_marquardtFixed + vw::Vector2 solution1 + = vw::math::levenberg_marquardtFixed (model, start, objective, status, ABS_TOL, REL_TOL, MAX_ITERATIONS); // Solution with the RPC initial guess @@ -190,18 +223,19 @@ vw::Vector2 ASTERCameraModel::point_to_pixel(Vector3 const& point, double error1 = norm_2(model(solution1)); double error2 = norm_2(model(solution2)); double error = std::min(error1, error2); - + vw::Vector2 solution; if (error1 < error2) { solution = solution1; - } else if (error1 > error2) { + } else { solution = solution2; } // Check the error - If it is too high then the solver probably got // stuck at the edge of the image. - VW_ASSERT( (status > 0) && (error < MAX_ERROR), - vw::camera::PointToPixelErr() << "Unable to project point into LinescanASTER model" ); + VW_ASSERT((status > 0) && (error < MAX_ERROR), + vw::camera::PointToPixelErr() + << "Unable to project point into LinescanASTER model."); return solution; } @@ -211,12 +245,19 @@ vw::Vector2 ASTERCameraModel::point_to_pixel(Vector3 const& point, double starty } vw::Vector3 ASTERCameraModel::camera_center(vw::Vector2 const& pix) const { + if (stereo_settings().aster_use_csm) + return m_csm_model.camera_center(pix); + return m_interp_sat_pos(pix.y()); } -vw::Vector3 ASTERCameraModel::pixel_to_vector(vw::Vector2 const& pixel) const{ +vw::Vector3 ASTERCameraModel::pixel_to_vector(vw::Vector2 const& pix) const { + + if (stereo_settings().aster_use_csm) + return m_csm_model.pixel_to_vector(pix); + try { - return m_interp_sight_mat(pixel); + return m_interp_sight_mat(pix); } catch(const vw::Exception &e) { // Repackage any of our exceptions thrown below this point as a // pixel to ray exception that other code will be able to handle. diff --git a/src/asp/Camera/LinescanASTERModel.h b/src/asp/Camera/LinescanASTERModel.h index b5fba9be5..83ceaf906 100644 --- a/src/asp/Camera/LinescanASTERModel.h +++ b/src/asp/Camera/LinescanASTERModel.h @@ -18,19 +18,20 @@ /// \file LinescanASTERModel.h /// -/// Linescan model for ASTER. Don't bother with time, just interpolate -/// between pointing vectors. -/// +/// Linescan model for ASTER. Old approach: Don't bother with time, just interpolate +/// between pointing vectors. New approach: Find best-fitting camera poses and +/// create a CSM model. /// #ifndef __STEREO_CAMERA_LINESCAN_ASTER_MODEL_H__ #define __STEREO_CAMERA_LINESCAN_ASTER_MODEL_H__ +#include + #include #include #include #include - namespace asp { // References: @@ -56,7 +57,7 @@ namespace asp { //------------------------------------------------------------------ // Constructors / Destructors //------------------------------------------------------------------ - ASTERCameraModel(std::vector< std::vector > const& lattice_mat, + ASTERCameraModel(std::vector< std::vector> const& lattice_mat, std::vector< std::vector > const& sight_mat, std::vector< std::vector > const& world_sight_mat, std::vector const& sat_pos, @@ -83,14 +84,16 @@ namespace asp { protected: - std::vector< std::vector > m_lattice_mat; - std::vector< std::vector > m_sight_mat; - std::vector< std::vector > m_world_sight_mat; - std::vector m_sat_pos; - vw::Vector2 m_image_size; + std::vector> m_lattice_mat; + std::vector> m_sight_mat; + std::vector> m_world_sight_mat; + std::vector m_sat_pos; + vw::Vector2 m_image_size; vw::camera::LinearPiecewisePositionInterpolation m_interp_sat_pos; vw::camera::SlerpGridPointingInterpolation m_interp_sight_mat; boost::shared_ptr m_rpc_model; // rpc approx, for initial guess + + asp::CsmModel m_csm_model; }; // End class ASTERCameraModel @@ -100,6 +103,7 @@ namespace asp { boost::shared_ptr load_ASTER_camera_model_from_xml(std::string const& path, boost::shared_ptr rpc_model); + } // namespace asp diff --git a/src/asp/Camera/LinescanDGModel.cc b/src/asp/Camera/LinescanDGModel.cc index 4f4e57ee0..2c2acb444 100644 --- a/src/asp/Camera/LinescanDGModel.cc +++ b/src/asp/Camera/LinescanDGModel.cc @@ -104,9 +104,11 @@ vw::CamPtr load_dg_camera_model_from_xml(std::string const& path) { // It is assumed that EPH and ATT are sampled at the same rate and time. VW_ASSERT(eph.satellite_position_vec.size() == att.satellite_quat_vec.size(), - vw::MathErr() << "Ephemeris and attitude don't have the same number of samples."); + vw::MathErr() + << "Ephemeris and attitude don't have the same number of samples."); VW_ASSERT(eph.start_time == att.start_time && eph.time_interval == att.time_interval, - vw::MathErr() << "Ephemeris and attitude don't seem to use the same t0 or dt."); + vw::MathErr() + << "Ephemeris and attitude don't seem to use the same t0 or dt."); // Load up the time interpolation class. If the TLCList only has // one entry, then we have to manually drop in the slope and offset. @@ -188,16 +190,16 @@ vw::CamPtr load_dg_camera_model_from_xml(std::string const& path) { if (len_q > 0 && asp::stereo_settings().propagate_errors) q = q / len_q; // Normalization is not needed without covariance logic q = q + dq; - vw::Quat qt(q[3], q[0], q[1], q[2]); // Note the swapping, the order is now w, x, y, z. + vw::Quat qt(q[3], q[0], q[1], q[2]); // Note the swap. The order is now w, x, y, z. camera_position_vec[i] = p + qt.rotate(geo.perspective_center); camera_quat_vec[i] = qt * sensor_to_body; } vw::CamPtr cam_ptr - (new DGCameraModel(vw::camera::PiecewiseAPositionInterpolation(camera_position_vec, - eph.velocity_vec, et0, edt), - vw::camera::LinearPiecewisePositionInterpolation(eph.velocity_vec, - et0, edt), + (new DGCameraModel(vw::camera::PiecewiseAPositionInterpolation + (camera_position_vec, eph.velocity_vec, et0, edt), + vw::camera::LinearPiecewisePositionInterpolation + (eph.velocity_vec, et0, edt), vw::camera::SLERPPoseInterpolation(camera_quat_vec, at0, adt), tlc_time_interpolation, img.image_size, final_detector_origin, geo.principal_distance, mean_ground_elevation, @@ -238,8 +240,9 @@ DGCameraModel::DGCameraModel double const mean_ground_elevation, bool correct_velocity, bool correct_atmosphere): - DGCameraModelBase(position, velocity, pose, time, image_size, detector_origin, focal_length, - mean_ground_elevation, correct_velocity, correct_atmosphere) { + DGCameraModelBase(position, velocity, pose, time, image_size, detector_origin, + focal_length, mean_ground_elevation, + correct_velocity, correct_atmosphere) { // It is convenient to have the CSM model exist even if it is not used. // The cam_test.cc and jitter_solve.cc tools uses this assumption. @@ -261,11 +264,11 @@ void DGCameraModel::populateCsmModel() { // 2,002,252.25. m_csm_model.reset(new CsmModel); - // this sensor is used for Earth only - vw::cartography::Datum datum = vw::cartography::Datum("WGS84"); + // This sensor is used for Earth only + vw::cartography::Datum datum("WGS84"); m_csm_model->m_desired_precision = asp::DEFAULT_CSM_DESIRED_PRECISION; - m_csm_model->m_semi_major_axis = datum.semi_major_axis(); // WGS84 - m_csm_model->m_semi_minor_axis = datum.semi_minor_axis(); // WGS84 + m_csm_model->m_semi_major_axis = datum.semi_major_axis(); + m_csm_model->m_semi_minor_axis = datum.semi_minor_axis(); // Create a linescan model as a smart pointer, and do smart pointer // casting Follow the CSM API. The type of m_gm_model is @@ -410,9 +413,8 @@ void DGCameraModel::populateCsmModel() { } // Re-implement base class functions - +// TODO(oalexan1): This must be wiped when no longer inheriting from VW linescan double DGCameraModel::get_time_at_line(double line) const { - if (stereo_settings().dg_use_csm) { csm::ImageCoord csm_pix; vw::Vector2 pix(0, line); @@ -423,6 +425,7 @@ double DGCameraModel::get_time_at_line(double line) const { return m_time_func(line); } +// TODO(oalexan1): This must be wiped when no longer inheriting from VW linescan vw::Vector3 DGCameraModel::get_camera_center_at_time(double time) const { if (stereo_settings().dg_use_csm) { csm::EcefCoord ecef = m_ls_model->getSensorPosition(time); @@ -432,6 +435,7 @@ vw::Vector3 DGCameraModel::get_camera_center_at_time(double time) const { return m_position_func(time); } +// TODO(oalexan1): This must be wiped when no longer inheriting from VW linescan vw::Vector3 DGCameraModel::get_camera_velocity_at_time(double time) const { if (stereo_settings().dg_use_csm) { csm::EcefVector ecef = m_ls_model->getSensorVelocity(time); @@ -444,6 +448,7 @@ vw::Vector3 DGCameraModel::get_camera_velocity_at_time(double time) const { // Function to interpolate quaternions with the CSM model. This is used // for CSM model validation but not in production. // TODO(oalexan1): Move this to a new CsmModelUtils.cc file and call it from here. +// TODO(oalexan1): This must be wiped when removing the ASP linescan implementation void DGCameraModel::getQuaternions(const double& time, double q[4]) const { if (!stereo_settings().dg_use_csm) @@ -469,10 +474,10 @@ void DGCameraModel::interpSatellitePosCov(vw::Vector2 const& pix, if (!stereo_settings().dg_use_csm) vw::vw_throw(vw::ArgumentErr() - << "interpSatellitePosCov: It was expected that the CSM model was used.\n"); + << "interpSatellitePosCov: It was expected that the CSM model " + << "was used.\n"); double time = get_time_at_line(pix.y()); - int numCov = m_satellite_pos_cov.size() / SAT_POS_COV_SIZE; int nOrder = 8; @@ -522,13 +527,9 @@ vw::Quat DGCameraModel::get_camera_pose_at_time(double time) const { // Gives a pointing vector in the world coordinates. vw::Vector3 DGCameraModel::pixel_to_vector(vw::Vector2 const& pix) const { - - if (stereo_settings().dg_use_csm) { - csm::ImageCoord csm_pix; - asp::toCsmPixel(pix, csm_pix); - csm::EcefLocus locus = m_ls_model->imageToRemoteImagingLocus(csm_pix); - return vw::Vector3(locus.direction.x, locus.direction.y, locus.direction.z); - } + + if (stereo_settings().dg_use_csm) + return m_csm_model->pixel_to_vector(pix); return vw::camera::LinescanModel::pixel_to_vector(pix); } @@ -555,80 +556,11 @@ double DGCameraModel::errorFunc(double y, vw::Vector3 const& point) const { // Point to pixel with no initial guess vw::Vector2 DGCameraModel::point_to_pixel(vw::Vector3 const& point) const { - if (stereo_settings().dg_use_csm) { + if (stereo_settings().dg_use_csm) + return m_csm_model->point_to_pixel(point); - csm::EcefCoord ecef(point[0], point[1], point[2]); - - // Do not show warnings, it becomes too verbose - double achievedPrecision = -1.0; - csm::WarningList warnings; - csm::WarningList * warnings_ptr = NULL; - bool show_warnings = false; - // Do not use here a desired precision than than 1e-8, as - // then CSM can return junk. - csm::ImageCoord csm_pix - = m_ls_model->groundToImage(ecef, - m_csm_model->m_desired_precision, - &achievedPrecision, warnings_ptr); - - vw::Vector2 asp_pix; - asp::fromCsmPixel(asp_pix, csm_pix); - -#if 0 - // This logic is from UsgsAstroLsSensorModel, with fixes to make - // it more robust for DG cameras. Not used yet. - - // TODO(oalexan1): Use this logic - // with non-CSM cameras. Should be much faster than the current - // approach of solving a minimization problem. Do not set - // desired_precision to less than 1e-8 as then the algorithm will - // produce junk due to numerical precision issues with the large - // DG focal length. - - // For non-CSM logic, need to start iterating from sensor - // midpoint. asp_pix[1] = m_image_size.y()/2; - - double L0 = 0.0; // Line increment - double lineErr0 = errorFunc(L0 + asp_pix[1], point); - double L1 = 0.1; - double lineErr1 = errorFunc(L1 + asp_pix[1], point); - - for (int count = 0; count < 15; count++) { - - if (lineErr1 == lineErr0) - break; // avoid division by 0 - - // Secant method update - // https://en.wikipedia.org/wiki/Secant_method - double increment = lineErr1 * (L1 - L0) / (lineErr1 - lineErr0); - double L2 = L1 - increment; - double lineErr2 = errorFunc(L2 + asp_pix[1], point); - - // Update for the next step - L0 = L1; lineErr0 = lineErr1; - L1 = L2; lineErr1 = lineErr2; - - // If the solution changes by less than this, we achieved the desired line precision - if (increment < m_csm_model->m_desired_precision) - break; - } - - asp_pix[1] += L1; - - // Solve for sample location now that we know the correct line - double t = get_time_at_line(asp_pix[1]); - vw::Quat q = get_camera_pose_at_time(t); - vw::Vector3 pt = inverse(q).rotate(point - get_camera_center_at_time(t)); - pt *= m_focal_length / pt.z(); - asp_pix = vw::Vector2(pt.x() - m_detector_origin[0], asp_pix[1]); -#endif - - return asp_pix; - } - // Non-CSM version return vw::camera::LinescanModel::point_to_pixel(point); - } // TODO(oalexan1): Wipe this and use the logic above, after much testing. @@ -648,11 +580,12 @@ vw::Vector2 DGCameraModel::point_to_pixel(vw::Vector3 const& point, double start const double ABS_TOL = 1e-16; const double REL_TOL = 1e-16; const int MAX_ITERATIONS = 1e+5; - vw::Vector2 solution = vw::math::levenberg_marquardtFixed - (model, start, objective, status, - ABS_TOL, REL_TOL, MAX_ITERATIONS); + vw::Vector2 solution + = vw::math::levenberg_marquardtFixed + (model, start, objective, status, ABS_TOL, REL_TOL, MAX_ITERATIONS); VW_ASSERT(status > 0, - vw::camera::PointToPixelErr() << "Unable to project point into LinescanDG model."); + vw::camera::PointToPixelErr() + << "Unable to project point into LinescanDG model."); return solution; } @@ -670,15 +603,8 @@ vw::Quaternion DGCameraModel::camera_pose(vw::Vector2 const& pix) const // Gives the camera position in world coordinates. vw::Vector3 DGCameraModel::camera_center(vw::Vector2 const& pix) const { - if (stereo_settings().dg_use_csm) { - csm::ImageCoord csm_pix; - asp::toCsmPixel(pix, csm_pix); - - double time = m_ls_model->getImageTime(csm_pix); - csm::EcefCoord ecef = m_ls_model->getSensorPosition(time); - - return vw::Vector3(ecef.x, ecef.y, ecef.z); - } + if (stereo_settings().dg_use_csm) + return m_csm_model->camera_center(pix); return vw::camera::LinescanModel::camera_center(pix); } diff --git a/src/asp/Camera/LinescanFit.cc b/src/asp/Camera/LinescanFit.cc index 693ff6c87..4a7053886 100644 --- a/src/asp/Camera/LinescanFit.cc +++ b/src/asp/Camera/LinescanFit.cc @@ -157,7 +157,6 @@ void fitBestRotationsIntrinsics( // for that, first convert to quaternion vw::Quat q(rotation_vec[row]); vw::Vector3 axis_angle = q.axis_angle(); - //std::cout << "axis angle is " << axis_angle << std::endl; axis_angle_vec[row] = axis_angle; } @@ -181,23 +180,22 @@ void fitBestRotationsIntrinsics( options.linear_solver_type = ceres::ITERATIVE_SCHUR; options.num_threads = 1; options.max_num_iterations = 50; // 50 iterations is enough - options.minimizer_progress_to_stdout = true; + options.minimizer_progress_to_stdout = false; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); + + // Copy back from axis_angle_vec to rotation_vec + for (int row = 0; row < num_rows; row++) + rotation_vec[row] + = vw::math::axis_angle_to_quaternion(axis_angle_vec[row]).rotation_matrix(); - std::cout << summary.FullReport() << "\n"; - std::cout << "Starting average reprojection error: " +#if 0 // for debugging + vw::vw_out() << summary.FullReport() << "\n"; + vw::vw_out() << "Starting average reprojection error: " << summary.initial_cost << "\n"; - std::cout << "Final average reprojection error: " + vw::vw_out() << "Final average reprojection error: " << summary.final_cost << "\n"; - // Print final optical center - std::cout << "final optical center is " << optical_center << std::endl; - // print final focal length - std::cout << "final focal length is " << focal_length << std::endl; - - // TODO(oalexan1): Turn this off -#if 1 // Evaluate the final residuals. For debugging. double total_cost = 0.0; ceres::Problem::EvaluateOptions eval_options; @@ -208,7 +206,7 @@ void fitBestRotationsIntrinsics( // Save the residuals std::string resFile = "residual_norms.txt"; - std::cout << "Writing residual norms to: " << resFile << std::endl; + vw::vw_out() << "Writing residual norms to: " << resFile << std::endl; std::ofstream ofs(resFile.c_str()); for (int i = 0; i < residuals.size()/3; i++) { int j = 3*i; @@ -217,30 +215,6 @@ void fitBestRotationsIntrinsics( ofs.close(); #endif - // Copy back from axis_angle_vec to rotation_vec - for (int row = 0; row < num_rows; row++) - rotation_vec[row] - = vw::math::axis_angle_to_quaternion(axis_angle_vec[row]).rotation_matrix(); - - // TODO(oalexan1): Check that this has same info as the residuals - // saved to disk, then wipe this - - // Try to see if the sight vectors agree with the rotation matrices - for (int row = 0; row < num_rows; row++) { - for (int col = 0; col < num_cols; col++) { - - // Vector in sensor coordinates - vw::Vector3 in(col * d_col - optical_center[0], -optical_center[1], focal_length); - in = in/norm_2(in); - // Rotate to world coordinates - in = rotation_vec[row] * in; - - vw::Vector3 out = world_sight_mat[row][col]; - out = out/norm_2(out); - std::cout << "norm of diff is " << norm_2(in-out) << std::endl; - } - } - return; } diff --git a/src/asp/Camera/SatSim.cc b/src/asp/Camera/SatSim.cc index d53495416..a2dd33352 100644 --- a/src/asp/Camera/SatSim.cc +++ b/src/asp/Camera/SatSim.cc @@ -900,10 +900,9 @@ void genPinholeCameras(SatSimOptions const & opt, cam2world[i]); cams[i] = vw::CamPtr(csmPtr); // will own this pointer } else { - pinPtr = new vw::camera::PinholeModel(positions[i], - cam2world[i], - opt.focal_length, opt.focal_length, - opt.optical_center[0], opt.optical_center[1]); + pinPtr = new vw::camera::PinholeModel(positions[i], cam2world[i], + opt.focal_length, opt.focal_length, + opt.optical_center[0], opt.optical_center[1]); cams[i] = vw::CamPtr(pinPtr); // will own this pointer } @@ -919,8 +918,7 @@ void genPinholeCameras(SatSimOptions const & opt, positions[i], ref_cam2world[i]); else - pinRefCam = vw::camera::PinholeModel(positions[i], - ref_cam2world[i], + pinRefCam = vw::camera::PinholeModel(positions[i], ref_cam2world[i], opt.focal_length, opt.focal_length, opt.optical_center[0], opt.optical_center[1]); } diff --git a/src/asp/Camera/SyntheticLinescan.cc b/src/asp/Camera/SyntheticLinescan.cc index 0a9dc0d73..b6134de2e 100644 --- a/src/asp/Camera/SyntheticLinescan.cc +++ b/src/asp/Camera/SyntheticLinescan.cc @@ -256,22 +256,20 @@ void genLinescanCameras(double orbit_len, std::string sensor_id = "SyntheticLinescan"; // We assume no optical offset in y for this synthetic camera - vw::Vector2 detector_origin; - detector_origin[0] = -opt.optical_center[0]; - detector_origin[1] = 0.0; + vw::Vector2 local_optical_center(opt.optical_center[0], 0.0); // If creating square pixels, must use the camera without jitter to estimate // the image height. Otherwise the image height produced from the camera with // jitter will be inconsistent with the one without jitter. This is a bugfix. if (!opt.square_pixels) populateCsmLinescan(first_line_time, dt_line, t0_ephem, dt_ephem, t0_quat, dt_quat, - opt.focal_length, detector_origin, + opt.focal_length, local_optical_center, opt.image_size, dem_georef.datum(), sensor_id, positions, velocities, cam2world, *ls_cam); // output else populateCsmLinescan(first_line_time, dt_line, t0_ephem, dt_ephem, t0_quat, dt_quat, - opt.focal_length, detector_origin, + opt.focal_length, local_optical_center, opt.image_size, dem_georef.datum(), sensor_id, positions, velocities, cam2world_no_jitter, *ls_cam); // output @@ -293,7 +291,7 @@ void genLinescanCameras(double orbit_len, // Recreate the camera with this aspect ratio. This time potentially use the // camera with jitter. populateCsmLinescan(first_line_time, dt_line, t0_ephem, dt_ephem, t0_quat, dt_quat, - opt.focal_length, detector_origin, + opt.focal_length, local_optical_center, opt.image_size, dem_georef.datum(), sensor_id, positions, velocities, cam2world, *ls_cam); // output @@ -306,7 +304,7 @@ void genLinescanCameras(double orbit_len, if (opt.save_ref_cams) { asp::CsmModel ref_cam; populateCsmLinescan(first_line_time, dt_line, t0_ephem, dt_ephem, t0_quat, dt_quat, - opt.focal_length, detector_origin, + opt.focal_length, local_optical_center, opt.image_size, dem_georef.datum(), sensor_id, positions, velocities, ref_cam2world, ref_cam); // output diff --git a/src/asp/Core/StereoSettings.cc b/src/asp/Core/StereoSettings.cc index 17b7ff872..134266e7d 100644 --- a/src/asp/Core/StereoSettings.cc +++ b/src/asp/Core/StereoSettings.cc @@ -203,7 +203,9 @@ namespace asp { "Turn on atmospheric refraction correction for Optical Bar and non-ISIS linescan cameras. This option impairs the convergence of bundle adjustment.") ("dg-use-csm", po::bool_switch(&global.dg_use_csm)->default_value(false)->implicit_value(true), "Use the CSM model with DigitalGlobe linescan cameras (-t dg). No corrections are done for velocity aberration or atmospheric refraction.") - + ("aster-use-csm", po::bool_switch(&global.aster_use_csm)->default_value(false)->implicit_value(true), + "Use the CSM model with ASTER cameras (-t aster).") + // For bathymetry correction ("left-bathy-mask", po::value(&global.left_bathy_mask), "Mask to use for the left image when doing bathymetry.") diff --git a/src/asp/Core/StereoSettings.h b/src/asp/Core/StereoSettings.h index ce755908a..ce26f8e5d 100644 --- a/src/asp/Core/StereoSettings.h +++ b/src/asp/Core/StereoSettings.h @@ -149,7 +149,9 @@ namespace asp { int disparity_range_expansion_percent; ///< Expand the estimated disparity range by this percentage before computing the stereo correlation with local alignment - bool dg_use_csm; // Use the CSM camera model with Digital Globe images. + // These two flags will be the default in the future and will go away + bool dg_use_csm; // Use the CSM camera model with Digital Globe images + bool aster_use_csm; // Use the CSM camera model with ASTER images // Correlation options diff --git a/src/asp/Tools/bundle_adjust.cc b/src/asp/Tools/bundle_adjust.cc index 5a7c77138..a59a5f572 100644 --- a/src/asp/Tools/bundle_adjust.cc +++ b/src/asp/Tools/bundle_adjust.cc @@ -81,7 +81,8 @@ void saveResults(Options const& opt, asp::BAParams const& param_storage) { // For CSM camera models export, in addition, the JSON state // with the adjustment applied to it. if (opt.stereo_session == "csm" || opt.stereo_session == "pleiades" || - (opt.stereo_session == "dg" && asp::stereo_settings().dg_use_csm)) + (opt.stereo_session == "dg" && asp::stereo_settings().dg_use_csm) || + (opt.stereo_session == "aster" && asp::stereo_settings().aster_use_csm)) write_csm_output_file_no_intr(opt, icam, adjust_file, param_storage); } break; @@ -2093,6 +2094,8 @@ void handle_arguments(int argc, char *argv[], Options& opt) { "Turn on atmospheric refraction correction for Optical Bar and non-ISIS linescan cameras. This option impairs the convergence of bundle adjustment.") ("dg-use-csm", po::bool_switch(&opt.dg_use_csm)->default_value(false)->implicit_value(true), "Use the CSM model with DigitalGlobe linescan cameras (-t dg). No corrections are done for velocity aberration or atmospheric refraction.") + ("aster-use-csm", po::bool_switch(&opt.aster_use_csm)->default_value(false)->implicit_value(true), + "Use the CSM model with ASTER cameras (-t aster).") ("mapprojected-data", po::value(&opt.mapprojected_data)->default_value(""), "Given map-projected versions of the input images and the DEM they " "were mapprojected onto, create interest point matches among the " diff --git a/src/asp/Tools/bundle_adjust.h b/src/asp/Tools/bundle_adjust.h index 223cb0528..783bc46d2 100644 --- a/src/asp/Tools/bundle_adjust.h +++ b/src/asp/Tools/bundle_adjust.h @@ -80,7 +80,8 @@ struct Options: public asp::BaBaseOptions { bool skip_rough_homography, enable_rough_homography, disable_tri_filtering, enable_tri_filtering, no_datum, individually_normalize, use_llh_error, force_reuse_match_files, save_cnet_as_csv, - enable_correct_velocity_aberration, enable_correct_atmospheric_refraction, dg_use_csm; + enable_correct_velocity_aberration, enable_correct_atmospheric_refraction, + dg_use_csm, aster_use_csm; vw::Vector2 elevation_limit; // Expected range of elevation to limit results to. vw::BBox2 lon_lat_limit; // Limit the triangulated interest points to this lonlat range vw::BBox2 proj_win; // Limit input triangulated points to this projwin @@ -117,6 +118,7 @@ struct Options: public asp::BaBaseOptions { asp::stereo_settings().enable_correct_velocity_aberration = enable_correct_velocity_aberration; asp::stereo_settings().dg_use_csm = dg_use_csm; + asp::stereo_settings().aster_use_csm = aster_use_csm; asp::stereo_settings().ip_per_tile = ip_per_tile; asp::stereo_settings().ip_per_image = ip_per_image; asp::stereo_settings().matches_per_tile = matches_per_tile; diff --git a/src/asp/Tools/cam_test.cc b/src/asp/Tools/cam_test.cc index 5eb9015dd..2b3a0dd91 100644 --- a/src/asp/Tools/cam_test.cc +++ b/src/asp/Tools/cam_test.cc @@ -49,7 +49,8 @@ struct Options : vw::GdalWriteOptions { int sample_rate; // use one out of these many pixels double subpixel_offset, height_above_datum; bool enable_correct_velocity_aberration, enable_correct_atmospheric_refraction, - print_per_pixel_results, dg_use_csm, dg_vs_csm, test_error_propagation; + print_per_pixel_results, dg_use_csm, dg_vs_csm, aster_use_csm, aster_vs_csm, + test_error_propagation; vw::Vector2 single_pixel; Options() {} @@ -92,6 +93,10 @@ void handle_arguments(int argc, char *argv[], Options& opt) { "Use the CSM model with DigitalGlobe linescan cameras (-t dg). No corrections are done for velocity aberration or atmospheric refraction.") ("dg-vs-csm", po::bool_switch(&opt.dg_vs_csm)->default_value(false)->implicit_value(true), "Compare projecting into the camera without and with using the CSM model for Digital Globe.") + ("aster-use-csm", po::bool_switch(&opt.aster_use_csm)->default_value(false)->implicit_value(true), + "Use the CSM model with ASTER cameras (-t aster).") + ("aster-vs-csm", po::bool_switch(&opt.aster_vs_csm)->default_value(false)->implicit_value(true), + "Compare projecting into the camera without and with using the CSM model for ASTER.") ("bundle-adjust-prefix", po::value(&opt.bundle_adjust_prefix), "Adjust the cameras using this prefix.") ("test-error-propagation", po::bool_switch(&opt.test_error_propagation)->default_value(false)->implicit_value(true), @@ -123,16 +128,17 @@ void handle_arguments(int argc, char *argv[], Options& opt) { asp::stereo_settings().enable_correct_atmospheric_refraction = opt.enable_correct_atmospheric_refraction; asp::stereo_settings().dg_use_csm = opt.dg_use_csm; + asp::stereo_settings().aster_use_csm = opt.aster_use_csm; // Need this to be able to load adjusted camera models. This must be set // before loading the cameras. asp::stereo_settings().bundle_adjust_prefix = opt.bundle_adjust_prefix; if (opt.test_error_propagation) { - if (!asp::stereo_settings().dg_use_csm) { - vw_out() << "Enabling option --dg-use-csm as point cloud stddev will be computed.\n"; - asp::stereo_settings().dg_use_csm = true; - } + vw_out() << "Using the CSM model for Digital Globe or ASTER when " + << "testing error propagation.\n"; + asp::stereo_settings().dg_use_csm = true; + asp::stereo_settings().aster_use_csm = true; asp::stereo_settings().propagate_errors = true; } } @@ -184,14 +190,14 @@ void testErrorPropagation(Options const& opt, break; } - std::cout << "Left pixel: " << pix1 << std::endl; - std::cout << "Right pixel: " << pix2 << std::endl; + vw::vw_out() << "Left pixel: " << pix1 << std::endl; + vw::vw_out() << "Right pixel: " << pix2 << std::endl; auto const& v = asp::stereo_settings().horizontal_stddev; // alias vw::Vector2 ans = asp::propagateCovariance(triPt, datum, v[0], v[1], cam1_model.get(), cam2_model.get(), pix1, pix2); - std::cout << "Horizontal and vertical stddev: " << ans << std::endl; + vw::vw_out() << "Horizontal and vertical stddev: " << ans << std::endl; } int main(int argc, char *argv[]) { @@ -289,7 +295,7 @@ int main(int argc, char *argv[]) { double major_axis = datum.semi_major_axis() + opt.height_above_datum; double minor_axis = datum.semi_minor_axis() + opt.height_above_datum; // Iterate over the image - std::vector ctr_diff, dir_diff, cam1_to_cam2_diff, cam2_to_cam1_diff, dg_vs_csm_diff; + std::vector ctr_diff, dir_diff, cam1_to_cam2_diff, cam2_to_cam1_diff, nocsm_vs_csm_diff; for (int col = 0; col < image_cols; col += opt.sample_rate) { for (int row = 0; row < image_rows; row += opt.sample_rate) { @@ -320,18 +326,19 @@ int main(int argc, char *argv[]) { // camera. Vector3 xyz = vw::cartography::datum_intersection(major_axis, minor_axis, cam1_ctr, cam1_dir); - Vector2 cam2_pix = cam2_model->point_to_pixel(xyz); cam1_to_cam2_diff.push_back(norm_2(image_pix - cam2_pix)); if (opt.print_per_pixel_results) vw_out() << "cam1 to cam2 pixel diff: " << image_pix - cam2_pix << std::endl; - if (opt.dg_vs_csm) { + if (opt.dg_vs_csm || opt.aster_vs_csm) { asp::stereo_settings().dg_use_csm = !asp::stereo_settings().dg_use_csm; + asp::stereo_settings().aster_use_csm = !asp::stereo_settings().aster_use_csm; Vector2 cam2_pix2 = cam2_model->point_to_pixel(xyz); asp::stereo_settings().dg_use_csm = !asp::stereo_settings().dg_use_csm; - dg_vs_csm_diff.push_back(norm_2(cam2_pix - cam2_pix2)); + asp::stereo_settings().aster_use_csm = !asp::stereo_settings().aster_use_csm; + nocsm_vs_csm_diff.push_back(norm_2(cam2_pix - cam2_pix2)); } // Shoot a ray from the cam2 camera, intersect it with the @@ -345,11 +352,13 @@ int main(int argc, char *argv[]) { if (opt.print_per_pixel_results) vw_out() << "cam2 to cam1 pixel diff: " << image_pix - cam1_pix << "\n\n"; - if (opt.dg_vs_csm) { + if (opt.dg_vs_csm || opt.aster_vs_csm) { asp::stereo_settings().dg_use_csm = !asp::stereo_settings().dg_use_csm; + asp::stereo_settings().aster_use_csm = !asp::stereo_settings().aster_use_csm; Vector2 cam1_pix2 = cam1_model->point_to_pixel(xyz); asp::stereo_settings().dg_use_csm = !asp::stereo_settings().dg_use_csm; - dg_vs_csm_diff.push_back(norm_2(cam1_pix - cam1_pix2)); + asp::stereo_settings().aster_use_csm = !asp::stereo_settings().aster_use_csm; + nocsm_vs_csm_diff.push_back(norm_2(cam1_pix - cam1_pix2)); } if (single_pix) @@ -367,8 +376,8 @@ int main(int argc, char *argv[]) { print_diffs("cam1 to cam2 camera center diff (meters)", ctr_diff); print_diffs("cam1 to cam2 pixel diff", cam1_to_cam2_diff); print_diffs("cam2 to cam1 pixel diff", cam2_to_cam1_diff); - if (opt.dg_vs_csm) - print_diffs("dg vs csm pixel diff", dg_vs_csm_diff); + if (opt.dg_vs_csm || opt.aster_vs_csm) + print_diffs("no-csm vs csm pixel diff", nocsm_vs_csm_diff); double elapsed_sec = sw.elapsed_seconds(); vw_out() << "\nElapsed time per sample: " << 1e+6 * elapsed_sec/ctr_diff.size() diff --git a/src/asp/Tools/jitter_solve.cc b/src/asp/Tools/jitter_solve.cc index d06572417..1bb2b78d3 100644 --- a/src/asp/Tools/jitter_solve.cc +++ b/src/asp/Tools/jitter_solve.cc @@ -258,6 +258,7 @@ void handle_arguments(int argc, char *argv[], Options& opt) { // Set this before loading cameras, as jitter for DG can be modeled only with CSM // cameras. asp::stereo_settings().dg_use_csm = true; + asp::stereo_settings().aster_use_csm = true; std::vector inputs = opt.image_files; bool ensure_equal_sizes = true; diff --git a/src/asp/Tools/mapproject_single.cc b/src/asp/Tools/mapproject_single.cc index 99042f1e3..fc3f7dfeb 100644 --- a/src/asp/Tools/mapproject_single.cc +++ b/src/asp/Tools/mapproject_single.cc @@ -47,7 +47,8 @@ struct Options : vw::GdalWriteOptions { // Input std::string dem_file, image_file, camera_file, output_file, stereo_session, bundle_adjust_prefix; - bool isQuery, noGeoHeaderInfo, nearest_neighbor, parseOptions, dg_use_csm; + bool isQuery, noGeoHeaderInfo, nearest_neighbor, parseOptions, + dg_use_csm, aster_use_csm; bool multithreaded_model; // This is set based on the session type. bool enable_correct_velocity_aberration, enable_correct_atmospheric_refraction; @@ -102,6 +103,8 @@ void handle_arguments( int argc, char *argv[], Options& opt ) { "Turn on atmospheric refraction correction for Optical Bar and non-ISIS linescan cameras. This option impairs the convergence of bundle adjustment.") ("dg-use-csm", po::bool_switch(&opt.dg_use_csm)->default_value(false)->implicit_value(true), "Use the CSM model with DigitalGlobe linescan cameras (-t dg). No corrections are done for velocity aberration or atmospheric refraction.") + ("aster-use-csm", po::bool_switch(&opt.aster_use_csm)->default_value(false)->implicit_value(true), + "Use the CSM model with ASTER cameras (-t aster).") ("parse-options", po::bool_switch(&opt.parseOptions)->default_value(false), "Parse the options and print the results. Used by the mapproject script.") ; @@ -161,6 +164,7 @@ void handle_arguments( int argc, char *argv[], Options& opt ) { = opt.enable_correct_atmospheric_refraction; asp::stereo_settings().dg_use_csm = opt.dg_use_csm; + asp::stereo_settings().aster_use_csm = opt.aster_use_csm; if (fs::path(opt.dem_file).extension() != "") { // A path to a real DEM file was provided, load it!