Add relative landmarks concept

Author: Fabien Servant

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -533,7 +533,17 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
 
         std::array<double, 3>& landmarkBlock = _landmarksBlocks[landmarkId];
         for (std::size_t i = 0; i < 3; ++i)
+        {
             landmarkBlock.at(i) = landmark.X(Eigen::Index(i));
+        }
+
+        //If the landmark has a referenceViewIndex set, then retrieve the reference pose
+        double * referencePoseBlockPtr = nullptr;
+        if (landmark.referenceViewIndex != UndefinedIndexT)
+        {
+            const sfmData::View& refview = sfmData.getView(landmark.referenceViewIndex);
+            referencePoseBlockPtr = _posesBlocks.at(refview.getPoseId()).data();
+        }
 
         double* landmarkBlockPtr = landmarkBlock.data();
         problem.AddParameterBlock(landmarkBlockPtr, 3);
@@ -590,6 +600,23 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
 
                 problem.AddResidualBlock(costFunction, lossFunction, params);
             }
+            if (referencePoseBlockPtr != nullptr)
+            {
+                bool samePose = (referencePoseBlockPtr == poseBlockPtr);
+                ceres::CostFunction* costFunction = ProjectionRelativeErrorFunctor::createCostFunction(intrinsic, observation, samePose);
+ 
+                std::vector<double*> params;
+                params.push_back(intrinsicBlockPtr);
+                params.push_back(distortionBlockPtr);
+                if (!samePose)
+                {
+                    params.push_back(poseBlockPtr);
+                    params.push_back(referencePoseBlockPtr);
+                }
+                params.push_back(landmarkBlockPtr);
+
+                problem.AddResidualBlock(costFunction, lossFunction, params);
+            }
             else
             {
                 ceres::CostFunction* costFunction = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation);

src/aliceVision/sfm/bundle/costfunctions/projection.hpp

@@ -281,6 +281,130 @@ struct ProjectionErrorFunctor
     ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
 };
 
+struct ProjectionRelativeErrorFunctor
+{
+    explicit ProjectionRelativeErrorFunctor(const sfmData::Observation& obs
+    , const std::shared_ptr<camera::IntrinsicBase>& intrinsics
+    , bool noPose)        
+    : _intrinsicFunctor(new CostIntrinsicsProject(obs, intrinsics))
+    , withoutPose(noPose)
+    {        
+    }
+
+    template<typename T>
+    bool operator()(T const* const* parameters, T* residuals) const
+    {       
+        const T* parameter_intrinsics = parameters[0];
+        const T* parameter_distortion = parameters[1];
+        
+        
+        T transformedPoint[3];
+        if (withoutPose)
+        {
+            //withoutPose is used if reference view and current view are the same views
+            const T* parameter_relativepoint = parameters[2];
+            transformedPoint[0] = parameter_relativepoint[0];
+            transformedPoint[1] = parameter_relativepoint[1];
+            transformedPoint[2] = parameter_relativepoint[2];
+        }
+        else
+        {
+            const T* parameter_pose = parameters[2];
+            const T* parameter_refpose = parameters[3];
+            const T* parameter_relativepoint = parameters[4];
+
+            //Retrieve point
+            T relpoint[3];
+            relpoint[0] = parameter_relativepoint[0];
+            relpoint[1] = parameter_relativepoint[1];
+            relpoint[2] = parameter_relativepoint[2];
+
+            const T* refcam_R = parameter_refpose;
+            const T* refcam_t = &parameter_refpose[3];
+            
+            // Apply transformation such that the point
+            // Which was defined in the camera geometric frame
+            // is now defined in the world frame
+            relpoint[0] = relpoint[0] - refcam_t[0];
+            relpoint[1] = relpoint[1] - refcam_t[1];
+            relpoint[2] = relpoint[2] - refcam_t[2];
+
+            T invrefcam_R[3];
+            invrefcam_R[0] = -refcam_R[0];
+            invrefcam_R[1] = -refcam_R[1];
+            invrefcam_R[2] = -refcam_R[2];
+
+            T absolutePoint[3];  
+            ceres::AngleAxisRotatePoint(invrefcam_R, relpoint, absolutePoint);
+
+            //--
+            // Apply external parameters (Pose)
+            //--
+            const T* cam_R = parameter_pose;
+            const T* cam_t = &parameter_pose[3];
+            
+            
+            // Rotate the point according the camera rotation
+            ceres::AngleAxisRotatePoint(cam_R, absolutePoint, transformedPoint);
+
+            // Apply the camera translation
+            transformedPoint[0] += cam_t[0];
+            transformedPoint[1] += cam_t[1];
+            transformedPoint[2] += cam_t[2];
+        }
+
+        const T * innerParameters[3];
+        innerParameters[0] = parameter_intrinsics;
+        innerParameters[1] = parameter_distortion;
+        innerParameters[2] = transformedPoint;
+
+        return _intrinsicFunctor(innerParameters, residuals);
+    }
+
+    /**
+     * @brief Create the appropriate cost functor according the provided input camera intrinsic model
+     * @param[in] intrinsicPtr The intrinsic pointer
+     * @param[in] observation The corresponding observation
+     * @param[in] withoutPose When reference view and current view are the same, poses must be ignored
+     * @return cost functor
+     */
+    inline static ceres::CostFunction* createCostFunction(
+        const std::shared_ptr<camera::IntrinsicBase> intrinsic, 
+        const sfmData::Observation& observation,
+        bool withoutPose)
+    {
+        auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionRelativeErrorFunctor>(new ProjectionRelativeErrorFunctor(observation, intrinsic, withoutPose));
+
+        int distortionSize = 1;
+        auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
+        if (isod)
+        {
+            auto distortion = isod->getDistortion();
+            if (distortion)
+            {
+                distortionSize = distortion->getParameters().size();
+            }
+        }
+
+        costFunction->AddParameterBlock(intrinsic->getParameters().size());
+        costFunction->AddParameterBlock(distortionSize);
+
+        if (!withoutPose)
+        {
+            costFunction->AddParameterBlock(6);
+            costFunction->AddParameterBlock(6);
+        }
+
+        costFunction->AddParameterBlock(3);
+        costFunction->SetNumResiduals(2);
+
+        return costFunction;
+    }
+
+    ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
+    bool withoutPose;
+};
+
 
 }  // namespace sfm
 }  // namespace aliceVision

src/aliceVision/sfmData/Landmark.hpp

@@ -41,11 +41,15 @@ class Landmark
     feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED;
     image::RGBColor rgb = image::WHITE;  //!> the color associated to the point
     EEstimatorParameterState state = EEstimatorParameterState::REFINED;
+    IndexT referenceViewIndex = UndefinedIndexT;
 
     bool operator==(const Landmark& other) const
     {
-        return AreVecNearEqual(X, other.X, 1e-3) && AreVecNearEqual(rgb, other.rgb, 1e-3) && _observations == other._observations &&
-               descType == other.descType;
+        return AreVecNearEqual(X, other.X, 1e-3) 
+        && AreVecNearEqual(rgb, other.rgb, 1e-3) 
+        && _observations == other._observations 
+        && descType == other.descType
+        && referenceViewIndex == other.referenceViewIndex;
     }
 
     inline bool operator!=(const Landmark& other) const { return !(*this == other); }

src/aliceVision/sfmDataIO/AlembicExporter.cpp

@@ -505,11 +505,13 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
         return;
 
     // Fill vector with the values taken from AliceVision
+    std::vector<IndexT> referenceViewIndices;
     std::vector<V3f> positions;
     std::vector<Imath::C3f> colors;
     std::vector<Alembic::Util::uint32_t> descTypes;
     positions.reserve(landmarks.size());
     descTypes.reserve(landmarks.size());
+    referenceViewIndices.reserve(landmarks.size());
 
     // For all the 3d points in the hash_map
     for (const auto& landmark : landmarks)
@@ -520,6 +522,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
         positions.emplace_back(pt[0], -pt[1], -pt[2]);
         colors.emplace_back(color.r() / 255.f, color.g() / 255.f, color.b() / 255.f);
         descTypes.emplace_back(static_cast<Alembic::Util::uint8_t>(landmark.second.descType));
+        referenceViewIndices.emplace_back(landmark.second.referenceViewIndex);
     }
 
     std::vector<Alembic::Util::uint64_t> ids(positions.size());
@@ -542,6 +545,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
     OCompoundProperty userProps = pSchema.getUserProperties();
 
     OUInt32ArrayProperty(userProps, "mvg_describerType").set(descTypes);
+    OUInt32ArrayProperty(userProps, "mvg_referenceViewIndices").set(referenceViewIndices);
 
     if (withVisibility)
     {

src/aliceVision/sfmDataIO/AlembicImporter.cpp

@@ -186,6 +186,18 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
         }
     }
 
+    AV_UInt32ArraySamplePtr sampleReferenceViewIndices;
+    if (userProps && userProps.getPropertyHeader("mvg_referenceViewIndices"))
+    {
+        sampleReferenceViewIndices.read(userProps, "mvg_referenceViewIndices");
+        if (sampleReferenceViewIndices.size() != positions->size())
+        {
+            ALICEVISION_LOG_WARNING("[Alembic Importer] Describer type will be ignored. describerType vector size: "
+                                    << sampleReferenceViewIndices.size() << ", positions vector size: " << positions->size());
+            sampleReferenceViewIndices.reset();
+        }
+    }
+
     // Number of points before adding the Alembic data
     const std::size_t nbPointsInit = sfmdata.getLandmarks().size();
     for (std::size_t point3d_i = 0; point3d_i < positions->size(); ++point3d_i)
@@ -217,6 +229,11 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
             const std::size_t descType_i = sampleDescs[point3d_i];
             landmark.descType = static_cast<feature::EImageDescriberType>(descType_i);
         }
+
+        if (sampleReferenceViewIndices)
+        {
+            landmark.referenceViewIndex = sampleReferenceViewIndices[point3d_i];
+        }
     }
 
     // for compatibility with files generated with a previous version

src/aliceVision/sfmDataIO/jsonIO.cpp

@@ -500,6 +500,7 @@ void saveLandmark(const std::string& name,
     bpt::ptree landmarkTree;
 
     landmarkTree.put("landmarkId", landmarkId);
+    landmarkTree.put("referenceViewIndex", landmark.referenceViewIndex);
     landmarkTree.put("descType", feature::EImageDescriberType_enumToString(landmark.descType));
 
     saveMatrix("color", landmark.rgb, landmarkTree);
@@ -538,7 +539,7 @@ void loadLandmark(IndexT& landmarkId, sfmData::Landmark& landmark, bpt::ptree& l
 {
     landmarkId = landmarkTree.get<IndexT>("landmarkId");
     landmark.descType = feature::EImageDescriberType_stringToEnum(landmarkTree.get<std::string>("descType"));
-
+    landmark.referenceViewIndex = landmarkTree.get<IndexT>("referenceViewIndex", UndefinedIndexT);
     loadMatrix("color", landmark.rgb, landmarkTree);
     loadMatrix("X", landmark.X, landmarkTree);