From eb02f11be6e035bb4c0ab276637e05fc4d419788 Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jam826@nau.edu>
Date: Mon, 11 Feb 2019 13:32:45 -0700
Subject: [PATCH] Added debug output to ground to image error message. (#168)

---
 src/UsgsAstroLsSensorModel.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index fbe7b16..96c49c8 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -614,9 +614,15 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
    // If the final correction is greater than 10 meters,
    // the solution is not valid enough to report even with a warning
    if (len > 100.0) {
+      std::ostringstream msg;
+      msg << "Did not converge. Ground point: (" << ground_pt.x << ", "
+          << ground_pt.y << ", " << ground_pt.z << ") Computed image point: ("
+          << calculatedPixel.line << ", " << calculatedPixel.samp
+          << ") Computed ground point: (" << calculatedPoint.x << ", "
+          << calculatedPoint.y << ", " << calculatedPoint.z << ")";
       throw csm::Error(
          csm::Error::ALGORITHM,
-         "Did not converge.",
+         msg.str(),
          "UsgsAstroLsSensorModel::groundToImage");
    }
 
-- 
GitLab