mix symbolic and autodiff bundle

pyTests/camera/test_equidistant.py

@@ -120,7 +120,7 @@
 # - Vec2 project(geometry::Pose3& pose, Vec4& pt3D, bool applyDistortion = true) /
 #                    Vec2, Pose3 and Vec4 not binded
 # - Vec2 project(Eigen::Matrix4d& pose, Vec4& pt3D, bool applyDistortion = true)
-# - Vec3 backproject(Vec2& pt2D, bool applyUndistortion = true,
+# - Vec3 backprojectTransform(Vec2& pt2D, bool applyUndistortion = true,
 #                    geometry::Pose3& pose = geometry::Pose3(),
 #                    double depth = 1.0) / Vec3, Vec2 and Pose3 not binded
 # - Vec4 getCartesianfromSphericalCoordinates(Vec3& pt) / Vec3 not binded

pyTests/camera/test_pinhole.py

@@ -110,7 +110,7 @@
 # - Vec2 project(geometry::Pose3& pose, Vec4& pt3D, bool applyDistortion = true) /
 #                    Vec2, Pose3 and Vec4 not binded
 # - Vec2 project(Eigen::Matrix4d& pose, Vec4& pt3D, bool applyDistortion = true)
-# - Vec3 backproject(Vec2& pt2D, bool applyUndistortion = true,
+# - Vec3 backprojectTransform(Vec2& pt2D, bool applyUndistortion = true,
 #                    geometry::Pose3& pose = geometry::Pose3(),
 #                    double depth = 1.0) / Vec3, Vec2 and Pose3 not binded
 # - Vec4 getCartesianfromSphericalCoordinates(Vec3& pt) / Vec3 not binded

src/aliceVision/camera/Equidistant.cpp

@@ -510,7 +510,30 @@ double Equidistant::pixelProbability() const
     return 1.0 / double(w());
 }
 
+Eigen::Matrix<double, 3, Eigen::Dynamic> Equidistant::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const 
+{
+    size_t disto_size = getDistortionParamsSize();
+
+    const Vec2 ptMeters = ima2cam(pt2D);
+    const Vec2 ptUndist = removeDistortion(ptMeters);
+    const Vec3 ptSphere = toUnitSphere(ptUndist);
+
+
+
+
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParams().size());
 
+    J.block<3, 2>(0, 0) = getDerivativetoUnitSphereWrtScale(ptUndist);
+    J.block<3, 2>(0, 2) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtPrincipalPoint();
+
+
+    if (disto_size > 0)
+    {
+        J.block(0, 4, 3, disto_size) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
+    }
+
+    return J;
+}
 
 }  // namespace camera
 }  // namespace aliceVision

src/aliceVision/camera/Equidistant.hpp

@@ -74,7 +74,7 @@ class Equidistant : public IntrinsicScaleOffsetDisto
 
     Vec2 project(const Vec4& pt, bool applyDistortion = true) const override;
 
-    Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const;
+    Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
 
     Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
 
@@ -112,6 +112,13 @@ class Equidistant : public IntrinsicScaleOffsetDisto
 
     Eigen::Matrix2d getDerivativeIma2CamWrtPrincipalPoint() const override;
 
+    /**
+     * @brief Get the derivative of the unit sphere backprojection
+     * @param[in] pt2D The 2D point
+     * @return The backproject jacobian with respect to the pose
+     */
+    Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override;
+
     /**
      * @brief Return true if this ray should be visible in the image
      * @param[in] ray the ray that may or may not be visible in the image

src/aliceVision/camera/IntrinsicBase.cpp

@@ -17,7 +17,7 @@ bool IntrinsicBase::operator==(const IntrinsicBase& other) const
            _serialNumber == other._serialNumber && _initializationMode == other._initializationMode && getType() == other.getType();
 }
 
-Vec3 IntrinsicBase::backproject(const Vec2& pt2D, bool applyUndistortion, const geometry::Pose3& pose, double depth) const
+Vec3 IntrinsicBase::backprojectTransform(const Vec2& pt2D, bool applyUndistortion, const geometry::Pose3& pose, double depth) const
 {
     const Vec2 pt2D_cam = ima2cam(pt2D);
     const Vec2 pt2D_undist = applyUndistortion ? removeDistortion(pt2D_cam) : pt2D_cam;
@@ -27,6 +27,15 @@ Vec3 IntrinsicBase::backproject(const Vec2& pt2D, bool applyUndistortion, const
     return output;
 }
 
+Vec3 IntrinsicBase::backProjectUnit(const Vec2& pt2D) const
+{
+    const Vec2 ptMeters = ima2cam(pt2D);
+    const Vec2 ptUndist = removeDistortion(ptMeters);
+    const Vec3 ptSphere = toUnitSphere(ptUndist);
+
+    return ptSphere;
+}
+
 Vec4 IntrinsicBase::getCartesianfromSphericalCoordinates(const Vec3& pt)
 {
     Vec4 rpt;

src/aliceVision/camera/IntrinsicBase.hpp

@@ -123,7 +123,14 @@ class IntrinsicBase
      * @param[in] depth The depth
      * @return The 3D point
      */
-    Vec3 backproject(const Vec2& pt2D, bool applyUndistortion = true, const geometry::Pose3& pose = geometry::Pose3(), double depth = 1.0) const;
+    Vec3 backprojectTransform(const Vec2& pt2D, bool applyUndistortion = true, const geometry::Pose3& pose = geometry::Pose3(), double depth = 1.0) const;
+
+    /**
+     * @brief Back-projection of a 2D point on a unitsphere
+     * @param[in] pt2D The 2D point
+     * @return The 3D point
+     */
+    Vec3 backProjectUnit(const Vec2& pt2D) const;
 
     Vec4 getCartesianfromSphericalCoordinates(const Vec3& pt);
 
@@ -137,6 +144,8 @@ class IntrinsicBase
      */
     virtual Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
 
+    virtual Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const = 0;
+
     /**
      * @brief Get the derivative of a projection of a 3D point into the camera plane
      * @param[in] pose The pose
@@ -169,7 +178,12 @@ class IntrinsicBase
      */
     virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;
 
-
+    /**
+     * @brief Get the derivative of the unit sphere backprojection
+     * @param[in] pt2D The 2D point
+     * @return The backproject jacobian with respect to the pose
+     */
+    virtual Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const = 0;
 
     /**
      * @brief Compute the residual between the 3D projected point X and an image observation x

src/aliceVision/camera/Pinhole.cpp

@@ -56,7 +56,7 @@ Vec2 Pinhole::project(const Vec4& pt, bool applyDistortion) const
     return impt;
 }
 
-Eigen::Matrix<double, 2, 9> Pinhole::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt)
+Eigen::Matrix<double, 2, 9> Pinhole::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
 {
     const Vec4 X = pose * pt;  // apply pose
 
@@ -282,6 +282,29 @@ Eigen::Matrix<double, 3, 2> Pinhole::getDerivativetoUnitSphereWrtPoint(const Vec
     return (norm * d_ptcam_d_pt - ptcam * d_norm_d_pt) / norm2;
 }
 
+Eigen::Matrix<double, 3, Eigen::Dynamic> Pinhole::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const 
+{
+    size_t disto_size = getDistortionParamsSize();
+
+    const Vec2 ptMeters = ima2cam(pt2D);
+    const Vec2 ptUndist = removeDistortion(ptMeters);
+    const Vec3 ptSphere = toUnitSphere(ptUndist);
+
+
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParams().size());
+
+    J.block<3, 2>(0, 0) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtScale(pt2D);
+    J.block<3, 2>(0, 2) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtPrincipalPoint();
+
+    if (disto_size > 0)
+    {
+        J.block(0, 4, 3, disto_size) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
+    }
+
+
+    return J;
+}
+
 double Pinhole::imagePlaneToCameraPlaneError(double value) const { return value / _scale(0); }
 
 Mat34 Pinhole::getProjectiveEquivalent(const geometry::Pose3& pose) const

src/aliceVision/camera/Pinhole.hpp

@@ -72,7 +72,7 @@ class Pinhole : public IntrinsicScaleOffsetDisto
 
     Vec2 project(const Vec4& pt, bool applyDistortion = true) const override;
 
-    Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt);
+    Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
 
     Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
 
@@ -94,6 +94,13 @@ class Pinhole : public IntrinsicScaleOffsetDisto
 
     Eigen::Matrix<double, 3, 2> getDerivativetoUnitSphereWrtPoint(const Vec2& pt) const;
 
+    /**
+     * @brief Get the derivative of the unit sphere backprojection
+     * @param[in] pt2D The 2D point
+     * @return The backproject jacobian with respect to the pose
+     */
+    Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override;
+
     double imagePlaneToCameraPlaneError(double value) const override;
 
     Mat34 getProjectiveEquivalent(const geometry::Pose3& pose) const;

src/aliceVision/camera/equidistant_test.cpp

@@ -60,7 +60,7 @@ BOOST_AUTO_TEST_CASE(cameraEquidistant_disto_undisto_Radial)
         const double depth_gt = std::abs(Vec2::Random()(0)) * 100.0;
         const geometry::Pose3 pose(geometry::randomPose());
 
-        const Vec3 pt3d = cam->backproject(ptImage_gt, true, pose, depth_gt);
+        const Vec3 pt3d = cam->backprojectTransform(ptImage_gt, true, pose, depth_gt);
         const Vec2 pt2d_proj = cam->transformProject(pose, pt3d.homogeneous(), true);
 
         EXPECT_MATRIX_NEAR(ptImage_gt, pt2d_proj, epsilon);

src/aliceVision/camera/pinholeBrown_test.cpp

@@ -50,7 +50,7 @@ BOOST_AUTO_TEST_CASE(cameraPinholeBrown_disto_undisto_T2)
         const double depth_gt = std::abs(Vec2::Random()(0)) * 100.0;
         const geometry::Pose3 pose(geometry::randomPose());
 
-        const Vec3 pt3d = cam->backproject(ptImage_gt, true, pose, depth_gt);
+        const Vec3 pt3d = cam->backprojectTransform(ptImage_gt, true, pose, depth_gt);
         const Vec2 pt2d_proj = cam->transformProject(pose, pt3d.homogeneous(), true);
 
         EXPECT_MATRIX_NEAR(ptImage_gt, pt2d_proj, epsilon);

src/aliceVision/camera/pinholeFisheye1_test.cpp

@@ -50,7 +50,7 @@ BOOST_AUTO_TEST_CASE(cameraPinholeFisheye_disto_undisto_Fisheye1)
         const double depth_gt = std::abs(Vec2::Random()(0)) * 100.0;
         const geometry::Pose3 pose(geometry::randomPose());
 
-        const Vec3 pt3d = cam->backproject(ptImage_gt, true, pose, depth_gt);
+        const Vec3 pt3d = cam->backprojectTransform(ptImage_gt, true, pose, depth_gt);
         const Vec2 pt2d_proj = cam->transformProject(pose, pt3d.homogeneous(), true);
 
         EXPECT_MATRIX_NEAR(ptImage_gt, pt2d_proj, epsilon);

src/aliceVision/camera/pinholeFisheye_test.cpp

@@ -50,7 +50,7 @@ BOOST_AUTO_TEST_CASE(cameraPinholeFisheye_disto_undisto_Fisheye)
         const double depth_gt = std::abs(Vec2::Random()(0)) * 100.0;
         const geometry::Pose3 pose(geometry::randomPose());
 
-        const Vec3 pt3d = cam->backproject(ptImage_gt, true, pose, depth_gt);
+        const Vec3 pt3d = cam->backprojectTransform(ptImage_gt, true, pose, depth_gt);
         const Vec2 pt2d_proj = cam->transformProject(pose, pt3d.homogeneous(), true);
 
         EXPECT_MATRIX_NEAR(ptImage_gt, pt2d_proj, epsilon);

src/aliceVision/camera/pinholeRadial_test.cpp

@@ -51,7 +51,7 @@ BOOST_AUTO_TEST_CASE(cameraPinholeRadial_disto_undisto_K1)
         const double depth_gt = std::abs(Vec2::Random()(0)) * 100.0;
         const geometry::Pose3 pose(geometry::randomPose());
 
-        const Vec3 pt3d = cam->backproject(ptImage_gt, true, pose, depth_gt);
+        const Vec3 pt3d = cam->backprojectTransform(ptImage_gt, true, pose, depth_gt);
         const Vec2 pt2d_proj = cam->transformProject(pose, pt3d.homogeneous(), true);
 
         EXPECT_MATRIX_NEAR(ptImage_gt, pt2d_proj, epsilon);
@@ -95,7 +95,7 @@ BOOST_AUTO_TEST_CASE(cameraPinholeRadial_disto_undisto_K3)
         const double depth_gt = std::abs(Vec2::Random()(0)) * 100.0;
         const geometry::Pose3 pose(geometry::randomPose());
 
-        const Vec3 pt3d = cam->backproject(ptImage_gt, true, pose, depth_gt);
+        const Vec3 pt3d = cam->backprojectTransform(ptImage_gt, true, pose, depth_gt);
         const Vec2 pt2d_proj = cam->transformProject(pose, pt3d.homogeneous(), true);
 
         EXPECT_MATRIX_NEAR(ptImage_gt, pt2d_proj, epsilon);

src/aliceVision/fuseCut/PointCloud.cpp

@@ -183,7 +183,7 @@ void createVerticesWithVisibilities(const StaticVector<int>& cams,
                 if (depth <= 0.0f)
                     continue;
 
-                const Point3d p = mp.backproject(c, Point2d(x, y), depth);
+                const Point3d p = mp.backprojectTransform(c, Point2d(x, y), depth);
                 const double pixSize = mp.getCamPixelSize(p, c);
 
                 nanoflann::KNNResultSet<double, std::size_t> resultSet(1);

src/aliceVision/mesh/MeshIntersection.cpp

@@ -31,7 +31,7 @@ bool MeshIntersection::initialize(const std::string & pathToModel)
 bool MeshIntersection::peekPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
 {
     const Vec3 posCamera = _pose.center();
-    const Vec3 wdir = intrinsic.backproject(imageCoords, true, _pose, 1.0);
+    const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0);
     const Vec3 dir = (wdir - posCamera).normalized();
 
     //Create geogram ray from alicevision ray
@@ -61,7 +61,7 @@ bool MeshIntersection::peekPoint(Vec3 & output, const camera::IntrinsicBase & in
 bool MeshIntersection::peekNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
 {
     const Vec3 posCamera = _pose.center();
-    const Vec3 wdir = intrinsic.backproject(imageCoords, true, _pose, 1.0);
+    const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0);
     const Vec3 dir = (wdir - posCamera).normalized();
 
     //Create geogram ray from alicevision ray

src/aliceVision/mvsUtils/MultiViewParams.hpp

@@ -122,7 +122,7 @@ class MultiViewParams
 
     ~MultiViewParams();
 
-    inline Point3d backproject(const int camIndex, const Point2d& pix, double depth) const
+    inline Point3d backprojectTransform(const int camIndex, const Point2d& pix, double depth) const
     {
         const Point3d p = CArr[camIndex] + (iCamArr[camIndex] * pix).normalize() * depth;
         return p;

src/aliceVision/sfm/CMakeLists.txt

@@ -2,13 +2,11 @@
 set(sfm_bundle_files_headers
   bundle/BundleAdjustment.hpp
   bundle/BundleAdjustmentCeres.hpp
-  bundle/BundleAdjustmentSymbolicCeres.hpp
 )
 
 # Sources
 set(sfm_bundle_files_sources
   bundle/BundleAdjustmentCeres.cpp
-  bundle/BundleAdjustmentSymbolicCeres.cpp
 )
 
 set(sfm_files_headers
@@ -47,7 +45,6 @@ set(sfm_files_headers
   utils/syntheticScene.hpp
   LocalBundleAdjustmentGraph.hpp
   FrustumFilter.hpp
-  ResidualErrorFunctor.hpp
   filters.hpp
   generateReport.hpp
   sfm.hpp