Upgrade survey injection for new format

Author: Fabien Servant

src/aliceVision/sfmData/SurveyPoint.hpp

@@ -18,17 +18,19 @@ struct SurveyPoint
 {
     SurveyPoint() = default;
 
-    SurveyPoint(const Vec3 & paramPoint3d, const Vec2 & paramSurvey)
+    SurveyPoint(const Vec3 & paramPoint3d, const Vec2 & paramSurvey, const Vec2 & paramResidual)
       : point3d(paramPoint3d), 
-        survey(paramSurvey)
+        survey(paramSurvey),
+        residual(paramResidual)
     {}
 
     Vec3 point3d;
     Vec2 survey;
+    Vec2 residual;
 
     bool operator==(const SurveyPoint& other) const
     {
-        return (point3d == other.point3d) && (survey == other.survey);
+        return (point3d == other.point3d) && (survey == other.survey) && (residual == other.residual);
     }
 
     inline bool operator!=(const SurveyPoint& other) const { return !(*this == other); }

src/aliceVision/sfmDataIO/AlembicExporter.cpp

@@ -676,6 +676,8 @@ void AlembicExporter::addSurveys(const sfmData::SurveyPoints & points)
             data.push_back(sp.point3d.z());
             data.push_back(sp.survey.x());
             data.push_back(sp.survey.y());
+            data.push_back(sp.residual.x());
+            data.push_back(sp.residual.y());
         }
     }
 

src/aliceVision/sfmDataIO/AlembicImporter.cpp

@@ -468,7 +468,7 @@ bool readSurveys(IObject iObj, sfmData::SfMData& sfmdata)
     IDoubleArrayProperty propData(userProps, "mvg_surveyData");
     propData.get(sampleData);
 
-    const int sampleDataSize = 5;
+    const int sampleDataSize = 7;
 
     if (sampleId.size() * sampleDataSize != sampleData->size())
     {
@@ -486,6 +486,8 @@ bool readSurveys(IObject iObj, sfmData::SfMData& sfmdata)
         pt.point3d.z() = (*sampleData)[pos * sampleDataSize + 2];
         pt.survey.x() = (*sampleData)[pos * sampleDataSize + 3];
         pt.survey.y() = (*sampleData)[pos * sampleDataSize + 4];
+        pt.residual.x() = (*sampleData)[pos * sampleDataSize + 5];
+        pt.residual.y() = (*sampleData)[pos * sampleDataSize + 6];
 
         output[viewId].push_back(pt);
     }

src/aliceVision/sfmDataIO/jsonIO.cpp

@@ -598,6 +598,8 @@ void saveSurveyPoints(const sfmData::SurveyPoints& spoints, bpt::ptree& parentTr
             pointTree.put("Z", point.point3d.z());
             pointTree.put("u", point.survey.x());
             pointTree.put("v", point.survey.y());
+            pointTree.put("ru", point.residual.x());
+            pointTree.put("rv", point.residual.y());
 
             pointsTree.push_back(std::make_pair("", pointTree));
         }
@@ -623,6 +625,8 @@ void loadSurveyPoints(sfmData::SurveyPoints& spoints, bpt::ptree& parentTree)
             p.point3d.z() = pointTree.second.get<double>("Z");
             p.survey.x() = pointTree.second.get<double>("u");
             p.survey.y() = pointTree.second.get<double>("v");
+            p.residual.x() = pointTree.second.get<double>("ru");
+            p.residual.y() = pointTree.second.get<double>("rv");
 
             spoints[viewId].push_back(p);
         }

src/software/utils/main_sfmSurveyInjecting.cpp

@@ -27,62 +27,104 @@
 using namespace aliceVision;
 namespace po = boost::program_options;
 
-struct SurveyObservation
+// Internal structures for json reading
+
+struct Position3D 
 {
-    Vec3 point3d;
-    Vec2 point2d;
+    double x;
+    double y;
+    double z;
 };
 
-struct SurveyInput
+struct ImagePoint 
 {
-    int frameId;
-    std::vector<SurveyObservation> observations;
+    int frame;
+    double x;
+    double y;
 };
 
-/**
- * @brief get a survey from a boost json object (assume the file format is ok)
- * @param obj the input json object
- * @param readSurvey the output survey information
- * @return false if the process failed
- */
-bool getSurveyFromJson(const boost::json::object& obj, SurveyInput& readSurvey)
+struct Point 
 {
-    readSurvey.observations.clear();
-    readSurvey.frameId = boost::json::value_to<IndexT>(obj.at("frame_no"));
-
-    boost::json::value jv = obj.at("points");
-    if (!jv.is_array())
-    {
-        return false;
-    }
+    std::string point_id;
+    std::string point_name;
+    std::string calc_mode;
+    std::string survey_mode;
+    Position3D calc_3d;
+    Position3D survey_3d;
+    std::vector<ImagePoint> image_points;
+};
 
-    boost::json::array vobj = jv.as_array();
-    for (auto item : vobj)
-    {
-        const boost::json::object& obj = item.as_object();
+struct Survey 
+{
+    std::string pgroup_name;
+    std::string camera_name;
+    std::vector<Point> points;
+};
 
-        SurveyObservation obs;
-        obs.point3d.x() = boost::json::value_to<double>(obj.at("X"));
-        obs.point3d.y() = boost::json::value_to<double>(obj.at("Y"));
-        obs.point3d.z() = boost::json::value_to<double>(obj.at("Z"));
+Position3D tag_invoke(boost::json::value_to_tag<Position3D>, boost::json::value const& jv) 
+{
+    boost::json::object const& obj = jv.as_object();
+    return Position3D{
+        boost::json::value_to<double>(obj.at("x")),
+        boost::json::value_to<double>(obj.at("y")),
+        boost::json::value_to<double>(obj.at("z"))
+    };
+}
 
-        obs.point2d.x() = boost::json::value_to<double>(obj.at("u"));
-        obs.point2d.y() = boost::json::value_to<double>(obj.at("v"));
+ImagePoint tag_invoke(boost::json::value_to_tag<ImagePoint>, boost::json::value const& jv) 
+{
+    boost::json::object const& obj = jv.as_object();
+    return ImagePoint{
+        boost::json::value_to<int>(obj.at("frame")),
+        boost::json::value_to<double>(obj.at("x")),
+        boost::json::value_to<double>(obj.at("y"))
+    };
+}
 
-        readSurvey.observations.push_back(obs);
+Point tag_invoke(boost::json::value_to_tag<Point>, boost::json::value const& jv) 
+{
+    boost::json::object const& obj = jv.as_object();
+    Point pt;
+    pt.point_id = boost::json::value_to<std::string>(obj.at("point_id"));
+    pt.point_name = boost::json::value_to<std::string>(obj.at("point_name"));
+    pt.calc_mode = boost::json::value_to<std::string>(obj.at("calc_mode"));
+    pt.survey_mode = boost::json::value_to<std::string>(obj.at("survey_mode"));
+    pt.calc_3d = boost::json::value_to<Position3D>(obj.at("calc_3d"));
+    pt.survey_3d = boost::json::value_to<Position3D>(obj.at("survey_3d"));
+    
+    boost::json::array const& img_points = obj.at("image_points").as_array();
+    for (auto const& ip : img_points) 
+    {
+        pt.image_points.push_back(boost::json::value_to<ImagePoint>(ip));
     }
+    
+    return pt;
+}
 
-    return true;
+Survey tag_invoke(boost::json::value_to_tag<Survey>, boost::json::value const& jv) 
+{
+    boost::json::object const& obj = jv.as_object();
+    Survey survey;
+    survey.pgroup_name = boost::json::value_to<std::string>(obj.at("pgroup_name"));
+    survey.camera_name = boost::json::value_to<std::string>(obj.at("camera_name"));
+    
+    boost::json::array const& points = obj.at("points").as_array();
+    for (auto const& pt : points) 
+    {
+        survey.points.push_back(boost::json::value_to<Point>(pt));
+    }
+    
+    return survey;
 }
 
 /**
  * @brief Get a set of surveys from a JSON file (assumes the file format is ok).
  * The JSON file contains an array of objects. Each object describes a frameId, and a list of points.
  * @param surveyFilename the input JSON filename
- * @param output the read vector of surveyinput
+ * @param output a Survey object filled
  * @return false if the process failed, true otherwise
  */
-bool getSurveysFromJson(const std::string& surveyFilename, std::vector<SurveyInput> & output)
+bool getSurveysFromJson(const std::string& surveyFilename, Survey & output)
 {
     std::ifstream inputfile(surveyFilename);
     if (!inputfile.is_open())
@@ -94,23 +136,7 @@ bool getSurveysFromJson(const std::string& surveyFilename, std::vector<SurveyInp
     buffer << inputfile.rdbuf();
     boost::json::value jv = boost::json::parse(buffer.str());
 
-    if (!jv.is_array())
-    {
-        return false;
-    }
-
-    boost::json::array vobj = jv.as_array();
-
-    for (auto item : vobj)
-    {
-        const boost::json::object& obj = item.as_object();
-
-        SurveyInput input;
-        if (getSurveyFromJson(obj, input))
-        {
-            output.push_back(input);
-        }
-    }
+    output = boost::json::value_to<Survey>(jv);
 
     return true;
 }
@@ -164,56 +190,79 @@ int aliceVision_main(int argc, char** argv)
         return EXIT_FAILURE;
     }
 
-    auto & landmarks = sfmData.getLandmarks();
+    // Get First Frame ID as 3DE gives a [0, nbFrames - 1] frame ID
+    IndexT minFrameId = std::numeric_limits<IndexT>::max();
+    for (const auto& [viewId, pView] : sfmData.getViews())
+    {
+        minFrameId = std::min(minFrameId, pView->getFrameId());
+    }
+    ALICEVISION_LOG_INFO("Minimal frame id in sfmData is " << minFrameId << ".");
 
-    std::vector<SurveyInput> surveys;
-    if (!getSurveysFromJson(surveyFilename, surveys))
+    // Read survey points in an intermediate structure 
+    Survey survey;
+    if (!getSurveysFromJson(surveyFilename, survey))
     {
         ALICEVISION_LOG_ERROR("The survey file '" + surveyFilename + "' cannot be read.");
         return EXIT_FAILURE;
     }
 
     sfmData::SurveyPoints & spoints = sfmData.getSurveyPoints();
 
-    // Set the pose for all the views with frame IDs found in the JSON file
-    size_t idFeature = 0;
-    for (const auto& [viewId, pView] : sfmData.getViews())
+    // Fill up sfmData with the intermediate structure
+    for (const auto& [viewId, view] : sfmData.getViews().valueRange())
     {
-        IndexT frameId = pView->getFrameId();
-
-        if (!sfmData.isIntrinsicDefined(viewId))
+        if (!sfmData.isPoseAndIntrinsicDefined(viewId))
         {
             continue;
         }
 
-        const auto & intrinsic = sfmData.getIntrinsic(pView->getIntrinsicId());
+        const auto & intrinsic = sfmData.getIntrinsic(view.getIntrinsicId());
 
-        for (const auto & survey: surveys)
-        {
-            if (frameId != survey.frameId)
-            {
-                continue;
-            }
+        std::vector<sfmData::SurveyPoint> spts;
 
-            for (const auto & obs : survey.observations)
-            {
-                sfmData::SurveyPoint p;
+        IndexT frameId = view.getFrameId();
 
-                p.survey.x() = obs.point2d.x() * intrinsic.w();
-                p.survey.y() = intrinsic.h() - 1.0 - (obs.point2d.y() * intrinsic.h());
+        //We assume this pose is also coming from 3DE and has not been modified !
+        geometry::Pose3 pose = sfmData.getPose(view).getTransform();
 
-                p.point3d.x() = obs.point3d.x();
-                p.point3d.y() = - obs.point3d.y();
-                p.point3d.z() = - obs.point3d.z();
+        for (const auto & point: survey.points)
+        {
+            sfmData::SurveyPoint p;
 
-                spoints[viewId].push_back(p);
+            p.point3d.x() = point.survey_3d.x;
+            p.point3d.y() = -point.survey_3d.y;
+            p.point3d.z() = -point.survey_3d.z;
 
-                idFeature++;
+            for (const auto & image_point: point.image_points)
+            {
+                // Json frame is the sequence offset starting by 1.
+                if (frameId != (image_point.frame - 1 + minFrameId))
+                {
+                    continue;
+                }
+
+                // From 3DE representation to Alicevision
+                p.survey.x() = image_point.x * intrinsic.w();
+                p.survey.y() = (1.0 - image_point.y) * intrinsic.h();
+
+                // Compute residual
+                Vec2 obsEstimated = intrinsic.transformProject(pose, p.point3d.homogeneous(), true);                
+                p.residual.x() = p.survey.x() - obsEstimated.x();
+                p.residual.y() = p.survey.y() - obsEstimated.y();
+
+                spts.push_back(p);
+                
+                ALICEVISION_LOG_INFO("Found observation for frame " << frameId << ".");
+                ALICEVISION_LOG_INFO("X residual : " << std::abs(p.survey.x() - obsEstimated.x()));
+                ALICEVISION_LOG_INFO("Y residual : " << std::abs(p.survey.y() - obsEstimated.y()));
             }
         }
-    }
 
-    std::cout << sfmData.getSurveyPoints().size() << std::endl;
+        if (spts.size() > 0)
+        {
+            spoints[viewId] = spts;
+        }
+    }
 
     sfmDataIO::save(sfmData, sfmDataOutputFilename, sfmDataIO::ESfMData::ALL);