From 0896265e01ce315e27c2143e2ca9f57eb30e1689 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 5 Nov 2024 10:44:35 +0100 Subject: [PATCH] Use same pose model for symbolic and autodiff --- src/aliceVision/geometry/lie.hpp | 166 +++++++++++++++++ .../bundle/BundleAdjustmentSymbolicCeres.cpp | 169 ++++++++++-------- .../bundle/BundleAdjustmentSymbolicCeres.hpp | 11 +- .../costfunctions/panoramaEquidistant.hpp | 30 ++-- .../bundle/costfunctions/panoramaPinhole.hpp | 32 ++-- .../sfm/bundle/costfunctions/projection.hpp | 55 ++++-- .../bundle/costfunctions/projectionSimple.hpp | 25 ++- .../bundle/costfunctions/rotationPrior.hpp | 30 ++-- src/aliceVision/sfm/bundle/manifolds/so3.hpp | 62 ------- 9 files changed, 360 insertions(+), 220 deletions(-) diff --git a/src/aliceVision/geometry/lie.hpp b/src/aliceVision/geometry/lie.hpp index c901074a7a..20ee04722d 100644 --- a/src/aliceVision/geometry/lie.hpp +++ b/src/aliceVision/geometry/lie.hpp @@ -118,6 +118,172 @@ inline Eigen::Vector3d logm(const Eigen::Matrix3d& R) return ret; } + +/** +Compute the jacobian of the logarithm wrt changes in the rotation matrix values +@param R the input rotation matrix +@return the jacobian matrix (3*9 matrix) +*/ +inline Eigen::Matrix dlogmdr(const Eigen::Matrix3d& R) +{ + double p1 = R(2, 1) - R(1, 2); + double p2 = R(0, 2) - R(2, 0); + double p3 = R(1, 0) - R(0, 1); + + double costheta = (R.trace() - 1.0) / 2.0; + if (costheta > 1.0) + costheta = 1.0; + else if (costheta < -1.0) + costheta = -1.0; + + double theta = acos(costheta); + + if (fabs(theta) < std::numeric_limits::epsilon()) + { + Eigen::Matrix J; + J.fill(0); + J(0, 5) = 1; + J(0, 7) = -1; + J(1, 2) = -1; + J(1, 6) = 1; + J(2, 1) = 1; + J(2, 3) = -1; + return J; + } + + double scale = theta / (2.0 * sin(theta)); + + Eigen::Vector3d resnoscale; + resnoscale(0) = p1; + resnoscale(1) = p2; + resnoscale(2) = p3; + + Eigen::Matrix dresdp = Eigen::Matrix3d::Identity() * scale; + Eigen::Matrix dpdmat; + dpdmat.fill(0); + dpdmat(0, 5) = 1; + dpdmat(0, 7) = -1; + dpdmat(1, 2) = -1; + dpdmat(1, 6) = 1; + dpdmat(2, 1) = 1; + dpdmat(2, 3) = -1; + + double dscaledtheta = -0.5 * theta * cos(theta) / (sin(theta) * sin(theta)) + 0.5 / sin(theta); + double dthetadcostheta = -1.0 / sqrt(-costheta * costheta + 1.0); + + Eigen::Matrix dcosthetadmat; + dcosthetadmat << 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5; + Eigen::Matrix dscaledmat = dscaledtheta * dthetadcostheta * dcosthetadmat; + + return dpdmat * scale + resnoscale * dscaledmat; +} + + + +/** +Compute the jacobian of the exponential wrt changes in the rotation vector values +@param vecr the rotation vector +@return the jacobian matrix (9*3 matrix) +*/ +inline Eigen::Matrix dexpmdr(const Eigen::Vector3d & vecr) +{ + double angle = vecr.norm(); + + if (angle < 1e-24) + { + Eigen::Matrix d_K_d_vecr; + d_K_d_vecr.fill(0); + d_K_d_vecr(1, 2) = 1; + d_K_d_vecr(2, 1) = -1; + d_K_d_vecr(3, 2) = -1; + d_K_d_vecr(5, 0) = 1; + d_K_d_vecr(6, 1) = 1; + d_K_d_vecr(7, 0) = -1; + return d_K_d_vecr; + } + + const double x = vecr(0); + const double y = vecr(1); + const double z = vecr(2); + + double angle2 = angle * angle; + double angle3 = angle2 * angle; + double sina = sin(angle); + double cosa = cos(angle); + double mcosa = 1.0 - cosa; + double c = mcosa/ angle2; + double s = sina / angle; + + double d_s_d_angle = cosa/angle - sina/angle2; + double d_c_d_angle = sina/angle2 - 2.0*mcosa/angle3; + + double d_angle_d_x = x / angle; + double d_angle_d_y = y / angle; + double d_angle_d_z = z / angle; + + //Jacobian of s*K + c*K*K + Eigen::Matrix J; + J(0,0)=0; + J(1,0)=c*y; + J(2,0)=c*z; + J(3,0)=c*y; + J(4,0)=-2*c*x; + J(5,0)=s; + J(6,0)=c*z; + J(7,0)=-s; + J(8,0)=-2*c*x; + J(0,1)=-2*c*y; + J(1,1)=c*x; + J(2,1)=-s; + J(3,1)=c*x; + J(4,1)=0; + J(5,1)=c*z; + J(6,1)=s; + J(7,1)=c*z; + J(8,1)=-2*c*y; + J(0,2)=-2*c*z; + J(1,2)=s; + J(2,2)=c*x; + J(3,2)=-s; + J(4,2)=-2*c*z; + J(5,2)=c*y; + J(6,2)=c*x; + J(7,2)=c*y; + J(8,2)=0; + J(0,3)=0; + J(1,3)=z; + J(2,3)=-y; + J(3,3)=-z; + J(4,3)=0; + J(5,3)=x; + J(6,3)=y; + J(7,3)=-x; + J(8,3)=0; + J(0,4)=-y*y - z*z; + J(1,4)=x*y; + J(2,4)=x*z; + J(3,4)=x*y; + J(4,4)=-x*x - z*z; + J(5,4)=y*z; + J(6,4)=x*z; + J(7,4)=y*z; + J(8,4)=-x*x - y*y; + + //Jacobian of [x y z s c] wrt [x y z] + Eigen::Matrix M = Eigen::Matrix::Zero(); + M(0, 0) = 1.0; + M(1, 1) = 1.0; + M(2, 2) = 1.0; + M(3, 0) = d_s_d_angle * d_angle_d_x; + M(3, 1) = d_s_d_angle * d_angle_d_y; + M(3, 2) = d_s_d_angle * d_angle_d_z; + M(4, 0) = d_c_d_angle * d_angle_d_x; + M(4, 1) = d_c_d_angle * d_angle_d_y; + M(4, 2) = d_c_d_angle * d_angle_d_z; + + return J * M; +} + } // namespace SO3 namespace SE3 { diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp index 9c33b4047b..d09a8582e8 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -35,47 +36,6 @@ namespace sfm { using namespace aliceVision::camera; using namespace aliceVision::geometry; -void BundleAdjustmentSymbolicCeres::addPose(const sfmData::CameraPose& cameraPose, - bool isConstant, - SE3::Matrix& poseBlock, - ceres::Problem& problem, - bool refineTranslation, - bool refineRotation) -{ - const Mat3& R = cameraPose.getTransform().rotation(); - const Vec3& t = cameraPose.getTransform().translation(); - - poseBlock = SE3::Matrix::Identity(); - poseBlock.block<3, 3>(0, 0) = R; - poseBlock.block<3, 1>(0, 3) = t; - double* poseBlockPtr = poseBlock.data(); - problem.AddParameterBlock(poseBlockPtr, 16); - - // add pose parameter to the all parameters blocks pointers list - _allParametersBlocks.push_back(poseBlockPtr); - - // keep the camera extrinsics constants - if (cameraPose.isLocked() || isConstant || (!refineTranslation && !refineRotation)) - { - // set the whole parameter block as constant. - _statistics.addState(EParameter::POSE, EEstimatorParameterState::CONSTANT); - - problem.SetParameterBlockConstant(poseBlockPtr); - return; - } - - if (refineRotation || refineTranslation) - { - problem.SetManifold(poseBlockPtr, new SE3ManifoldLeft(refineRotation, refineTranslation)); - } - else - { - ALICEVISION_THROW_ERROR("BundleAdjustmentSymbolicCeres: Constant extrinsics not supported at this time"); - } - - _statistics.addState(EParameter::POSE, EEstimatorParameterState::REFINED); -} - void BundleAdjustmentSymbolicCeres::CeresOptions::setDenseBA() { // default configuration use a DENSE representation @@ -258,12 +218,69 @@ void BundleAdjustmentSymbolicCeres::setSolverOptions(ceres::Solver::Options& sol } void BundleAdjustmentSymbolicCeres::addExtrinsicsToProblem(const sfmData::SfMData& sfmData, - BundleAdjustment::ERefineOptions refineOptions, - ceres::Problem& problem) + BundleAdjustment::ERefineOptions refineOptions, + ceres::Problem& problem) { const bool refineTranslation = refineOptions & BundleAdjustment::REFINE_TRANSLATION; const bool refineRotation = refineOptions & BundleAdjustment::REFINE_ROTATION; + const auto addPose = [&](const sfmData::CameraPose& cameraPose, bool isConstant, std::array& poseBlock) { + const Mat3& R = cameraPose.getTransform().rotation(); + const Vec3& t = cameraPose.getTransform().translation(); + + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis(static_cast(R.data()), angleAxis); + poseBlock.at(0) = angleAxis[0]; + poseBlock.at(1) = angleAxis[1]; + poseBlock.at(2) = angleAxis[2]; + poseBlock.at(3) = t(0); + poseBlock.at(4) = t(1); + poseBlock.at(5) = t(2); + + double* poseBlockPtr = poseBlock.data(); + problem.AddParameterBlock(poseBlockPtr, 6); + + // add pose parameter to the all parameters blocks pointers list + _allParametersBlocks.push_back(poseBlockPtr); + + // keep the camera extrinsics constants + if (cameraPose.isLocked() || isConstant || (!refineTranslation && !refineRotation)) + { + // set the whole parameter block as constant. + _statistics.addState(EParameter::POSE, EEstimatorParameterState::CONSTANT); + problem.SetParameterBlockConstant(poseBlockPtr); + return; + } + + // constant parameters + std::vector constantExtrinsic; + + // don't refine rotations + if (!refineRotation) + { + constantExtrinsic.push_back(0); + constantExtrinsic.push_back(1); + constantExtrinsic.push_back(2); + } + + // don't refine translations + if (!refineTranslation) + { + constantExtrinsic.push_back(3); + constantExtrinsic.push_back(4); + constantExtrinsic.push_back(5); + } + + // subset parametrization + if (!constantExtrinsic.empty()) + { + auto* subsetManifold = new ceres::SubsetManifold(6, constantExtrinsic); + problem.SetManifold(poseBlockPtr, subsetManifold); + } + + _statistics.addState(EParameter::POSE, EEstimatorParameterState::REFINED); + }; + // setup poses data for (const auto& posePair : sfmData.getPoses()) { @@ -279,7 +296,7 @@ void BundleAdjustmentSymbolicCeres::addExtrinsicsToProblem(const sfmData::SfMDat const bool isConstant = (pose.getState() == EEstimatorParameterState::CONSTANT); - addPose(pose, isConstant, _posesBlocks[poseId], problem, refineTranslation, refineRotation); + addPose(pose, isConstant, _posesBlocks[poseId]); } // setup sub-poses data @@ -298,12 +315,9 @@ void BundleAdjustmentSymbolicCeres::addExtrinsicsToProblem(const sfmData::SfMDat const bool isConstant = (rigSubPose.status == sfmData::ERigSubPoseStatus::CONSTANT); - addPose(sfmData::CameraPose(rigSubPose.pose), isConstant, _rigBlocks[rigId][subPoseId], problem, refineTranslation, refineRotation); + addPose(sfmData::CameraPose(rigSubPose.pose), isConstant, _rigBlocks[rigId][subPoseId]); } } - - // Add default rig pose - addPose(sfmData::CameraPose(), true, _rigNull, problem, refineTranslation, refineRotation); } void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmData, @@ -391,7 +405,7 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat { std::shared_ptr intrinsicScaleOffset = std::dynamic_pointer_cast(intrinsicPtr); - if (intrinsicScaleOffset->getInitialScale().x() > 0 && intrinsicScaleOffset->getInitialScale().y() > 0 && _ceresOptions.useFocalPrior) + if (intrinsicScaleOffset->getInitialScale().x() > 0 && intrinsicScaleOffset->getInitialScale().y() > 0) { // if we have an initial guess, we only authorize a margin around this value. assert(intrinsicBlock.size() >= 1); @@ -479,7 +493,6 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) { const bool refineStructure = refineOptions & REFINE_STRUCTURE; - const bool refineStructureAsNormal = refineOptions & REFINE_STRUCTURE_AS_NORMALS; // set a LossFunction to be less penalized by false measurements. // note: set it to NULL if you don't want use a lossFunction. @@ -505,10 +518,7 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData double* landmarkBlockPtr = landmarkBlock.data(); problem.AddParameterBlock(landmarkBlockPtr, 3); - if (refineStructureAsNormal) - { - problem.SetManifold(landmarkBlockPtr, new SO3Vec); - } + // add landmark parameter to the all parameters blocks pointers list _allParametersBlocks.push_back(landmarkBlockPtr); @@ -523,10 +533,7 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData const std::shared_ptr intrinsic = _intrinsicObjects[view.getIntrinsicId()]; const auto& pose = sfmData.getPose(view); - // each residual block takes a point and a camera as input and outputs a 2 - // dimensional residual. Internally, the cost function stores the observed - // image location and compares the reprojection against the observation. - + assert(pose.getState() != EEstimatorParameterState::IGNORED); assert(intrinsic->getState() != EEstimatorParameterState::IGNORED); @@ -534,17 +541,6 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData double* poseBlockPtr = _posesBlocks.at(view.getPoseId()).data(); double* intrinsicBlockPtr = _intrinsicsBlocks.at(view.getIntrinsicId()).data(); - bool withRig = (view.isPartOfRig() && !view.isPoseIndependant()); - double* rigBlockPtr = nullptr; - if (withRig) - { - rigBlockPtr = _rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data(); - } - else - { - rigBlockPtr = _rigNull.data(); - } - // apply a specific parameter ordering: if (_ceresOptions.useParametersOrdering) { @@ -553,15 +549,24 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData _linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2); } - if (withRig) + if (view.isPartOfRig() && !view.isPoseIndependant()) { - ceres::CostFunction* costFunction = new CostProjection(observation, intrinsic, withRig); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr, rigBlockPtr, intrinsicBlockPtr, landmarkBlockPtr); + ceres::CostFunction* costFunction = new CostProjectionSimple(observation, intrinsic); + + double* rigBlockPtr = _rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data(); + _linearSolverOrdering.AddElementToGroup(rigBlockPtr, 1); + + problem.AddResidualBlock(costFunction, + lossFunction, + poseBlockPtr, + rigBlockPtr, // subpose of the cameras rig + intrinsicBlockPtr, + landmarkBlockPtr); // do we need to copy 3D point to avoid false motion, if failure ? } else { ceres::CostFunction* costFunction = new CostProjectionSimple(observation, intrinsic); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr, intrinsicBlockPtr, landmarkBlockPtr); + problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr, intrinsicBlockPtr, landmarkBlockPtr); } if (!refineStructure || landmark.state == EEstimatorParameterState::CONSTANT) @@ -712,14 +717,16 @@ void BundleAdjustmentSymbolicCeres::updateFromSolution(sfmData::SfMData& sfmData // do not update a camera pose set as Ignored or Constant in the Local strategy if (posePair.second.getState() != EEstimatorParameterState::REFINED) - { continue; - } - const SE3::Matrix& poseBlock = _posesBlocks.at(poseId); + const std::array& poseBlock = _posesBlocks.at(poseId); + + Mat3 R_refined; + ceres::AngleAxisToRotationMatrix(poseBlock.data(), R_refined.data()); + const Vec3 t_refined(poseBlock.at(3), poseBlock.at(4), poseBlock.at(5)); // update the pose - posePair.second.setTransform(poseFromRT(poseBlock.block<3, 3>(0, 0), poseBlock.block<3, 1>(0, 3))); + posePair.second.setTransform(poseFromRT(R_refined, t_refined)); } // rig sub-poses @@ -730,10 +737,14 @@ void BundleAdjustmentSymbolicCeres::updateFromSolution(sfmData::SfMData& sfmData for (const auto& subPoseit : rigIt.second) { sfmData::RigSubPose& subPose = rig.getSubPose(subPoseit.first); - const SE3::Matrix& subPoseBlock = subPoseit.second; + const std::array& subPoseBlock = subPoseit.second; + + Mat3 R_refined; + ceres::AngleAxisToRotationMatrix(subPoseBlock.data(), R_refined.data()); + const Vec3 t_refined(subPoseBlock.at(3), subPoseBlock.at(4), subPoseBlock.at(5)); // update the sub-pose - subPose.pose = poseFromRT(subPoseBlock.block<3, 3>(0, 0), subPoseBlock.block<3, 1>(0, 3)); + subPose.pose = poseFromRT(R_refined, t_refined); } } } diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp index 2ab2be0f86..30945687fa 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp @@ -61,7 +61,6 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment, ceres::Evaluation bool useParametersOrdering = true; bool summary = false; bool verbose = true; - bool useFocalPrior = true; }; /** @@ -150,12 +149,6 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment, ceres::Evaluation virtual void PrepareForEvaluation(bool evaluate_jacobians, bool new_evaluation_point); private: - void addPose(const sfmData::CameraPose& cameraPose, - bool isConstant, - SE3::Matrix& poseBlock, - ceres::Problem& problem, - bool refineTranslation, - bool refineRotation); /** * @brief Clear structures for a new problem @@ -252,7 +245,7 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment, ceres::Evaluation std::vector _allParametersBlocks; /// poses blocks wrapper /// block: ceres angleAxis(3) + translation(3) - std::map _posesBlocks; // TODO : maybe we can use boost::flat_map instead of std::map ? + std::map> _posesBlocks; // TODO : maybe we can use boost::flat_map instead of std::map ? /// intrinsics blocks wrapper /// block: intrinsics params std::map> _intrinsicsBlocks; @@ -262,7 +255,7 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment, ceres::Evaluation std::map> _landmarksBlocks; /// rig sub-poses blocks wrapper /// block: ceres angleAxis(3) + translation(3) - std::map> _rigBlocks; + std::map>> _rigBlocks; /// Rig pose to use when there is no rig SE3::Matrix _rigNull = SE3::Matrix::Identity(); diff --git a/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp b/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp index 5f1ce7232e..202da9a005 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp @@ -11,7 +11,7 @@ namespace aliceVision { namespace sfm { -class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 16, 16, 7> +class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 6, 6, 7> { public: CostPanoramaEquidistant(Vec2 fi, Vec2 fj, std::shared_ptr& intrinsic) @@ -29,11 +29,11 @@ class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 16, 16, 7> const double* parameter_pose_j = parameters[1]; const double* parameter_intrinsics = parameters[2]; - const Eigen::Map> iTo(parameter_pose_i); - const Eigen::Map> jTo(parameter_pose_j); + const Eigen::Map iro(parameter_pose_i); + const Eigen::Map jro(parameter_pose_j); - Eigen::Matrix iRo = iTo.block<3, 3>(0, 0); - Eigen::Matrix jRo = jTo.block<3, 3>(0, 0); + Eigen::Matrix iRo = SO3::expm(iro); + Eigen::Matrix jRo = SO3::expm(jro); _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); @@ -60,30 +60,26 @@ class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 16, 16, 7> if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); - Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * + Eigen::Matrix J3 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); + SO3::dexpmdr(iro); J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); + J.block<2, 3>(0, 0) = J3.block<2, 3>(0, 0); } if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); - Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * + Eigen::Matrix J3 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); + SO3::dexpmdr(jro); J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); + J.block<2, 3>(0, 0) = J3.block<2, 3>(0, 0); } if (jacobians[2] != nullptr) diff --git a/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp b/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp index 65b494f864..848b1b5f33 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp @@ -21,8 +21,8 @@ class CostPanoramaPinHole : public ceres::CostFunction { set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(6); + mutable_parameter_block_sizes()->push_back(6); mutable_parameter_block_sizes()->push_back(intrinsic->getParams().size()); } @@ -35,11 +35,11 @@ class CostPanoramaPinHole : public ceres::CostFunction const double* parameter_pose_j = parameters[1]; const double* parameter_intrinsics = parameters[2]; - const Eigen::Map> iTo(parameter_pose_i); - const Eigen::Map> jTo(parameter_pose_j); + const Eigen::Map iro(parameter_pose_i); + const Eigen::Map jro(parameter_pose_j); - Eigen::Matrix iRo = iTo.block<3, 3>(0, 0); - Eigen::Matrix jRo = jTo.block<3, 3>(0, 0); + Eigen::Matrix iRo = SO3::expm(iro); + Eigen::Matrix jRo = SO3::expm(jro); _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); @@ -70,30 +70,26 @@ class CostPanoramaPinHole : public ceres::CostFunction if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); - Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * + Eigen::Matrix J3 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); + SO3::dexpmdr(iro); J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); + J.block<2, 3>(0, 0) = J3.block<2, 3>(0, 0); } if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); - Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * + Eigen::Matrix J3 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); + SO3::dexpmdr(jro); J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); + J.block<2, 3>(0, 0) = J3.block<2, 3>(0, 0); } if (jacobians[2] != nullptr) diff --git a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp index 5048fd3dd3..01f539a54e 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp @@ -14,15 +14,14 @@ namespace sfm { class CostProjection : public ceres::CostFunction { public: - CostProjection(const sfmData::Observation& measured, const std::shared_ptr& intrinsics, bool withRig) + CostProjection(const sfmData::Observation& measured, const std::shared_ptr& intrinsics) : _measured(measured), - _intrinsics(intrinsics), - _withRig(withRig) + _intrinsics(intrinsics) { set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(6); + mutable_parameter_block_sizes()->push_back(6); mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); mutable_parameter_block_sizes()->push_back(3); } @@ -34,10 +33,20 @@ class CostProjection : public ceres::CostFunction const double* parameter_intrinsics = parameters[2]; const double* parameter_landmark = parameters[3]; - const Eigen::Map rTo(parameter_pose); - const Eigen::Map cTr(parameter_rig); + const Eigen::Map rro(parameter_pose); + const Eigen::Map rto(parameter_pose + 3); + const Eigen::Map crr(parameter_rig); + const Eigen::Map ctr(parameter_rig + 3); const Eigen::Map pt(parameter_landmark); + Eigen::Matrix4d rTo = Eigen::Matrix4d::Identity(); + rTo.block<3, 3>(0, 0) = SO3::expm(rro); + rTo.block<3, 1>(0, 3) = rto; + + Eigen::Matrix4d cTr = Eigen::Matrix4d::Identity(); + cTr.block<3, 3>(0, 0) = SO3::expm(crr); + cTr.block<3, 1>(0, 3) = ctr; + /*Update intrinsics object with estimated parameters*/ size_t params_size = _intrinsics->getParams().size(); std::vector params; @@ -67,18 +76,37 @@ class CostProjection : public ceres::CostFunction if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); + + + Eigen::Matrix Jpose; + Jpose = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_B<4, 4, 4>(cTr, rTo); + + Eigen::Matrix Jbuf = SO3::dexpmdr(rro); - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_B<4, 4, 4>(cTr, rTo) * - getJacobian_AB_wrt_A<4, 4, 4>(Eigen::Matrix4d::Identity(), rTo); + Eigen::Matrix Jalg = Eigen::Matrix::Zero(); + Jalg.block<3, 3>(0, 0) = Jbuf.block<3, 3>(0, 0); + Jalg.block<3, 3>(4, 0) = Jbuf.block<3, 3>(3, 0); + Jalg.block<3, 3>(8, 0) = Jbuf.block<3, 3>(6, 0); + Jalg.block<3, 3>(12, 3).setIdentity(); + + J = Jpose * Jalg; } if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); + + Eigen::Matrix Jpose; + Jpose = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_A<4, 4, 4>(cTr, rTo); + + Eigen::Matrix Jbuf = SO3::dexpmdr(crr); - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_A<4, 4, 4>(cTr, rTo) * - getJacobian_AB_wrt_A<4, 4, 4>(Eigen::Matrix4d::Identity(), cTr); + Eigen::Matrix Jalg = Eigen::Matrix::Zero(); + Jalg.block<3, 3>(0, 0) = Jbuf.block<3, 3>(0, 0); + Jalg.block<3, 3>(4, 0) = Jbuf.block<3, 3>(3, 0); + Jalg.block<3, 3>(8, 0) = Jbuf.block<3, 3>(6, 0); + Jalg.block<3, 3>(12, 3).setIdentity(); } if (jacobians[2] != nullptr) @@ -101,7 +129,6 @@ class CostProjection : public ceres::CostFunction private: const sfmData::Observation& _measured; const std::shared_ptr _intrinsics; - bool _withRig; }; } // namespace sfm diff --git a/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp b/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp index 9f20715d11..13d9abd33b 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp @@ -20,7 +20,7 @@ class CostProjectionSimple : public ceres::CostFunction { set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(6); mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); mutable_parameter_block_sizes()->push_back(3); } @@ -31,9 +31,15 @@ class CostProjectionSimple : public ceres::CostFunction const double* parameter_intrinsics = parameters[1]; const double* parameter_landmark = parameters[2]; - const Eigen::Map T(parameter_pose); + const Eigen::Map vecR(parameter_pose); + const Eigen::Map vecT(parameter_pose + 3); const Eigen::Map pt(parameter_landmark); + + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.block<3, 3>(0, 0) = SO3::expm(vecR); + T.block<3, 1>(0, 3) = vecT; + const Vec4 pth = pt.homogeneous(); const Vec4 cpt = T * pth; @@ -56,9 +62,20 @@ class CostProjectionSimple : public ceres::CostFunction if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); + + Eigen::Matrix Jpose; + Jpose = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth); + + Eigen::Matrix Jbuf = SO3::dexpmdr(vecR); + + Eigen::Matrix Jalg = Eigen::Matrix::Zero(); + Jalg.block<3, 3>(0, 0) = Jbuf.block<3, 3>(0, 0); + Jalg.block<3, 3>(4, 0) = Jbuf.block<3, 3>(3, 0); + Jalg.block<3, 3>(8, 0) = Jbuf.block<3, 3>(6, 0); + Jalg.block<3, 3>(12, 3).setIdentity(); - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoseLeft(T, pth); + J = Jpose * Jalg; } if (jacobians[1] != nullptr) diff --git a/src/aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp b/src/aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp index 4a1d8408e9..f09a5810ed 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp @@ -11,7 +11,7 @@ namespace aliceVision { namespace sfm { -class CostRotationPrior : public ceres::SizedCostFunction<3, 16, 16> +class CostRotationPrior : public ceres::SizedCostFunction<3, 6, 6> { public: explicit CostRotationPrior(const Eigen::Matrix3d& two_R_one) @@ -23,11 +23,11 @@ class CostRotationPrior : public ceres::SizedCostFunction<3, 16, 16> const double* parameter_pose_one = parameters[0]; const double* parameter_pose_two = parameters[1]; - const Eigen::Map> oneTo(parameter_pose_one); - const Eigen::Map> twoTo(parameter_pose_two); + const Eigen::Map onero(parameter_pose_one); + const Eigen::Map tworo(parameter_pose_two); - Eigen::Matrix oneRo = oneTo.block<3, 3>(0, 0); - Eigen::Matrix twoRo = twoTo.block<3, 3>(0, 0); + Eigen::Matrix oneRo = SO3::expm(onero); + Eigen::Matrix twoRo = SO3::expm(tworo); Eigen::Matrix3d two_R_one_est = twoRo * oneRo.transpose(); Eigen::Matrix3d error_R = two_R_one_est * _two_R_one.transpose(); @@ -44,30 +44,26 @@ class CostRotationPrior : public ceres::SizedCostFunction<3, 16, 16> if (jacobians[0]) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); - Eigen::Matrix J9 = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * + Eigen::Matrix J3 = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_B<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), oneRo); + SO3::dexpmdr(onero); J.fill(0); - J.block<3, 3>(0, 0) = J9.block<3, 3>(0, 0); - J.block<3, 3>(0, 4) = J9.block<3, 3>(0, 3); - J.block<3, 3>(0, 8) = J9.block<3, 3>(0, 6); + J.block<3, 3>(0, 0) = J3.block<3, 3>(0, 0); } if (jacobians[1]) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); - Eigen::Matrix J9 = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * + Eigen::Matrix J3 = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(twoRo, oneRo.transpose()) * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), twoRo); + SO3::dexpmdr(tworo); J.fill(0); - J.block<3, 3>(0, 0) = J9.block<3, 3>(0, 0); - J.block<3, 3>(0, 4) = J9.block<3, 3>(0, 3); - J.block<3, 3>(0, 8) = J9.block<3, 3>(0, 6); + J.block<3, 3>(0, 0) = J3.block<3, 3>(0, 0); } return true; diff --git a/src/aliceVision/sfm/bundle/manifolds/so3.hpp b/src/aliceVision/sfm/bundle/manifolds/so3.hpp index 009b806f13..c2a686c413 100644 --- a/src/aliceVision/sfm/bundle/manifolds/so3.hpp +++ b/src/aliceVision/sfm/bundle/manifolds/so3.hpp @@ -12,68 +12,6 @@ #include namespace aliceVision { -namespace SO3 { - -/** -Compute the jacobian of the logarithm wrt changes in the rotation matrix values -@param R the input rotation matrix -@return the jacobian matrix (3*9 matrix) -*/ -inline Eigen::Matrix dlogmdr(const Eigen::Matrix3d& R) -{ - double p1 = R(2, 1) - R(1, 2); - double p2 = R(0, 2) - R(2, 0); - double p3 = R(1, 0) - R(0, 1); - - double costheta = (R.trace() - 1.0) / 2.0; - if (costheta > 1.0) - costheta = 1.0; - else if (costheta < -1.0) - costheta = -1.0; - - double theta = acos(costheta); - - if (fabs(theta) < std::numeric_limits::epsilon()) - { - Eigen::Matrix J; - J.fill(0); - J(0, 5) = 1; - J(0, 7) = -1; - J(1, 2) = -1; - J(1, 6) = 1; - J(2, 1) = 1; - J(2, 3) = -1; - return J; - } - - double scale = theta / (2.0 * sin(theta)); - - Eigen::Vector3d resnoscale; - resnoscale(0) = p1; - resnoscale(1) = p2; - resnoscale(2) = p3; - - Eigen::Matrix dresdp = Eigen::Matrix3d::Identity() * scale; - Eigen::Matrix dpdmat; - dpdmat.fill(0); - dpdmat(0, 5) = 1; - dpdmat(0, 7) = -1; - dpdmat(1, 2) = -1; - dpdmat(1, 6) = 1; - dpdmat(2, 1) = 1; - dpdmat(2, 3) = -1; - - double dscaledtheta = -0.5 * theta * cos(theta) / (sin(theta) * sin(theta)) + 0.5 / sin(theta); - double dthetadcostheta = -1.0 / sqrt(-costheta * costheta + 1.0); - - Eigen::Matrix dcosthetadmat; - dcosthetadmat << 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5; - Eigen::Matrix dscaledmat = dscaledtheta * dthetadcostheta * dcosthetadmat; - - return dpdmat * scale + resnoscale * dscaledmat; -} -} // namespace SO3 - namespace sfm { class SO3Manifold : public ceres::Manifold