├── examples ├── image.jpg └── keypoints.json ├── image └── frontier.png ├── C++ ├── scope │ ├── math │ │ ├── operation.h │ │ └── macro.h │ ├── factor │ │ ├── ExtraPinholeCamera.cpp │ │ ├── ExtraPinholeCamera.h │ │ ├── Factors.h │ │ ├── Factor.cpp │ │ ├── UnitPOFFactor.h │ │ ├── ParameterFactor.h │ │ ├── POFFactor.h │ │ ├── EulerAngleConstFactor.h │ │ ├── DepthCameraFactor.h │ │ ├── PoseFactor.h │ │ ├── JointFactor.h │ │ ├── PinholeCameraFactor.h │ │ ├── JointDepthCameraFactor.h │ │ ├── JointPinholeCameraFactor.h │ │ ├── PinholeCameraFactor.cpp │ │ ├── DepthCameraFactor.cpp │ │ ├── POFFactor.cpp │ │ ├── PoseConstFactor.h │ │ ├── JointConstFactor.h │ │ ├── FullJointPinholeCameraFactor.h │ │ ├── ScaledPOFFactor.h │ │ ├── FullJointExtraPinholeCameraFactor.h │ │ ├── JointExtraPinholeCameraFactor.h │ │ ├── VertexDepthCameraFactor.h │ │ ├── VertexPinholeCameraFactor.h │ │ ├── RelPOFFactor.h │ │ ├── FullVertexPinholeCameraFactor.h │ │ ├── ParameterFactor.cpp │ │ ├── FullVertexExtraPinholeCameraFactor.h │ │ ├── VertexExtraPinholeCameraFactor.h │ │ ├── JointLimitFactor.h │ │ ├── JointFactor.cpp │ │ ├── PoseFactor.cpp │ │ ├── UnitPOFFactor.cpp │ │ ├── Factor.h │ │ ├── EulerAngleConstFactor.cpp │ │ ├── JointConstFactor.cpp │ │ ├── JointDepthCameraFactor.cpp │ │ ├── JointExtraPinholeCameraFactor.cpp │ │ ├── JointPinholeCameraFactor.cpp │ │ ├── PoseConstFactor.cpp │ │ ├── FullJointPinholeCameraFactor.cpp │ │ └── FullJointExtraPinholeCameraFactor.cpp │ ├── initializer │ │ ├── Initializers.h │ │ ├── factor │ │ │ ├── Factors.h │ │ │ ├── Factor.cpp │ │ │ ├── EulerAngleConstFactor.h │ │ │ ├── PoseFactor.h │ │ │ ├── JointFactor.h │ │ │ ├── POFFactor.h │ │ │ ├── PoseConstFactor.h │ │ │ ├── JointConstFactor.h │ │ │ ├── DepthCameraFactor.h │ │ │ ├── PinholeCameraFactor.h │ │ │ ├── JointLimitFactor.h │ │ │ ├── Factor.h │ │ │ ├── JointFactor.cpp │ │ │ ├── PoseFactor.cpp │ │ │ ├── POFFactor.cpp │ │ │ ├── JointConstFactor.cpp │ │ │ ├── EulerAngleConstFactor.cpp │ │ │ ├── DepthCameraFactor.cpp │ │ │ ├── PinholeCameraFactor.cpp │ │ │ └── PoseConstFactor.cpp │ │ ├── HeadInitializer.cpp │ │ ├── TorsoInitializer.cpp │ │ ├── HeadInitializer.h │ │ ├── TorsoInitializer.h │ │ ├── ArmRefiner.h │ │ ├── ArmInitializer.h │ │ ├── LegInitializer.h │ │ ├── ExtTorsoInitializer.h │ │ ├── ArmRefiner.cpp │ │ ├── ExtTorsoInitializer.cpp │ │ └── ArmInitializer.cpp │ ├── model │ │ ├── SMPL.h │ │ ├── Link.h │ │ ├── SMPL.cpp │ │ └── Model.h │ ├── utils │ │ ├── Stopwatch.h │ │ ├── Initialization.h │ │ └── JointConstInfo.h │ ├── CMakeLists.txt │ ├── base │ │ ├── Types.h │ │ └── Pose.h │ └── optimizer │ │ └── SMPLify.h ├── examples │ └── CMakeLists.txt ├── cnpy │ └── CMakeLists.txt └── CMakeLists.txt ├── LICENSE ├── model └── preprocess.py └── README.md /examples/image.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fantaosha/SCOPE/HEAD/examples/image.jpg -------------------------------------------------------------------------------- /image/frontier.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fantaosha/SCOPE/HEAD/image/frontier.png -------------------------------------------------------------------------------- /C++/scope/math/operation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace scope { 4 | namespace math { 5 | enum class OPS { EQL, ADD, SUB }; 6 | } 7 | } // namespace scope 8 | -------------------------------------------------------------------------------- /C++/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Boost REQUIRED COMPONENTS program_options filesystem) 2 | include_directories(${Boost_INCLUDE_DIRS}) 3 | 4 | add_executable(run ./run.cpp) 5 | target_link_libraries(run cnpy SCOPE ${Boost_LIBRARIES}) 6 | -------------------------------------------------------------------------------- /C++/cnpy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ZLIB REQUIRED) 2 | include_directories(${ZLIB_LIBRARIES}) 3 | 4 | add_library(cnpy SHARED cnpy.cpp) 5 | target_link_libraries(cnpy ${ZLIB_LIBRARIES}) 6 | 7 | export(PACKAGE cnpy) 8 | export(TARGETS cnpy FILE cnpyConfig.cmake) 9 | -------------------------------------------------------------------------------- /C++/scope/factor/ExtraPinholeCamera.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace scope { 4 | ExtraPinholeCamera::ExtraPinholeCamera(const Pose &T) : mExtraCamPose(T) {} 5 | 6 | const Pose &ExtraPinholeCamera::getExtraCameraPose() const { 7 | return mExtraCamPose; 8 | } 9 | 10 | } // namespace scope 11 | -------------------------------------------------------------------------------- /C++/scope/initializer/Initializers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | -------------------------------------------------------------------------------- /C++/scope/math/macro.h: -------------------------------------------------------------------------------- 1 | #ifndef _MACRO_H 2 | #define _MACRO_H 3 | 4 | #define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS(TYPE, ROWS) \ 5 | EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS, \ 6 | "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_SPECIFIC_ROWS") 7 | 8 | #define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_COLS(TYPE, COLS) \ 9 | EIGEN_STATIC_ASSERT(TYPE::ColsAtCompileTime == COLS, \ 10 | "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_SPECIFIC_COLS") 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/Factors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | -------------------------------------------------------------------------------- /C++/scope/model/SMPL.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | template 7 | class SMPL : public Model<23, P, 6890> { 8 | public: 9 | const static std::vector mvLeftLeg, mvRightLeg, mvLeftArm, mvRightArm, 10 | mvHead, mvBack; 11 | 12 | SMPL(const MatrixX& RelJDirs, const VectorX& RelJ, 13 | const MatrixX& VDirs, const VectorX& V); 14 | 15 | virtual const std::vector & getKinematicsTree() const override; 16 | 17 | protected: 18 | virtual int setKinematicsTree() override; 19 | }; 20 | 21 | extern template class SMPL<10>; 22 | extern template class SMPL<11>; 23 | } // namespace scope 24 | -------------------------------------------------------------------------------- /C++/scope/factor/ExtraPinholeCamera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | class ExtraPinholeCamera { 8 | public: 9 | struct Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | // position in the extra camera frame 13 | Vector3 pExtraCam; 14 | }; 15 | 16 | struct Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Matrix<2, 3> JPoint; 20 | }; 21 | 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | ExtraPinholeCamera(const Pose &T); 26 | 27 | const Pose &getExtraCameraPose() const; 28 | 29 | protected: 30 | // rigid body transformation from the main camera frame to the extra camera 31 | // frame 32 | Pose mExtraCamPose; 33 | }; 34 | } // namespace scope 35 | -------------------------------------------------------------------------------- /C++/scope/model/Link.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace scope { 8 | class Link { 9 | public: 10 | std::string mName; 11 | 12 | int mId; 13 | int mParent; 14 | int mJoint; 15 | 16 | std::vector mvChildren; 17 | 18 | public: 19 | Link() : mName(""), mId(-1), mParent(-1), mJoint(-1){}; 20 | 21 | const std::string& name() const { return mName; }; 22 | 23 | const int id() const { return mId; }; 24 | 25 | const int parent() const { return mParent; } 26 | 27 | const int joint() const { return mJoint; } 28 | 29 | const std::vector& children() const { return mvChildren; } 30 | }; 31 | 32 | using LinkPtr = std::shared_ptr; 33 | using LinkConstPtr = std::shared_ptr; 34 | } // namespace scope 35 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/Factor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace scope { 4 | namespace Initializer { 5 | Factor::Evaluation::Evaluation() : status(Status::INVALID) {} 6 | 7 | int Factor::Evaluation::reset() { 8 | status = Status::INVALID; 9 | 10 | return 0; 11 | } 12 | 13 | int Factor::Evaluation::clear() { 14 | reset(); 15 | 16 | error.resize(0); 17 | 18 | return 0; 19 | } 20 | 21 | Factor::Linearization::Linearization() : status(Status::INVALID) {} 22 | 23 | int Factor::Linearization::reset() { 24 | status = Status::INVALID; 25 | 26 | return 0; 27 | } 28 | 29 | int Factor::Linearization::clear() { 30 | reset(); 31 | 32 | return 0; 33 | } 34 | 35 | Factor::Factor(const std::string &name, int index) 36 | : mIndex(index), mName(name) {} 37 | } // namespace Initializer 38 | } // namespace scope 39 | -------------------------------------------------------------------------------- /C++/scope/initializer/HeadInitializer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | HeadInitializer::HeadInitializer(const Options &options) 9 | : Initializer(0, options) {} 10 | 11 | int HeadInitializer::updateGaussNewton() const { 12 | mH = mvMxx[0]; 13 | mh = mvmx[0]; 14 | 15 | return 0; 16 | } 17 | 18 | int HeadInitializer::update(Scalar stepsize) const { 19 | assert(stepsize > 0); 20 | 21 | mDRootPoseChange = mhGN * stepsize; 22 | Pose::exp(mDRootPoseChange, mRootPoseChange); 23 | 24 | mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R; 25 | mvPoses[1][0].t = mRootPoseChange.t; 26 | mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t; 27 | 28 | return 0; 29 | } 30 | 31 | } // namespace Initializer 32 | } // namespace scope 33 | -------------------------------------------------------------------------------- /C++/scope/initializer/TorsoInitializer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | TorsoInitializer::TorsoInitializer(const Options &options) 9 | : Initializer(0, options) {} 10 | 11 | int TorsoInitializer::updateGaussNewton() const { 12 | mH = mvMxx[0]; 13 | mh = mvmx[0]; 14 | 15 | return 0; 16 | } 17 | 18 | int TorsoInitializer::update(Scalar stepsize) const { 19 | assert(stepsize > 0); 20 | 21 | mDRootPoseChange = mhGN * stepsize; 22 | Pose::exp(mDRootPoseChange, mRootPoseChange); 23 | 24 | mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R; 25 | mvPoses[1][0].t = mRootPoseChange.t; 26 | mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t; 27 | 28 | return 0; 29 | } 30 | 31 | } // namespace Initializer 32 | } // namespace scope 33 | -------------------------------------------------------------------------------- /C++/scope/initializer/HeadInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace Initializer { 10 | 11 | class HeadInitializer : public Initializer { 12 | protected: 13 | // intermediates used for update 14 | mutable Vector6 mDRootPoseChange; 15 | mutable Pose mRootPoseChange; 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | HeadInitializer(const Options &options); 21 | 22 | HeadInitializer(const HeadInitializer &) = delete; 23 | 24 | HeadInitializer &operator=(const HeadInitializer &) = delete; 25 | 26 | protected: 27 | virtual int FKintree(int n) const override { return 0; } 28 | virtual int DFKintree() const override { return 0; } 29 | 30 | virtual int updateGaussNewton() const override; 31 | virtual int update(Scalar stepsize) const override; 32 | }; 33 | 34 | } // namespace Initializer 35 | } // namespace scope 36 | -------------------------------------------------------------------------------- /C++/scope/initializer/TorsoInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace Initializer { 10 | 11 | class TorsoInitializer : public Initializer { 12 | protected: 13 | // intermediates used for update 14 | mutable Vector6 mDRootPoseChange; 15 | mutable Pose mRootPoseChange; 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | TorsoInitializer(const Options &options); 21 | 22 | TorsoInitializer(const TorsoInitializer &) = delete; 23 | 24 | TorsoInitializer &operator=(const TorsoInitializer &) = delete; 25 | 26 | protected: 27 | virtual int FKintree(int n) const override { return 0; } 28 | virtual int DFKintree() const override { return 0; } 29 | 30 | virtual int updateGaussNewton() const override; 31 | virtual int update(Scalar stepsize) const override; 32 | }; 33 | 34 | } // namespace Initializer 35 | } // namespace scope 36 | -------------------------------------------------------------------------------- /C++/scope/initializer/ArmRefiner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace Initializer { 10 | 11 | class ArmRefiner : public Initializer { 12 | public: 13 | std::array mRelJointLocations; 14 | 15 | mutable Vector6 mDJointChange; 16 | mutable std::array mvJointChange; 17 | 18 | mutable std::array mvB; 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | ArmRefiner(const Options &options, 24 | const std::array &RelJointLocations); 25 | 26 | ArmRefiner(const ArmRefiner &) = delete; 27 | 28 | ArmRefiner &operator=(const ArmRefiner &) = delete; 29 | 30 | public: 31 | virtual int FKintree(int n) const override; 32 | virtual int DFKintree() const override; 33 | 34 | virtual int updateGaussNewton() const override; 35 | virtual int update(Scalar stepsize) const override; 36 | }; 37 | 38 | } // namespace Initializer 39 | } // namespace scope 40 | -------------------------------------------------------------------------------- /C++/scope/factor/Factors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Taosha Fan 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /C++/scope/utils/Stopwatch.h: -------------------------------------------------------------------------------- 1 | /** This small file provides a pair of convenience functions that are useful for 2 | * measuring elapsed computation times. 3 | * 4 | * Copyright (C) 2017 - 2018 by David M. Rosen (dmrosen@mit.edu) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | namespace scope { 12 | namespace Stopwatch { 13 | 14 | /** This function returns a chrono::time_point struct encoding the time at which 15 | * it is called.*/ 16 | inline std::chrono::time_point tick() { 17 | return std::chrono::high_resolution_clock::now(); 18 | } 19 | 20 | /** When this function is called with a chrono::time_point struct returned by 21 | * tick(), it returns the elapsed time (in seconds) between the calls to tick() 22 | * and tock().*/ 23 | inline double 24 | tock(const std::chrono::time_point 25 | &tick_time) { 26 | auto counter = std::chrono::high_resolution_clock::now() - tick_time; 27 | return std::chrono::duration_cast(counter).count() / 28 | 1000000000.0; 29 | } 30 | } // namespace Stopwatch 31 | } // namespace scope 32 | -------------------------------------------------------------------------------- /C++/scope/factor/Factor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace scope { 4 | Factor::Evaluation::Evaluation() : status(Status::INVALID) {} 5 | 6 | int Factor::Evaluation::reset() { 7 | status = Status::INVALID; 8 | 9 | return 0; 10 | } 11 | 12 | int Factor::Evaluation::clear() { 13 | reset(); 14 | 15 | error.resize(0); 16 | 17 | return 0; 18 | } 19 | 20 | Factor::Linearization::Linearization() : status(Status::INVALID) {} 21 | 22 | int Factor::Linearization::reset() { 23 | status = Status::INVALID; 24 | 25 | jacobians[0].clear(); 26 | jacobians[1].clear(); 27 | jacobians[2].clear(); 28 | jacobians[3].clear(); 29 | 30 | return 0; 31 | } 32 | 33 | int Factor::Linearization::clear() { 34 | reset(); 35 | 36 | jacobians[0].clear(); 37 | jacobians[1].clear(); 38 | jacobians[2].clear(); 39 | jacobians[3].clear(); 40 | 41 | return 0; 42 | } 43 | 44 | Factor::Factor(const std::vector &poses, const std::vector &shapes, 45 | const std::vector &joints, const std::vector ¶ms, 46 | const std::string &name, int index, bool active) 47 | : mvPoses(poses), mvShapes(shapes), mvJoints(joints), mvParams(params), 48 | mIndex(index), mName(name), mActive(active) {} 49 | 50 | } // namespace scope 51 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/EulerAngleConstFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | //TODO: test evaluate() and linearize() 4 | 5 | #include 6 | 7 | namespace scope { 8 | namespace Initializer { 9 | class EulerAngleConstFactor : public JointConstFactor { 10 | public: 11 | struct Linearization : public JointConstFactor::Linearization { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | Matrix3 H; 15 | Matrix3 D; 16 | }; 17 | 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | SCOAT_INITIAL_EVALUATION_NEW 21 | SCOAT_INITIAL_LINEARIZATION_NEW 22 | 23 | EulerAngleConstFactor(int joint, const Vector3 &weight, const Matrix3 &mean, 24 | const Vector3 &lbnd, const Vector3 &ubnd, 25 | const std::string &name = "", int index = -1); 26 | 27 | virtual int evaluate(const AlignedVector &poses, 28 | const AlignedVector &joints, 29 | Factor::Evaluation &base_eval) const override; 30 | 31 | virtual int linearize(const AlignedVector &poses, 32 | const AlignedVector &joints, 33 | const Factor::Evaluation &base_eval, 34 | Factor::Linearization &base_lin) const override; 35 | }; 36 | } // namespace Initializer 37 | } // namespace scope 38 | -------------------------------------------------------------------------------- /C++/scope/factor/UnitPOFFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class UnitPOFFactor : public POFFactor { 7 | public: 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | SCOAT_FACTOR_EVALUATION_NEW 10 | SCOAT_FACTOR_LINEARIZATION_NEW 11 | 12 | UnitPOFFactor(int pose, const Vector3 &s, const Scalar &sigma, 13 | const Scalar &eps, const Vector3 &measurement, 14 | const Scalar &confidence = 1.0, const std::string &name = "", 15 | int index = -1, bool active = true); 16 | 17 | virtual int evaluate(const AlignedVector &poses, 18 | const AlignedVector &shapes, 19 | const AlignedVector &joints, 20 | const AlignedVector ¶ms, 21 | Factor::Evaluation &base_eval) const override; 22 | 23 | virtual int linearize(const AlignedVector &poses, 24 | const AlignedVector &shapes, 25 | const AlignedVector &joints, 26 | const AlignedVector ¶ms, 27 | const Factor::Evaluation &base_eval, 28 | Factor::Linearization &base_lin) const override; 29 | 30 | protected: 31 | Vector3 mS; // unit directional vector 32 | }; 33 | } // namespace scope 34 | -------------------------------------------------------------------------------- /C++/scope/factor/ParameterFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class ParameterFactor : public Factor { 7 | public: 8 | struct Evaluation : public Factor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | VectorX unscaledError; 12 | }; 13 | 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | SCOAT_FACTOR_EVALUATION_NEW 17 | SCOAT_FACTOR_LINEARIZATION_NEW 18 | 19 | ParameterFactor(int param, const MatrixX &sqrtCov, const VectorX &mean, 20 | const std::string &name = "", int index = -1, 21 | bool active = true); 22 | 23 | virtual int evaluate(const AlignedVector &poses, 24 | const AlignedVector &shapes, 25 | const AlignedVector &joints, 26 | const AlignedVector ¶ms, 27 | Factor::Evaluation &base_eval) const override; 28 | 29 | virtual int linearize(const AlignedVector &poses, 30 | const AlignedVector &shapes, 31 | const AlignedVector &joints, 32 | const AlignedVector ¶ms, 33 | const Factor::Evaluation &base_eval, 34 | Factor::Linearization &base_lin) const override; 35 | 36 | protected: 37 | MatrixX mSqrtCov; 38 | VectorX mMean; 39 | }; 40 | } // namespace scope 41 | -------------------------------------------------------------------------------- /model/preprocess.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy as np 4 | import pickle 5 | import chumpy 6 | import sys 7 | from scipy.sparse import issparse 8 | 9 | def load_data(data): 10 | v_template = np.array(data['v_template']) 11 | Jreg = data['J_regressor'] 12 | 13 | if issparse(Jreg): 14 | Jreg = Jreg.todense() 15 | 16 | shapedirs = np.array(data['shapedirs']) 17 | parent = np.array(data['kintree_table'][0, :]) 18 | 19 | RelJreg = np.vstack([Jreg[i, :] - Jreg[parent[i], :] for i in 20 | range(1, Jreg.shape[0])]) 21 | RelJreg = np.vstack((Jreg[0, :], RelJreg)) 22 | 23 | JDirs = np.stack([Jreg.dot(shapedirs[:, :, i]).ravel() 24 | for i in range(shapedirs.shape[2])]) 25 | J = Jreg.dot(v_template).ravel() 26 | 27 | RelJDirs = np.stack([RelJreg.dot(shapedirs[:, :, i]).ravel() 28 | for i in range(shapedirs.shape[2])]) 29 | RelJ = RelJreg.dot(v_template).ravel() 30 | 31 | vDirs = np.stack([shapedirs[:, :, i].ravel() 32 | for i in range(shapedirs.shape[2])]) 33 | v = v_template.ravel() 34 | 35 | return {'JDirs': JDirs, 'J': J, 'RelJDirs': RelJDirs, 'RelJ': RelJ, 'vDirs': vDirs, 'v': v} 36 | 37 | 38 | def load(file): 39 | with open(file,'rb') as f: 40 | data = pickle.load(f,encoding = 'latin1') 41 | 42 | return load_data(data) 43 | 44 | 45 | smpl = load(sys.argv[2]) 46 | np.savez(sys.argv[1],**smpl) -------------------------------------------------------------------------------- /C++/scope/initializer/factor/PoseFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class PoseFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Matrix3 errorR; 13 | Vector6 xi; 14 | }; 15 | 16 | struct Linearization : public Factor::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Matrix6 jacobian; 20 | Matrix3 dexpinv; 21 | Matrix3 dexpinvR; 22 | }; 23 | 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | SCOAT_INITIAL_EVALUATION_NEW 27 | SCOAT_INITIAL_LINEARIZATION_NEW 28 | 29 | PoseFactor(int pose, const Matrix6 &sqrtCov, const Pose &mean, 30 | const std::string &name = "", int index = -1); 31 | 32 | virtual int evaluate(const AlignedVector &poses, 33 | const AlignedVector &joints, 34 | Factor::Evaluation &base_eval) const override; 35 | 36 | virtual int linearize(const AlignedVector &poses, 37 | const AlignedVector &joints, 38 | const Factor::Evaluation &base_eval, 39 | Factor::Linearization &base_lin) const override; 40 | 41 | int getPose() const { return mPose; } 42 | 43 | protected: 44 | int mPose; 45 | 46 | Matrix6 mSqrtCov; 47 | Pose mMean; 48 | }; 49 | } // namespace Initializer 50 | } // namespace scope 51 | -------------------------------------------------------------------------------- /C++/scope/factor/POFFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ROBUST_POF 1 4 | 5 | #include 6 | 7 | namespace scope { 8 | class POFFactor : public Factor { 9 | public: 10 | struct Evaluation : public Factor::Evaluation { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | Vector3 d; // direction 14 | 15 | Scalar squaredErrorNorm; 16 | }; 17 | 18 | struct Linearization : public Factor::Linearization { 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | Scalar Drho; 22 | Scalar sqrtDrho; 23 | Scalar alpha; 24 | 25 | Vector3 scaledError; 26 | 27 | Vector6 gPose; 28 | }; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | SCOAT_FACTOR_EVALUATION_NEW 33 | SCOAT_FACTOR_LINEARIZATION_NEW 34 | 35 | POFFactor(const std::vector &poses, const std::vector &shapes, 36 | const std::vector &joints, const std::vector ¶ms, 37 | const Scalar &sigma, const Scalar &eps, const Vector3 &measurement, 38 | const Scalar &confidence, const std::string &name = "", 39 | int index = -1, bool active = true); 40 | 41 | protected: 42 | virtual int preLinearizeRobustKernel(const Evaluation &eval, 43 | Linearization &lin) const; 44 | 45 | protected: 46 | Vector3 mMeasurement; 47 | 48 | Scalar mCon; 49 | Scalar mSqrtCon; 50 | Scalar mSigmaCon; 51 | 52 | Scalar mSigma; 53 | Scalar mGMThreshold; 54 | Scalar mMaxGMScale; 55 | }; 56 | } // namespace scope 57 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/JointFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class JointFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Matrix3 errorR; 13 | Vector3 xi; 14 | }; 15 | 16 | struct Linearization : public Factor::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Matrix3 jacobian; 20 | Matrix3 dexpinv; 21 | Matrix3 dexpinvR; 22 | }; 23 | 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | SCOAT_INITIAL_EVALUATION_NEW 27 | SCOAT_INITIAL_LINEARIZATION_NEW 28 | 29 | JointFactor(int pose, const Matrix3 &sqrtCov, const Matrix3 &mean, 30 | const std::string &name = "", int index = -1); 31 | 32 | virtual int evaluate(const AlignedVector &poses, 33 | const AlignedVector &joints, 34 | Factor::Evaluation &base_eval) const override; 35 | 36 | virtual int linearize(const AlignedVector &poses, 37 | const AlignedVector &joints, 38 | const Factor::Evaluation &base_eval, 39 | Factor::Linearization &base_lin) const override; 40 | 41 | int getJoint() const { return mJoint; } 42 | 43 | protected: 44 | int mJoint; 45 | 46 | Matrix3 mSqrtCov; 47 | Matrix3 mMean; 48 | }; 49 | } // namespace Initializer 50 | } // namespace scope 51 | -------------------------------------------------------------------------------- /C++/scope/factor/EulerAngleConstFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class EulerAngleConstFactor : public JointConstFactor { 7 | public: 8 | struct Linearization : public JointConstFactor::Linearization { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Matrix3 H; 12 | Matrix3 D; 13 | }; 14 | 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | SCOAT_FACTOR_EVALUATION_NEW 18 | SCOAT_FACTOR_LINEARIZATION_NEW 19 | 20 | EulerAngleConstFactor(int joint, const Vector3 &weight, const Matrix3 &mean, 21 | const Vector3 &lbnd, const Vector3 &ubnd, 22 | const std::string &name = "", int index = -1, 23 | bool active = true); 24 | 25 | virtual int evaluate(const AlignedVector &poses, 26 | const AlignedVector &shapes, 27 | const AlignedVector &joints, 28 | const AlignedVector ¶ms, 29 | Factor::Evaluation &base_eval) const override; 30 | 31 | virtual int linearize(const AlignedVector &poses, 32 | const AlignedVector &shapes, 33 | const AlignedVector &joints, 34 | const AlignedVector ¶ms, 35 | const Factor::Evaluation &base_eval, 36 | Factor::Linearization &base_lin) const override; 37 | }; 38 | } // namespace scope 39 | -------------------------------------------------------------------------------- /C++/scope/factor/DepthCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ROBUST_DEPTH 1 4 | 5 | #include 6 | 7 | namespace scope { 8 | class DepthCameraFactor : public Factor { 9 | public: 10 | struct Evaluation : public Factor::Evaluation { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | Scalar squaredErrorNorm; 14 | }; 15 | 16 | struct Linearization : public Factor::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Scalar Drho; 20 | Scalar sqrtDrho; 21 | Scalar alpha; 22 | 23 | Vector3 scaledError; 24 | 25 | Vector6 gPose; 26 | }; 27 | 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | SCOAT_FACTOR_EVALUATION_NEW 31 | SCOAT_FACTOR_LINEARIZATION_NEW 32 | 33 | DepthCameraFactor(const std::vector &poses, 34 | const std::vector &shapes, 35 | const std::vector &joints, 36 | const std::vector ¶ms, const Scalar &sigma, 37 | const Scalar &eps, const Vector3 &measurement, 38 | const Scalar &confidence, const std::string &name = "", 39 | int index = -1, bool active = true); 40 | 41 | protected: 42 | virtual int preLinearizeRobustKernel(const Evaluation &eval, 43 | Linearization &lin) const; 44 | 45 | protected: 46 | Vector3 mMeasurement; 47 | 48 | Scalar mCon; 49 | Scalar mSqrtCon; 50 | Scalar mSigmaCon; 51 | 52 | Scalar mSigma; 53 | Scalar mGMThreshold; 54 | Scalar mMaxGMScale; 55 | }; 56 | } // namespace scope 57 | -------------------------------------------------------------------------------- /C++/scope/factor/PoseFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class PoseFactor : public Factor { 7 | public: 8 | struct Evaluation : public Factor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Matrix3 errorR; 12 | Vector6 xi; 13 | }; 14 | 15 | struct Linearization : public Factor::Linearization { 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | Matrix3 dexpinv; 19 | Matrix3 dexpinvR; 20 | }; 21 | 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | SCOAT_FACTOR_EVALUATION_NEW 25 | SCOAT_FACTOR_LINEARIZATION_NEW 26 | 27 | PoseFactor(int pose, const Matrix6 &sqrtCov, const Pose &mean, 28 | const std::string &name = "", int index = -1, bool active = true); 29 | 30 | virtual int evaluate(const AlignedVector &poses, 31 | const AlignedVector &shapes, 32 | const AlignedVector &joints, 33 | const AlignedVector ¶ms, 34 | Factor::Evaluation &base_eval) const override; 35 | 36 | virtual int linearize(const AlignedVector &poses, 37 | const AlignedVector &shapes, 38 | const AlignedVector &joints, 39 | const AlignedVector ¶ms, 40 | const Factor::Evaluation &base_eval, 41 | Factor::Linearization &base_lin) const override; 42 | 43 | protected: 44 | Matrix6 mSqrtCov; 45 | Pose mMean; 46 | }; 47 | } // namespace scope 48 | -------------------------------------------------------------------------------- /C++/scope/factor/JointFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class JointFactor : public Factor { 7 | public: 8 | struct Evaluation : public Factor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Matrix3 errorR; 12 | Vector3 xi; 13 | }; 14 | 15 | struct Linearization : public Factor::Linearization { 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | Matrix3 dexpinv; 19 | Matrix3 dexpinvR; 20 | }; 21 | 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | SCOAT_FACTOR_EVALUATION_NEW 25 | SCOAT_FACTOR_LINEARIZATION_NEW 26 | 27 | JointFactor(int joint, const Matrix3 &sqrtCov, const Matrix3 &mean, 28 | const std::string &name = "", int index = -1, bool active = true); 29 | 30 | virtual int evaluate(const AlignedVector &poses, 31 | const AlignedVector &shapes, 32 | const AlignedVector &joints, 33 | const AlignedVector ¶ms, 34 | Factor::Evaluation &base_eval) const override; 35 | 36 | virtual int linearize(const AlignedVector &poses, 37 | const AlignedVector &shapes, 38 | const AlignedVector &joints, 39 | const AlignedVector ¶ms, 40 | const Factor::Evaluation &base_eval, 41 | Factor::Linearization &base_lin) const override; 42 | 43 | protected: 44 | Matrix3 mSqrtCov; 45 | Matrix3 mMean; 46 | }; 47 | } // namespace scope 48 | -------------------------------------------------------------------------------- /C++/scope/factor/PinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ROBUST_PINHOLE 1 4 | 5 | #include 6 | 7 | namespace scope { 8 | class PinholeCameraFactor : public Factor { 9 | public: 10 | struct Evaluation : public Factor::Evaluation { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | Vector2 point2D; 14 | 15 | Scalar squaredErrorNorm; 16 | }; 17 | 18 | struct Linearization : public Factor::Linearization { 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | Scalar Drho; 22 | Scalar sqrtDrho; 23 | Scalar alpha; 24 | 25 | Vector2 scaledError; 26 | 27 | Vector6 gPose; 28 | }; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | SCOAT_FACTOR_EVALUATION_NEW 33 | SCOAT_FACTOR_LINEARIZATION_NEW 34 | 35 | PinholeCameraFactor(const std::vector &poses, 36 | const std::vector &shapes, 37 | const std::vector &joints, 38 | const std::vector ¶ms, const Scalar &sigma, 39 | const Scalar &eps, const Vector2 &measurement, 40 | const Scalar &confidence, const std::string &name = "", 41 | int index = -1, bool active = true); 42 | 43 | protected: 44 | virtual int preLinearizeRobustKernel(const Evaluation &eval, 45 | Linearization &lin) const; 46 | 47 | protected: 48 | Vector2 mMeasurement; 49 | 50 | Scalar mCon; 51 | Scalar mSqrtCon; 52 | Scalar mSigmaCon; 53 | 54 | Scalar mSigma; 55 | Scalar mGMThreshold; 56 | Scalar mMaxGMScale; 57 | }; 58 | } // namespace scope 59 | -------------------------------------------------------------------------------- /C++/scope/initializer/ArmInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace Initializer { 10 | class ArmInitializer : public Initializer { 11 | public: 12 | enum class Arm { Left, Right }; 13 | 14 | protected: 15 | std::array mRelJointLocations; 16 | 17 | mutable Vector3 mDJointChange; 18 | mutable Matrix3 mJointChange; 19 | 20 | mutable Scalar mElbowJoint[2]; 21 | 22 | mutable Matrix63 mB0; 23 | mutable Vector6 mB1; 24 | 25 | mutable Vector<3> mS; 26 | mutable Vector<3> ms; 27 | mutable Scalar mA; 28 | mutable Scalar ma; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | 33 | ArmInitializer(const Options &options, 34 | const std::array &RelJointLocations, Arm LR); 35 | 36 | ArmInitializer(const ArmInitializer &) = delete; 37 | 38 | ArmInitializer &operator=(const ArmInitializer &) = delete; 39 | 40 | virtual int initialize(const Pose &T0, 41 | const AlignedVector &joints) const override; 42 | 43 | public: 44 | virtual int FKintree(int n) const override; 45 | virtual int DFKintree() const override; 46 | 47 | virtual int updateGaussNewton() const override; 48 | virtual int solveGaussNewton() const override; 49 | 50 | virtual int update(Scalar stepsize) const override; 51 | virtual int accept() const override; 52 | 53 | protected: 54 | Arm mArm; 55 | 56 | const Scalar mElbowLowerBnd; 57 | const Scalar mElbowUpperBnd; 58 | }; 59 | 60 | } // namespace Initializer 61 | } // namespace scope 62 | -------------------------------------------------------------------------------- /C++/scope/initializer/LegInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace Initializer { 10 | 11 | class LegInitializer : public Initializer { 12 | public: 13 | std::array mRelJointLocations; 14 | 15 | mutable Vector6 mDRootPoseChange; 16 | mutable Pose mRootPoseChange; 17 | 18 | mutable Vector3 mDJointChange; 19 | mutable Matrix3 mJointChange; 20 | 21 | mutable Scalar mKneeJoint[2]; 22 | 23 | mutable Matrix63 mB0; 24 | mutable Vector6 mB1; 25 | 26 | mutable Vector<9> mS; 27 | mutable Vector<9> ms; 28 | mutable Scalar mA; 29 | mutable Scalar ma; 30 | 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | 34 | LegInitializer(const Options &options, 35 | const std::array &RelJointLocations); 36 | 37 | LegInitializer(const LegInitializer &) = delete; 38 | 39 | LegInitializer &operator=(const LegInitializer &) = delete; 40 | 41 | virtual int initialize(const Pose &T0, 42 | const AlignedVector &joints) const override; 43 | 44 | public: 45 | virtual int FKintree(int n) const override; 46 | virtual int DFKintree() const override; 47 | 48 | virtual int updateGaussNewton() const override; 49 | virtual int solveGaussNewton() const override; 50 | 51 | virtual int update(Scalar stepsize) const override; 52 | virtual int accept() const override; 53 | 54 | protected: 55 | const Scalar KneeLowerBnd = 0; 56 | const Scalar KneeUpperBnd = 0.85 * M_PI; 57 | }; 58 | 59 | } // namespace Initializer 60 | } // namespace scope 61 | -------------------------------------------------------------------------------- /C++/scope/initializer/ExtTorsoInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace Initializer { 10 | class ExtTorsoInitializer : public Initializer { 11 | protected: 12 | Vector3 mRelJointLocation; 13 | 14 | mutable Vector6 mDRootPoseChange; 15 | mutable Pose mRootPoseChange; 16 | 17 | mutable Scalar mSpineAngle[2]; 18 | 19 | mutable Vector6 mB; 20 | 21 | mutable Vector<6> mS; 22 | mutable Vector<6> ms; 23 | mutable Scalar mA; 24 | mutable Scalar ma; 25 | 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | ExtTorsoInitializer(const Options &options, const Vector3 &RelJointLocation); 30 | 31 | ExtTorsoInitializer(const ExtTorsoInitializer &) = delete; 32 | 33 | ExtTorsoInitializer &operator=(const ExtTorsoInitializer &) = delete; 34 | 35 | virtual int initialize(const Pose &T0, 36 | const AlignedVector &joints) const override; 37 | 38 | int initialize(const Pose& T0, const Scalar& SpineAngle) const; 39 | 40 | Scalar getSpineAngle() const { return mSpineAngle[0]; }; 41 | 42 | public: 43 | virtual int FKintree(int n) const override; 44 | virtual int DFKintree() const override; 45 | 46 | virtual int updateGaussNewton() const override; 47 | virtual int solveGaussNewton() const override; 48 | 49 | virtual int update(Scalar stepsize) const override; 50 | virtual int accept() const override; 51 | 52 | protected: 53 | const Scalar SpineLowerBnd = 0; 54 | const Scalar SpineUpperBnd = 0.3 * M_PI; 55 | }; 56 | 57 | } // namespace Initializer 58 | } // namespace scope 59 | -------------------------------------------------------------------------------- /C++/scope/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | # Enable faster instruction sets (SIMD/AVX) 3 | set(ENABLE_FAST_INSTRUCTIONS ON CACHE BOOL "Enable faster instruction sets (SIMD/AVX)") 4 | # Enable OpenMP (if available) 5 | set(ENABLE_OPENMP OFF CACHE BOOL "Enable OpenMP (if available)") 6 | 7 | if(${ENABLE_FAST_INSTRUCTIONS}) 8 | message(STATUS "Enabling SIMD/AVX instruction sets") 9 | add_definitions(-march=native) 10 | endif() 11 | 12 | # PERFORMANCE IMPROVEMENTS 13 | if(${ENABLE_OPENMP}) 14 | find_package(OpenMP) 15 | if(OPENMP_FOUND) 16 | message(STATUS "\nFound OpenMP! Turning on support for parallelization\n") 17 | endif() 18 | endif() 19 | 20 | set(SCOPE_INCLUDE_DIR ${PROJECT_SOURCE_DIR}) 21 | set(SCOPE_INCLUDES ${SCOPE_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} CACHE INTERNAL "") 22 | 23 | set(SCOPE_SRC_DIR ${PROJECT_SOURCE_DIR}) 24 | 25 | file(GLOB_RECURSE SCOPE_SRCS ${SCOPE_SRC_DIR} "*.cpp") 26 | 27 | file(GLOB_RECURSE SCOPE_HDRS ${SCOPE_SRC_DIR} "*.h") 28 | 29 | add_library(SCOPE SHARED ${SCOPE_HDRS} ${SCOPE_SRCS}) 30 | target_include_directories(SCOPE PUBLIC ${SCOPE_INCLUDE_DIR}) 31 | target_link_libraries(SCOPE PUBLIC ${SCOPE_LIBRARIES}) 32 | 33 | if(OPENMP_FOUND) 34 | # Add additional compilation flags to enable OpenMP support 35 | set_target_properties(SCOPE PROPERTIES COMPILE_FLAGS ${OpenMP_CXX_FLAGS}) 36 | target_link_libraries(SCOPE PUBLIC omp) 37 | endif() 38 | 39 | # Add add entry for this project into CMake's package registry, so that this project can be found by other CMake projects 40 | export(PACKAGE SCOPE) 41 | # Create a configuration file for this project, so that it can be imported by other CMake projects 42 | export(TARGETS SCOPE FILE SCOPEConfig.cmake) 43 | -------------------------------------------------------------------------------- /C++/scope/factor/JointDepthCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class JointDepthCameraFactor : public DepthCameraFactor { 7 | public: 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | SCOAT_FACTOR_EVALUATION_NEW 10 | SCOAT_FACTOR_LINEARIZATION_NEW 11 | 12 | JointDepthCameraFactor(int pose, const Scalar &sigma, const Scalar &eps, 13 | const Vector3 &measurement, 14 | const Scalar &confidence = 1.0, 15 | const std::string &name = "", int index = -1, 16 | bool active = true); 17 | 18 | virtual int evaluate(const AlignedVector &poses, 19 | const AlignedVector &shapes, 20 | const AlignedVector &joints, 21 | const AlignedVector ¶ms, 22 | Factor::Evaluation &base_eval) const override; 23 | 24 | virtual int linearize(const AlignedVector &poses, 25 | const AlignedVector &shapes, 26 | const AlignedVector &joints, 27 | const AlignedVector ¶ms, 28 | const Factor::Evaluation &base_eval, 29 | Factor::Linearization &base_lin) const override; 30 | 31 | protected: 32 | virtual int evaluate(const Pose &pose, const Vector3 &measurement, 33 | Evaluation &eval) const; 34 | 35 | virtual int linearize(const Pose &pose, const Vector3 &measurement, 36 | const Evaluation &eval, Linearization &lin) const; 37 | }; 38 | 39 | } // namespace scope 40 | -------------------------------------------------------------------------------- /C++/scope/factor/JointPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class JointPinholeCameraFactor : public PinholeCameraFactor { 7 | public: 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | SCOAT_FACTOR_EVALUATION_NEW 10 | SCOAT_FACTOR_LINEARIZATION_NEW 11 | 12 | JointPinholeCameraFactor(int pose, const Scalar &sigma, const Scalar &eps, 13 | const Vector2 &measurement, 14 | const Scalar &confidence = 1.0, 15 | const std::string &name = "", int index = -1, 16 | bool active = true); 17 | 18 | virtual int evaluate(const AlignedVector &poses, 19 | const AlignedVector &shapes, 20 | const AlignedVector &joints, 21 | const AlignedVector ¶ms, 22 | Factor::Evaluation &base_eval) const override; 23 | 24 | virtual int linearize(const AlignedVector &poses, 25 | const AlignedVector &shapes, 26 | const AlignedVector &joints, 27 | const AlignedVector ¶ms, 28 | const Factor::Evaluation &base_eval, 29 | Factor::Linearization &base_lin) const override; 30 | 31 | protected: 32 | virtual int evaluate(const Pose &pose, const Vector2 &measurement, 33 | Evaluation &eval) const; 34 | 35 | virtual int linearize(const Pose &pose, const Vector2 &measurement, 36 | const Evaluation &eval, Linearization &lin) const; 37 | }; 38 | 39 | } // namespace scope 40 | -------------------------------------------------------------------------------- /C++/scope/factor/PinholeCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace scope { 4 | 5 | PinholeCameraFactor::PinholeCameraFactor( 6 | const std::vector &poses, const std::vector &shapes, 7 | const std::vector &joints, const std::vector ¶ms, 8 | const Scalar &sigma, const Scalar &eps, const Vector2 &measurement, 9 | const Scalar &confidence, const std::string &name, int index, bool active) 10 | : Factor(poses, shapes, joints, params, name, index, active), 11 | mMeasurement(measurement), 12 | mSigma(sigma), 13 | mCon(confidence), 14 | mSqrtCon(std::sqrt(confidence)), 15 | mSigmaCon(confidence * sigma) { 16 | assert(eps > 1e-5 && eps < 1.0); 17 | assert(sigma > 0); 18 | assert(confidence >= 0); 19 | 20 | mMaxGMScale = 1 - eps; 21 | mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma; 22 | } 23 | 24 | int PinholeCameraFactor::preLinearizeRobustKernel(const Evaluation &eval, 25 | Linearization &lin) const { 26 | // scaled jacobians and graidents related with GM kernel 27 | lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm); 28 | lin.Drho = lin.sqrtDrho * lin.sqrtDrho; 29 | lin.alpha = eval.squaredErrorNorm > mGMThreshold 30 | ? mMaxGMScale 31 | : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm / 32 | (mSigma + eval.squaredErrorNorm)); 33 | 34 | if (lin.alpha > 1e-5) { 35 | lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm; 36 | } else { 37 | lin.scaledError.setZero(); 38 | } 39 | 40 | lin.sqrtDrho *= mSqrtCon; 41 | lin.Drho *= mCon; 42 | 43 | return 0; 44 | } 45 | } // namespace scope 46 | -------------------------------------------------------------------------------- /C++/scope/factor/DepthCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace scope { 7 | DepthCameraFactor::DepthCameraFactor( 8 | const std::vector &poses, const std::vector &shapes, 9 | const std::vector &joints, const std::vector ¶ms, 10 | const Scalar &sigma, const Scalar &eps, const Vector3 &measurement, 11 | const Scalar &confidence, const std::string &name, int index, bool active) 12 | : Factor(poses, shapes, joints, params, name, index, active), 13 | mMeasurement(measurement), 14 | mSigma(sigma), 15 | mCon(confidence), 16 | mSqrtCon(std::sqrt(confidence)), 17 | mSigmaCon(confidence * sigma) { 18 | assert(eps > 1e-5 && eps < 1.0); 19 | assert(sigma > 0); 20 | assert(confidence >= 0); 21 | 22 | mMaxGMScale = 1 - eps; 23 | mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma; 24 | } 25 | 26 | int DepthCameraFactor::preLinearizeRobustKernel(const Evaluation &eval, 27 | Linearization &lin) const { 28 | // scaled jacobians and graidents related with GM kernel 29 | lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm); 30 | lin.Drho = lin.sqrtDrho * lin.sqrtDrho; 31 | lin.alpha = eval.squaredErrorNorm > mGMThreshold 32 | ? mMaxGMScale 33 | : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm / 34 | (mSigma + eval.squaredErrorNorm)); 35 | 36 | if (lin.alpha > 1e-5) { 37 | lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm; 38 | } else { 39 | lin.scaledError.setZero(); 40 | } 41 | 42 | lin.sqrtDrho *= mSqrtCon; 43 | lin.Drho *= mCon; 44 | 45 | return 0; 46 | } 47 | } // namespace scope 48 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/POFFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class POFFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Vector3 d; // direction 13 | 14 | Scalar squaredErrorNorm; 15 | }; 16 | 17 | struct Linearization : public Factor::Linearization { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | Matrix<1, 3> jacobian; 21 | 22 | Scalar sqrtDrho; 23 | Scalar alpha; 24 | Scalar scaledError; 25 | }; 26 | 27 | public: 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | SCOAT_INITIAL_EVALUATION_NEW 30 | SCOAT_INITIAL_LINEARIZATION_NEW 31 | 32 | POFFactor(int pose, const Vector3 &S, const Scalar &sigma, const Scalar &eps, 33 | const Vector3 &measurement, const Scalar &confidence = 1.0, 34 | const std::string &name = "", int index = -1); 35 | 36 | virtual int evaluate(const AlignedVector &poses, 37 | const AlignedVector &joints, 38 | Factor::Evaluation &base_eval) const override; 39 | 40 | virtual int linearize(const AlignedVector &poses, 41 | const AlignedVector &joints, 42 | const Factor::Evaluation &base_eval, 43 | Factor::Linearization &base_lin) const override; 44 | 45 | int getPose() const { return mPose; } 46 | 47 | protected: 48 | int mPose; 49 | 50 | Vector3 mMeasurement; 51 | 52 | Scalar mCon; 53 | Scalar mSqrtCon; 54 | Scalar mSigmaCon; 55 | 56 | Scalar mSigma; 57 | Scalar mGMThreshold; 58 | Scalar mMinAlpha; 59 | 60 | Vector3 mS; 61 | }; 62 | } // namespace Initializer 63 | } // namespace scope 64 | -------------------------------------------------------------------------------- /C++/scope/base/Types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | //#define EIGEN_VECTORIZE_SSE4_2 3 | #define EIGEN_VECTORIZE_AVX2 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace scope { 10 | using Scalar = double; 11 | 12 | template 13 | using Matrix = Eigen::Matrix; 14 | template 15 | using Vector = Eigen::Matrix; 16 | template 17 | using Diagonal = Eigen::Diagonal; 18 | 19 | using MatrixX = Matrix; 20 | using VectorX = Vector; 21 | using Matrix2X = Matrix<2, Eigen::Dynamic>; 22 | using Matrix3X = Matrix<3, Eigen::Dynamic>; 23 | using MatrixX2 = Matrix; 24 | using MatrixX3 = Matrix; 25 | using Matrix2 = Matrix<2, 2>; 26 | using Matrix3 = Matrix<3, 3>; 27 | using Matrix4 = Matrix<4, 4>; 28 | using Vector2 = Vector<2>; 29 | using Vector3 = Vector<3>; 30 | using Vector4 = Vector<4>; 31 | using Matrix4X = Matrix<4, Eigen::Dynamic>; 32 | using MatrixX4 = Matrix; 33 | using Matrix6X = Matrix<6, Eigen::Dynamic>; 34 | using MatrixX6 = Matrix; 35 | using Matrix63 = Matrix<6, 3>; 36 | using Matrix26 = Matrix<2, 6>; 37 | using Matrix36 = Matrix<3, 6>; 38 | using Matrix6 = Matrix<6, 6>; 39 | using Vector6 = Matrix<6, 1>; 40 | 41 | template 42 | using RowMajorMatrix = Eigen::Matrix; 43 | 44 | using RowMajorMatrixX = RowMajorMatrix; 45 | using RowMajorMatrix3X = RowMajorMatrix<3, Eigen::Dynamic>; 46 | using RowMajorMatrix36 = RowMajorMatrix<3, 6>; 47 | 48 | template 49 | using AlignedVector = std::vector>; 50 | 51 | enum class HumanModel { SMPL }; 52 | } // namespace scope 53 | -------------------------------------------------------------------------------- /C++/scope/factor/POFFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace scope { 4 | 5 | POFFactor::POFFactor(const std::vector &poses, 6 | const std::vector &shapes, 7 | const std::vector &joints, 8 | const std::vector ¶ms, const Scalar &sigma, 9 | const Scalar &eps, const Vector3 &measurement, 10 | const Scalar &confidence, const std::string &name, 11 | int index, bool active) 12 | : Factor(poses, shapes, joints, params, name, index, active), 13 | mMeasurement(measurement), 14 | mSigma(sigma), 15 | mCon(confidence), 16 | mSqrtCon(std::sqrt(confidence)), 17 | mSigmaCon(confidence * sigma) { 18 | assert(eps > 1e-5 && eps <= 1.0); 19 | assert(sigma > 0); 20 | assert(confidence >= 0); 21 | 22 | mMaxGMScale = 1 - eps; 23 | mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma; 24 | 25 | mMeasurement.stableNormalize(); 26 | } 27 | 28 | int POFFactor::preLinearizeRobustKernel(const Evaluation &eval, 29 | Linearization &lin) const { 30 | // scaled jacobians and graidents related with GM kernel 31 | lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm); 32 | lin.Drho = lin.sqrtDrho * lin.sqrtDrho; 33 | lin.alpha = eval.squaredErrorNorm > mGMThreshold 34 | ? mMaxGMScale 35 | : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm / 36 | (mSigma + eval.squaredErrorNorm)); 37 | 38 | if (lin.alpha > 1e-5) { 39 | lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm; 40 | } else { 41 | lin.scaledError.setZero(); 42 | } 43 | 44 | lin.sqrtDrho *= mSqrtCon; 45 | lin.Drho *= mCon; 46 | 47 | return 0; 48 | } 49 | 50 | } // namespace scope 51 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/PoseConstFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class PoseConstFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Matrix3 errorR; 13 | Vector6 xi; 14 | 15 | Vector6 errorL; 16 | Vector6 errorU; 17 | 18 | Vector6 errorLInv; 19 | Vector6 errorUInv; 20 | 21 | Vector6 expL; 22 | Vector6 expU; 23 | }; 24 | 25 | struct Linearization : public Factor::Linearization { 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | Matrix6 jacobian; 29 | 30 | Matrix3 dexpinv; 31 | 32 | Vector6 Derror; 33 | }; 34 | 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | SCOAT_INITIAL_EVALUATION_NEW 38 | SCOAT_INITIAL_LINEARIZATION_NEW 39 | 40 | PoseConstFactor(int pose, const Vector6 &weight, const Pose &mean, 41 | const Vector6 &lbnd, const Vector6 &ubnd, 42 | const std::string &name = "", int index = -1); 43 | 44 | virtual int evaluate(const AlignedVector &poses, 45 | const AlignedVector &joints, 46 | Factor::Evaluation &base_eval) const override; 47 | 48 | virtual int linearize(const AlignedVector &poses, 49 | const AlignedVector &joints, 50 | const Factor::Evaluation &base_eval, 51 | Factor::Linearization &base_lin) const override; 52 | 53 | int getPose() const { return mPose; } 54 | 55 | protected: 56 | int mPose; 57 | 58 | Vector6 mWeight; 59 | Pose mMean; 60 | 61 | Vector6 mLowerBnd; 62 | Vector6 mUpperBnd; 63 | 64 | const Scalar mScale = 2.5e-2; 65 | const Scalar mBnd = -1e-6; 66 | }; 67 | } // namespace Initializer 68 | } // namespace scope 69 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/JointConstFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class JointConstFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Matrix3 errorR; 13 | Vector3 xi; 14 | 15 | Vector3 errorL; 16 | Vector3 errorU; 17 | 18 | Vector3 errorLInv; 19 | Vector3 errorUInv; 20 | 21 | Vector3 expL; 22 | Vector3 expU; 23 | }; 24 | 25 | struct Linearization : public Factor::Linearization { 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | Matrix3 jacobian; 29 | 30 | Matrix3 DF; 31 | 32 | Vector3 Derror; 33 | }; 34 | 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | SCOAT_INITIAL_EVALUATION_NEW 38 | SCOAT_INITIAL_LINEARIZATION_NEW 39 | 40 | JointConstFactor(int pose, const Vector3 &weight, const Matrix3 &mean, 41 | const Vector3 &lbnd, const Vector3 &ubnd, 42 | const std::string &name = "", int index = -1); 43 | 44 | virtual int evaluate(const AlignedVector &poses, 45 | const AlignedVector &joints, 46 | Factor::Evaluation &base_eval) const override; 47 | 48 | virtual int linearize(const AlignedVector &poses, 49 | const AlignedVector &joints, 50 | const Factor::Evaluation &base_eval, 51 | Factor::Linearization &base_lin) const override; 52 | 53 | int getJoint() const { return mJoint; } 54 | 55 | protected: 56 | int mJoint; 57 | 58 | Vector3 mWeight; 59 | Matrix3 mMean; 60 | 61 | Vector3 mLowerBnd; 62 | Vector3 mUpperBnd; 63 | 64 | const Scalar mScale = 2.5e-2; 65 | const Scalar mBnd = -1e-6; 66 | }; 67 | } // namespace Initializer 68 | } // namespace scope 69 | -------------------------------------------------------------------------------- /C++/scope/factor/PoseConstFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class PoseConstFactor : public Factor { 7 | public: 8 | struct Evaluation : public Factor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Matrix3 errorR; 12 | Vector6 xi; 13 | 14 | Vector6 errorL; 15 | Vector6 errorU; 16 | 17 | Vector6 errorLInv; 18 | Vector6 errorUInv; 19 | 20 | Vector6 expL; 21 | Vector6 expU; 22 | }; 23 | 24 | struct Linearization : public Factor::Linearization { 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | Matrix3 dexpinv; 28 | 29 | Vector6 Derror; 30 | }; 31 | 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | SCOAT_FACTOR_EVALUATION_NEW 35 | SCOAT_FACTOR_LINEARIZATION_NEW 36 | 37 | PoseConstFactor(int pose, const Vector6 &weight, const Pose &mean, 38 | const Vector6 &lbnd, const Vector6 &ubnd, 39 | const std::string &name = "", int index = -1, 40 | bool active = true); 41 | 42 | virtual int evaluate(const AlignedVector &poses, 43 | const AlignedVector &shapes, 44 | const AlignedVector &joints, 45 | const AlignedVector ¶ms, 46 | Factor::Evaluation &base_eval) const override; 47 | 48 | virtual int linearize(const AlignedVector &poses, 49 | const AlignedVector &shapes, 50 | const AlignedVector &joints, 51 | const AlignedVector ¶ms, 52 | const Factor::Evaluation &base_eval, 53 | Factor::Linearization &base_lin) const override; 54 | 55 | protected: 56 | Vector6 mWeight; 57 | Pose mMean; 58 | 59 | Vector6 mLowerBnd; 60 | Vector6 mUpperBnd; 61 | 62 | const Scalar mScale = 1e-2; 63 | const Scalar mBnd = -1e-6; 64 | }; 65 | } // namespace scope 66 | -------------------------------------------------------------------------------- /C++/scope/factor/JointConstFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class JointConstFactor : public Factor { 7 | public: 8 | struct Evaluation : public Factor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Matrix3 errorR; 12 | Vector3 xi; 13 | 14 | Vector3 errorL; 15 | Vector3 errorU; 16 | 17 | Vector3 errorLInv; 18 | Vector3 errorUInv; 19 | 20 | Vector3 expL; 21 | Vector3 expU; 22 | }; 23 | 24 | struct Linearization : public Factor::Linearization { 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | Matrix3 DF; 28 | 29 | Vector3 Derror; 30 | }; 31 | 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | SCOAT_FACTOR_EVALUATION_NEW 35 | SCOAT_FACTOR_LINEARIZATION_NEW 36 | 37 | JointConstFactor(int joint, const Vector3 &weight, const Matrix3 &mean, 38 | const Vector3 &lbnd, const Vector3 &ubnd, 39 | const std::string &name = "", int index = -1, 40 | bool active = true); 41 | 42 | virtual int evaluate(const AlignedVector &poses, 43 | const AlignedVector &shapes, 44 | const AlignedVector &joints, 45 | const AlignedVector ¶ms, 46 | Factor::Evaluation &base_eval) const override; 47 | 48 | virtual int linearize(const AlignedVector &poses, 49 | const AlignedVector &shapes, 50 | const AlignedVector &joints, 51 | const AlignedVector ¶ms, 52 | const Factor::Evaluation &base_eval, 53 | Factor::Linearization &base_lin) const override; 54 | 55 | protected: 56 | Vector3 mWeight; 57 | Matrix3 mMean; 58 | 59 | Vector3 mLowerBnd; 60 | Vector3 mUpperBnd; 61 | 62 | const Scalar mScale = 2.5e-2; 63 | const Scalar mBnd = -1e-6; 64 | }; 65 | } // namespace scope 66 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/DepthCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class DepthCameraFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Vector3 point3D; 13 | Scalar squaredErrorNorm; 14 | }; 15 | 16 | struct Linearization : public Factor::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Matrix36 jacobian; 20 | 21 | Scalar Drho; 22 | Scalar sqrtDrho; 23 | Scalar alpha; 24 | 25 | Vector3 scaledError; 26 | 27 | Vector6 g; 28 | }; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | SCOAT_INITIAL_EVALUATION_NEW 33 | SCOAT_INITIAL_LINEARIZATION_NEW 34 | 35 | DepthCameraFactor(int pose, const Vector3 &point, const Scalar &sigma, 36 | const Scalar &eps, const Vector3 &measurement, 37 | const Scalar &confidence = 1.0, 38 | const std::string &name = "", int index = -1); 39 | 40 | virtual int evaluate(const AlignedVector &poses, 41 | const AlignedVector &joints, 42 | Factor::Evaluation &base_eval) const override; 43 | 44 | virtual int linearize(const AlignedVector &poses, 45 | const AlignedVector &joints, 46 | const Factor::Evaluation &base_eval, 47 | Factor::Linearization &base_lin) const override; 48 | 49 | int getPose() const { return mPose; } 50 | 51 | const Vector3 &point() const { return mPoint; } 52 | 53 | const Vector3 &measurement() const { return mMeasurement; } 54 | 55 | protected: 56 | int mPose; 57 | 58 | Vector3 mPoint; 59 | Vector3 mMeasurement; 60 | 61 | Scalar mCon; 62 | Scalar mSqrtCon; 63 | Scalar mSigmaCon; 64 | 65 | Scalar mSigma; 66 | Scalar mGMThreshold; 67 | Scalar mMaxGMScale; 68 | }; 69 | } // namespace Initializer 70 | } // namespace scope 71 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/PinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class PinholeCameraFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Vector3 pCam; 13 | 14 | Vector2 point2D; 15 | Scalar squaredErrorNorm; 16 | }; 17 | 18 | struct Linearization : public Factor::Linearization { 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | Matrix26 jacobian; 22 | 23 | Scalar Drho; 24 | Scalar sqrtDrho; 25 | Scalar alpha; 26 | 27 | Vector2 scaledError; 28 | 29 | Vector6 g; 30 | }; 31 | 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | SCOAT_INITIAL_EVALUATION_NEW 35 | SCOAT_INITIAL_LINEARIZATION_NEW 36 | 37 | PinholeCameraFactor(int pose, const Vector3 &point, const Scalar &sigma, 38 | const Scalar &eps, const Vector2 &measurement, 39 | const Scalar &confidence = 1.0, 40 | const std::string &name = "", int index = -1); 41 | 42 | virtual int evaluate(const AlignedVector &poses, 43 | const AlignedVector &joints, 44 | Factor::Evaluation &base_eval) const override; 45 | 46 | virtual int linearize(const AlignedVector &poses, 47 | const AlignedVector &joints, 48 | const Factor::Evaluation &base_eval, 49 | Factor::Linearization &base_lin) const override; 50 | 51 | int getPose() const { return mPose; } 52 | 53 | const Vector3 &point() const { return mPoint; } 54 | 55 | const Vector2 &measurement() const { return mMeasurement; } 56 | 57 | protected: 58 | int mPose; 59 | 60 | Vector3 mPoint; 61 | Vector2 mMeasurement; 62 | 63 | Scalar mCon; 64 | Scalar mSqrtCon; 65 | Scalar mSigmaCon; 66 | 67 | Scalar mSigma; 68 | Scalar mGMThreshold; 69 | Scalar mMaxGMScale; 70 | }; 71 | } // namespace Initializer 72 | } // namespace scope 73 | -------------------------------------------------------------------------------- /C++/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | # PROJECT CONFIGURATION 4 | project(SCOPE LANGUAGES C CXX VERSION 1.0.0) 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) # We require C++ 17 7 | 8 | # Enable code profiling using gperftools 9 | set(ENABLE_PROFILING OFF CACHE BOOL "Enable code profiling using gperftools") 10 | 11 | if(${ENABLE_PROFILING}) 12 | message(STATUS "Enabling code profiling using Google Performance Tools") 13 | set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -lprofiler) 14 | endif() 15 | 16 | # Set build type to 'RelWithDebInfo' if one was not specified by the user 17 | if(NOT CMAKE_BUILD_TYPE) 18 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." FORCE) 19 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS RelWithDebInfo Release Debug MinSizeRel) 20 | message(STATUS "Setting build type to ${CMAKE_BUILD_TYPE}, as none was specified\n") 21 | else() 22 | message(STATUS "Building in ${CMAKE_BUILD_TYPE} mode\n") 23 | endif() 24 | 25 | if(${CMAKE_BUILD_TYPE} STREQUAL "Debug") 26 | add_definitions(-fstandalone-debug) 27 | endif() 28 | 29 | add_definitions(-march=native) 30 | 31 | # Directory for built libraries 32 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/lib CACHE PATH "The directory in which to place the library built by this project") 33 | # Directory for built executables 34 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/bin CACHE PATH "The directory in which to place executables built by this project") 35 | 36 | 37 | set(SCOPE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/scope/ CACHE PATH "Path to top-level directory of scope (the one containing CMakeLists.txt)") 38 | set(examples ${CMAKE_CURRENT_SOURCE_DIR}/examples/ CACHE PATH "Path to top-level directory of examples (the one containing CMakeLists.txt)") 39 | 40 | add_definitions(-DRETRACT_TOL=0.5 -DINVERSE_OF_RETRACT_TOL=3.76 -DDRETRACT_TOL=0.5) 41 | 42 | include_directories(./) 43 | 44 | find_package(Eigen3 REQUIRED) 45 | 46 | set(GLOG_LIBRARIES glog) 47 | 48 | include_directories(${EIGEN3_INCLUDE_DIR}) 49 | 50 | set(SCOPE_LIBRARIES ${GLOG_LIBRARIES}) 51 | 52 | add_subdirectory(./cnpy) 53 | add_subdirectory(${SCOPE_DIR}) 54 | add_subdirectory(${examples}) 55 | -------------------------------------------------------------------------------- /C++/scope/factor/FullJointPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace scope { 8 | class FullJointPinholeCameraFactor : public JointPinholeCameraFactor { 9 | public: 10 | struct Evaluation : public JointPinholeCameraFactor::Evaluation { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | Vector2 camMeasurement; 14 | }; 15 | 16 | struct Linearization : public JointPinholeCameraFactor::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Vector4 gCam; 20 | }; 21 | 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | SCOAT_FACTOR_EVALUATION_NEW 25 | SCOAT_FACTOR_LINEARIZATION_NEW 26 | 27 | FullJointPinholeCameraFactor(int pose, int camParam, const Scalar &sigma, 28 | const Scalar &eps, const Vector2 &measurement, 29 | const Scalar &confidence = 1.0, 30 | const std::string &name = "", int index = -1, 31 | bool active = true); 32 | 33 | virtual int evaluate(const AlignedVector &poses, 34 | const AlignedVector &shapes, 35 | const AlignedVector &joints, 36 | const AlignedVector ¶ms, 37 | Factor::Evaluation &base_eval) const override; 38 | 39 | virtual int linearize(const AlignedVector &poses, 40 | const AlignedVector &shapes, 41 | const AlignedVector &joints, 42 | const AlignedVector ¶ms, 43 | const Factor::Evaluation &base_eval, 44 | Factor::Linearization &base_lin) const override; 45 | 46 | protected: 47 | // camParam: the inverse of the camera intrinsic parameters 48 | virtual int evaluate(const Pose &pose, const VectorX &camParam, 49 | const Vector2 &measurement, Evaluation &eval) const; 50 | 51 | virtual int linearize(const Pose &pose, const VectorX &camParam, 52 | const Vector2 &measurement, const Evaluation &eval, 53 | Linearization &lin) const; 54 | }; 55 | 56 | } // namespace scope 57 | -------------------------------------------------------------------------------- /C++/scope/factor/ScaledPOFFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class ScaledPOFFactor : public POFFactor { 7 | public: 8 | struct Evaluation : public POFFactor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | // vertex positions 12 | Vector3 vertex; 13 | Vector3 D; 14 | Scalar DNorm; 15 | }; 16 | 17 | struct Linearization : public POFFactor::Linearization { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | Matrix3 DF; 21 | Matrix3 DFR; 22 | 23 | VectorX gVertex; 24 | }; 25 | 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | SCOAT_FACTOR_EVALUATION_NEW 29 | SCOAT_FACTOR_LINEARIZATION_NEW 30 | 31 | ScaledPOFFactor(int pose, int vParam, const std::array &VDirs, 32 | const std::array &V, const Scalar &sigma, 33 | const Scalar &eps, const Vector3 &measurement, 34 | const Scalar &confidence = 1.0, const std::string &name = "", 35 | int index = -1, bool active = true); 36 | 37 | virtual int evaluate(const AlignedVector &poses, 38 | const AlignedVector &shapes, 39 | const AlignedVector &joints, 40 | const AlignedVector ¶ms, 41 | Factor::Evaluation &base_eval) const override; 42 | 43 | virtual int linearize(const AlignedVector &poses, 44 | const AlignedVector &shapes, 45 | const AlignedVector &joints, 46 | const AlignedVector ¶ms, 47 | const Factor::Evaluation &base_eval, 48 | Factor::Linearization &base_lin) const override; 49 | 50 | protected: 51 | Matrix3X mVDirs; 52 | Vector3 mV; 53 | 54 | protected: 55 | virtual int evaluate(const Pose &pose, const VectorX &vertexParam, 56 | const Vector3 &measurement, Evaluation &eval) const; 57 | 58 | virtual int linearize(const Pose &pos, const VectorX &vertexParam, 59 | const Vector3 &measurement, const Evaluation &eval, 60 | Linearization &lin) const; 61 | }; 62 | } // namespace scope 63 | -------------------------------------------------------------------------------- /C++/scope/factor/FullJointExtraPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class FullJointExtraPinholeCameraFactor : public JointExtraPinholeCameraFactor { 7 | public: 8 | struct Evaluation : public JointExtraPinholeCameraFactor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Vector2 camMeasurement; 12 | }; 13 | 14 | struct Linearization : public JointExtraPinholeCameraFactor::Linearization { 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | Vector4 gCam; 18 | }; 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | SCOAT_FACTOR_EVALUATION_NEW 23 | SCOAT_FACTOR_LINEARIZATION_NEW 24 | 25 | FullJointExtraPinholeCameraFactor(int pose, int camParam, const Scalar &sigma, 26 | const Scalar &eps, const Pose &extraCamPose, 27 | const Vector2 &measurement, 28 | const Scalar &confidence = 1.0, 29 | const std::string &name = "", 30 | int index = -1, bool active = true); 31 | 32 | virtual int evaluate(const AlignedVector &poses, 33 | const AlignedVector &shapes, 34 | const AlignedVector &joints, 35 | const AlignedVector ¶ms, 36 | Factor::Evaluation &base_eval) const override; 37 | 38 | virtual int linearize(const AlignedVector &poses, 39 | const AlignedVector &shapes, 40 | const AlignedVector &joints, 41 | const AlignedVector ¶ms, 42 | const Factor::Evaluation &base_eval, 43 | Factor::Linearization &base_lin) const override; 44 | 45 | protected: 46 | virtual int evaluate(const Pose &pose, const VectorX &camParam, 47 | const Vector2 &measurement, Evaluation &eval) const; 48 | 49 | virtual int linearize(const Pose &pose, const VectorX &camParam, 50 | const Vector2 &measurement, const Evaluation &eval, 51 | Linearization &lin) const; 52 | }; 53 | } // namespace scope 54 | -------------------------------------------------------------------------------- /examples/keypoints.json: -------------------------------------------------------------------------------- 1 | {"keypoints2D": [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [779.0271606445312, 227.43362426757812, 0.919921875], [729.7330322265625, 234.82774353027344, 0.9501953125], [788.885986328125, 264.40423583984375, 0.9443359375], [710.0153198242188, 284.1219177246094, 0.9306640625], [833.250732421875, 338.3454895019531, 0.9375], [665.6505737304688, 358.06317138671875, 0.92626953125], [793.8154296875, 348.2043151855469, 0.908203125], [690.2976684570312, 397.4985046386719, 0.923828125], [779.0271606445312, 392.5690612792969, 0.9091796875], [705.0859375, 387.6396484375, 0.939453125], [793.8154296875, 525.663330078125, 0.93359375], [710.0153198242188, 530.5927124023438, 0.9326171875], [749.45068359375, 594.6751708984375, 0.9619140625], [705.0859375, 643.9692993164062, 0.9833984375], [754.3800659179688, 210.1806640625, 0.9208984375], [749.45068359375, 254.54541015625, 0.91650390625], [744.521240234375, 387.6396484375, 0.9150390625], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [749.45068359375, 323.5572509765625, 0.90234375], [754.3800659179688, 244.6865692138672, 0.96044921875]], "keypoints3D": [[0.000430583517299965, -0.00010900730558205396, 4.1587594751035795e-05], [0.13236862421035767, 0.0009572965791448951, 0.03587218374013901], [0.18842296302318573, 0.47035661339759827, 0.08599798381328583], [0.10788637399673462, 0.7413769960403442, 0.4313555061817169], [-0.13237665593624115, -0.0007385338540188968, -0.03581075370311737], [-0.10537257790565491, 0.476071834564209, 0.021793819963932037], [-0.09281030297279358, 0.8933771252632141, 0.19826242327690125], [0.009262217208743095, -0.2214859127998352, -0.06976015865802765], [-0.024990934878587723, -0.4052414298057556, -0.26614081859588623], [-0.029492607340216637, -0.415521502494812, -0.37359827756881714], [-0.034280434250831604, -0.5235323309898376, -0.3927011787891388], [0.11294405162334442, -0.38442671298980713, -0.22153836488723755], [0.2739526033401489, -0.1642414778470993, -0.12580379843711853], [0.10217417776584625, -0.09734351933002472, -0.33044514060020447], [-0.1488623470067978, -0.32767581939697266, -0.22934240102767944], [-0.2770284414291382, -0.09221624583005905, -0.13343201577663422], [-0.21732495725154877, 0.05343912914395332, -0.32920387387275696]], "camera": [1145.51133842318, 1144.77392807652, 514.968197319863, 501.882018537695]} -------------------------------------------------------------------------------- /C++/scope/factor/JointExtraPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | class JointExtraPinholeCameraFactor : public JointPinholeCameraFactor, 8 | public ExtraPinholeCamera { 9 | public: 10 | struct Evaluation : public JointPinholeCameraFactor::Evaluation, 11 | public ExtraPinholeCamera::Evaluation { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | }; 14 | 15 | struct Linearization : public JointPinholeCameraFactor::Linearization, 16 | public ExtraPinholeCamera::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | }; 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | SCOAT_FACTOR_EVALUATION_NEW 23 | SCOAT_FACTOR_LINEARIZATION_NEW 24 | 25 | JointExtraPinholeCameraFactor(int pose, const Scalar &sigma, 26 | const Scalar &eps, const Pose &extraCamPose, 27 | const Vector2 &measurement, 28 | const Scalar &confidence = 1.0, 29 | const std::string &name = "", int index = -1, 30 | bool active = true); 31 | 32 | virtual int evaluate(const AlignedVector &poses, 33 | const AlignedVector &shapes, 34 | const AlignedVector &joints, 35 | const AlignedVector ¶ms, 36 | Factor::Evaluation &base_eval) const override; 37 | 38 | virtual int linearize(const AlignedVector &poses, 39 | const AlignedVector &shapes, 40 | const AlignedVector &joints, 41 | const AlignedVector ¶ms, 42 | const Factor::Evaluation &base_eval, 43 | Factor::Linearization &base_lin) const override; 44 | 45 | protected: 46 | virtual int evaluate(const Pose &pose, const Vector2 &measurement, 47 | Evaluation &eval) const; 48 | 49 | virtual int linearize(const Pose &pose, const Vector2 &measurement, 50 | const Evaluation &eval, Linearization &lin) const; 51 | }; 52 | } // namespace scope 53 | -------------------------------------------------------------------------------- /C++/scope/factor/VertexDepthCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class VertexDepthCameraFactor : public DepthCameraFactor { 7 | public: 8 | struct Evaluation : public DepthCameraFactor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | // vertex position in the body frame 12 | Vector3 vertex; 13 | // vertex position in the camera frame 14 | Vector3 point3D; 15 | }; 16 | 17 | struct Linearization : public DepthCameraFactor::Linearization { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | VectorX gVertex; 21 | }; 22 | 23 | public: 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | SCOAT_FACTOR_EVALUATION_NEW 26 | SCOAT_FACTOR_LINEARIZATION_NEW 27 | 28 | VertexDepthCameraFactor(int pose, int vParam, const Matrix3X &vDirs, 29 | const Vector3 &v, const Scalar &sigma, 30 | const Scalar &eps, const Vector3 &measurement, 31 | const Scalar &confidence = 1.0, 32 | const std::string &name = "", int index = -1, 33 | bool active = true); 34 | 35 | virtual int evaluate(const AlignedVector &poses, 36 | const AlignedVector &shapes, 37 | const AlignedVector &joints, 38 | const AlignedVector ¶ms, 39 | Factor::Evaluation &base_eval) const override; 40 | 41 | virtual int linearize(const AlignedVector &poses, 42 | const AlignedVector &shapes, 43 | const AlignedVector &joints, 44 | const AlignedVector ¶ms, 45 | const Factor::Evaluation &base_eval, 46 | Factor::Linearization &base_lin) const override; 47 | 48 | protected: 49 | Matrix3X mvDirs; 50 | Vector3 mv; 51 | 52 | protected: 53 | virtual int evaluate(const Pose &pose, const VectorX &vertexParam, 54 | const Vector3 &measurement, Evaluation &eval) const; 55 | 56 | virtual int linearize(const Pose &pose, const VectorX &vertexParam, 57 | const Vector3 &measurement, const Evaluation &eval, 58 | Linearization &lin) const; 59 | }; 60 | } 61 | -------------------------------------------------------------------------------- /C++/scope/factor/VertexPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class VertexPinholeCameraFactor : public PinholeCameraFactor { 7 | public: 8 | struct Evaluation : public PinholeCameraFactor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | // vertex position in the body frame 12 | Vector3 vertex; 13 | // vertex position in the camera frame 14 | Vector3 pCam; 15 | }; 16 | 17 | struct Linearization : public PinholeCameraFactor::Linearization { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | MatrixX D; 21 | 22 | VectorX gVertex; 23 | }; 24 | 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | SCOAT_FACTOR_EVALUATION_NEW 28 | SCOAT_FACTOR_LINEARIZATION_NEW 29 | 30 | VertexPinholeCameraFactor(int pose, int vParam, const Matrix3X &vDirs, 31 | const Vector3 &v, const Scalar &sigma, 32 | const Scalar &eps, const Vector2 &measurement, 33 | const Scalar &confidence = 1.0, 34 | const std::string &name = "", int index = -1, 35 | bool active = true); 36 | 37 | virtual int evaluate(const AlignedVector &poses, 38 | const AlignedVector &shapes, 39 | const AlignedVector &joints, 40 | const AlignedVector ¶ms, 41 | Factor::Evaluation &base_eval) const override; 42 | 43 | virtual int linearize(const AlignedVector &poses, 44 | const AlignedVector &shapes, 45 | const AlignedVector &joints, 46 | const AlignedVector ¶ms, 47 | const Factor::Evaluation &base_eval, 48 | Factor::Linearization &base_lin) const override; 49 | 50 | protected: 51 | Matrix3X mvDirs; 52 | Vector3 mv; 53 | 54 | protected: 55 | virtual int evaluate(const Pose &pose, const VectorX &vertexParam, 56 | const Vector2 &measurement, Evaluation &eval) const; 57 | 58 | virtual int linearize(const Pose &pose, const VectorX &vertexParam, 59 | const Vector2 &measurement, const Evaluation &eval, 60 | Linearization &lin) const; 61 | }; 62 | } // namespace scope 63 | -------------------------------------------------------------------------------- /C++/scope/factor/RelPOFFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class RelPOFFactor : public POFFactor { 7 | public: 8 | struct Evaluation : public POFFactor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | // vertex positions 12 | Vector6 vertex; 13 | Vector3 D; 14 | Scalar DNorm; 15 | }; 16 | 17 | struct Linearization : public POFFactor::Linearization { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | Matrix3 DF; 21 | Matrix36 DFR; // u = [DF*R -Df*Rpar] 22 | 23 | Vector3 gJoint; 24 | VectorX gVertex; 25 | }; 26 | 27 | public: 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | SCOAT_FACTOR_EVALUATION_NEW 30 | SCOAT_FACTOR_LINEARIZATION_NEW 31 | 32 | RelPOFFactor(int pose, int vParam, const std::vector &parents, 33 | const std::array &VDirs, 34 | const std::array &V, const Scalar &sigma, 35 | const Scalar &eps, const Vector3 &measurement, 36 | const Scalar &confidence = 1.0, const std::string &name = "", 37 | int index = -1, bool active = true); 38 | 39 | virtual int evaluate(const AlignedVector &poses, 40 | const AlignedVector &shapes, 41 | const AlignedVector &joints, 42 | const AlignedVector ¶ms, 43 | Factor::Evaluation &base_eval) const override; 44 | 45 | virtual int linearize(const AlignedVector &poses, 46 | const AlignedVector &shapes, 47 | const AlignedVector &joints, 48 | const AlignedVector ¶ms, 49 | const Factor::Evaluation &base_eval, 50 | Factor::Linearization &base_lin) const override; 51 | 52 | protected: 53 | int mParent; 54 | 55 | Matrix6X mVDirs; 56 | Vector6 mV; 57 | 58 | protected: 59 | virtual int evaluate(const Pose &pose, const Pose &parent, 60 | const VectorX &vertexParam, const Vector3 &measurement, 61 | Evaluation &eval) const; 62 | 63 | virtual int linearize(const Pose &pose, const Pose &parent, 64 | const VectorX &vertexParam, const Vector3 &measurement, 65 | const Evaluation &eval, Linearization &lin) const; 66 | }; 67 | } // namespace scope 68 | -------------------------------------------------------------------------------- /C++/scope/factor/FullVertexPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class FullVertexPinholeCameraFactor : public VertexPinholeCameraFactor { 7 | public: 8 | struct Evaluation : public VertexPinholeCameraFactor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Vector2 camMeasurement; 12 | }; 13 | 14 | struct Linearization : public VertexPinholeCameraFactor::Linearization { 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | Vector4 gCam; 18 | }; 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | SCOAT_FACTOR_EVALUATION_NEW 23 | SCOAT_FACTOR_LINEARIZATION_NEW 24 | 25 | FullVertexPinholeCameraFactor(int pose, int vParams, int camParams, 26 | const Matrix3X &vDirs, const Vector3 &v, 27 | const Scalar &sigma, const Scalar &eps, 28 | const Vector2 &measurement, 29 | const Scalar &confidence = 1.0, 30 | const std::string &name = "", int index = -1, 31 | bool active = true); 32 | 33 | virtual int evaluate(const AlignedVector &poses, 34 | const AlignedVector &shapes, 35 | const AlignedVector &joints, 36 | const AlignedVector ¶ms, 37 | Factor::Evaluation &base_eval) const override; 38 | 39 | virtual int linearize(const AlignedVector &poses, 40 | const AlignedVector &shapes, 41 | const AlignedVector &joints, 42 | const AlignedVector ¶ms, 43 | const Factor::Evaluation &base_eval, 44 | Factor::Linearization &base_lin) const override; 45 | 46 | protected: 47 | // camParam: the inverse of the camera intrinsic parameters 48 | virtual int evaluate(const Pose &pose, const VectorX &vertexParam, 49 | const VectorX &camParam, const Vector2 &measurement, 50 | Evaluation &eval) const; 51 | 52 | virtual int linearize(const Pose &pose, const VectorX &vertexParam, 53 | const VectorX &camParam, const Vector2 &measurement, 54 | const Evaluation &eval, Linearization &lin) const; 55 | }; 56 | } // namespace scope 57 | -------------------------------------------------------------------------------- /C++/scope/factor/ParameterFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | ParameterFactor::ParameterFactor(int param, const MatrixX &sigma, 7 | const VectorX &mean, const std::string &name, 8 | int index, bool active) 9 | : Factor({}, {}, {}, {param}, name, index, active), mSqrtCov(sigma), 10 | mMean(mean) {} 11 | 12 | int ParameterFactor::evaluate(const AlignedVector &poses, 13 | const AlignedVector &shapes, 14 | const AlignedVector &joints, 15 | const AlignedVector ¶ms, 16 | Factor::Evaluation &base_eval) const { 17 | auto &eval = dynamic_cast(base_eval); 18 | eval.clear(); 19 | 20 | const auto &s = mvParams[0]; 21 | assert(s >= 0 && s < params.size()); 22 | 23 | if (s < 0 || s >= params.size()) { 24 | LOG(ERROR) << "The parameter must be valid." << std::endl; 25 | 26 | exit(-1); 27 | } 28 | 29 | eval.unscaledError = params[s] - mMean; 30 | 31 | eval.error.noalias() = mSqrtCov * eval.unscaledError; 32 | eval.f = eval.error.squaredNorm(); 33 | 34 | eval.status = Status::VALID; 35 | 36 | return 0; 37 | } 38 | 39 | int ParameterFactor::linearize(const AlignedVector &poses, 40 | const AlignedVector &shapes, 41 | const AlignedVector &joints, 42 | const AlignedVector ¶ms, 43 | const Factor::Evaluation &base_eval, 44 | Factor::Linearization &base_lin) const { 45 | assert(base_eval.status == Status::VALID); 46 | 47 | auto &eval = base_eval; 48 | auto &lin = base_lin; 49 | 50 | lin.clear(); 51 | 52 | if (eval.status != Status::VALID) { 53 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 54 | 55 | exit(-1); 56 | } 57 | 58 | const auto &s = mvParams[0]; 59 | assert(s >= 0 && s < params.size()); 60 | 61 | if (s >= params.size()) { 62 | LOG(ERROR) << "The parameter must be valid." << std::endl; 63 | 64 | exit(-1); 65 | } 66 | 67 | lin.jacobians[3].resize(mvParams.size()); 68 | 69 | lin.jacobians[3][0] = mSqrtCov; 70 | 71 | lin.status = Status::VALID; 72 | 73 | return 0; 74 | } 75 | 76 | } // namespace scope 77 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/JointLimitFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | class JointLimitFactor : public Factor { 8 | public: 9 | struct Evaluation : public Factor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | AlignedVector> outputs; 13 | AlignedVector> fin; 14 | AlignedVector> fexp[2]; 15 | AlignedVector> flog; 16 | AlignedVector> selected; 17 | 18 | virtual int clear() override; 19 | }; 20 | 21 | struct Linearization : public Factor::Linearization { 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | Matrix63 jacobian; 25 | 26 | AlignedVector derivatives; 27 | AlignedVector slopes; 28 | 29 | virtual int clear() override; 30 | }; 31 | 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | SCOAT_INITIAL_EVALUATION_NEW 35 | SCOAT_INITIAL_LINEARIZATION_NEW 36 | 37 | JointLimitFactor(int joint, const AlignedVector &NNWeight, 38 | const AlignedVector &NNBias, const VectorX &a, 39 | const VectorX &b, Scalar scale = 1.0, 40 | const std::string &name = "", int index = -1); 41 | 42 | virtual int evaluate(const AlignedVector &poses, 43 | const AlignedVector &joints, 44 | Factor::Evaluation &base_eval) const override; 45 | 46 | virtual int linearize(const AlignedVector &poses, 47 | const AlignedVector &joints, 48 | const Factor::Evaluation &base_eval, 49 | Factor::Linearization &base_lin) const override; 50 | 51 | int getJoint() const { return mJoint; } 52 | 53 | protected: 54 | int mJoint; 55 | 56 | int mnLayers; 57 | int mnInnerLayers; 58 | 59 | // weights for linear transformation 60 | AlignedVector NNWeight; 61 | // biases for linear transformation 62 | AlignedVector NNBias; 63 | // parameters for PRelu 64 | VectorX NNPReLUScale[2]; 65 | VectorX NNPReLURate; 66 | 67 | protected: 68 | int evaluate(const Matrix3 &Omega, Evaluation &eval) const; 69 | 70 | int linearize(const Matrix3 &Omega, const Evaluation &eval, 71 | Linearization &lin) const; 72 | }; 73 | } // namespace Initializer 74 | } // namespace scope 75 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/Factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define SCOAT_INITIAL_EVALUATION_NEW \ 8 | virtual std::shared_ptr newEvaluation() \ 9 | const override { \ 10 | return std::make_shared(); \ 11 | } 12 | 13 | #define SCOAT_INITIAL_LINEARIZATION_NEW \ 14 | virtual std::shared_ptr \ 15 | newLinearization() const override { \ 16 | return std::make_shared(); \ 17 | } 18 | 19 | namespace scope { 20 | namespace Initializer { 21 | class Factor : public std::enable_shared_from_this { 22 | protected: 23 | std::string mName; 24 | 25 | int mIndex; 26 | 27 | public: 28 | enum class Status { INVALID = -1, VALID = 0 }; 29 | 30 | struct Evaluation : std::enable_shared_from_this { 31 | Status status; 32 | 33 | VectorX error; 34 | 35 | Scalar f; 36 | 37 | Evaluation(); 38 | 39 | virtual int reset(); 40 | 41 | virtual int clear(); 42 | }; 43 | 44 | struct Linearization : std::enable_shared_from_this { 45 | Status status; 46 | 47 | Linearization(); 48 | 49 | virtual int reset(); 50 | 51 | virtual int clear(); 52 | }; 53 | 54 | public: 55 | Factor(const std::string &name = "", int index = -1); 56 | 57 | virtual int evaluate(const AlignedVector &poses, 58 | const AlignedVector &joints, 59 | Evaluation &eval) const = 0; 60 | 61 | virtual int linearize(const AlignedVector &poses, 62 | const AlignedVector &joints, 63 | const Evaluation &eval, Linearization &lin) const = 0; 64 | 65 | virtual std::shared_ptr newEvaluation() const { 66 | return std::make_shared(); 67 | } 68 | 69 | virtual std::shared_ptr newLinearization() const { 70 | return std::make_shared(); 71 | } 72 | 73 | void setID(int index) { mIndex = index; } 74 | 75 | int getID() const { return mIndex; } 76 | 77 | const std::string &getName() const { return mName; } 78 | }; 79 | } // namespace Initializer 80 | } // namespace scope 81 | -------------------------------------------------------------------------------- /C++/scope/factor/FullVertexExtraPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class FullVertexExtraPinholeCameraFactor 7 | : public VertexExtraPinholeCameraFactor { 8 | public: 9 | struct Evaluation : public VertexExtraPinholeCameraFactor::Evaluation { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | Vector2 camMeasurement; 13 | }; 14 | 15 | struct Linearization : public VertexExtraPinholeCameraFactor::Linearization { 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | Vector4 gCam; 19 | }; 20 | 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | SCOAT_FACTOR_EVALUATION_NEW 24 | SCOAT_FACTOR_LINEARIZATION_NEW 25 | 26 | FullVertexExtraPinholeCameraFactor(int pose, int vParams, int camParams, 27 | const Matrix3X &vDirs, const Vector3 &v, 28 | const Scalar &sigma, const Scalar &eps, 29 | const Pose &extraCamPose, 30 | const Vector2 &measurement, 31 | const Scalar &confidence = 1.0, 32 | const std::string &name = "", 33 | int index = -1, bool active = true); 34 | 35 | virtual int evaluate(const AlignedVector &poses, 36 | const AlignedVector &shapes, 37 | const AlignedVector &joints, 38 | const AlignedVector ¶ms, 39 | Factor::Evaluation &base_eval) const override; 40 | 41 | virtual int linearize(const AlignedVector &poses, 42 | const AlignedVector &shapes, 43 | const AlignedVector &joints, 44 | const AlignedVector ¶ms, 45 | const Factor::Evaluation &base_eval, 46 | Factor::Linearization &base_lin) const override; 47 | 48 | protected: 49 | virtual int evaluate(const Pose &pose, const VectorX &vertexParam, 50 | const VectorX &camParam, const Vector2 &measurement, 51 | Evaluation &eval) const; 52 | 53 | virtual int linearize(const Pose &pose, const VectorX &vertexParam, 54 | const VectorX &camParam, const Vector2 &measurement, 55 | const Evaluation &eval, Linearization &lin) const; 56 | }; 57 | } // namespace scope 58 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/JointFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | JointFactor::JointFactor(int joint, const Matrix3 &sqrtCov, const Matrix3 &mean, 9 | const std::string &name, int index) 10 | : Factor(name, index), mJoint(joint), mSqrtCov(sqrtCov), mMean(mean) { 11 | assert((mMean.col(0).cross(mMean.col(1)) - mMean.col(2)).norm() < 1e-5 && 12 | "mean.R must be a rotational matrix"); 13 | } 14 | 15 | int JointFactor::evaluate(const AlignedVector &poses, 16 | const AlignedVector &joints, 17 | Factor::Evaluation &base_eval) const { 18 | auto &eval = dynamic_cast(base_eval); 19 | eval.clear(); 20 | 21 | const auto &i = mJoint; 22 | 23 | assert(i >= 0 && i < joints.size()); 24 | 25 | if (i >= joints.size()) { 26 | LOG(ERROR) << "The pose must be valid." << std::endl; 27 | 28 | exit(-1); 29 | } 30 | 31 | const auto &R = joints[i]; 32 | 33 | eval.errorR.noalias() = mMean.transpose() * R; 34 | 35 | math::SO3::log(eval.errorR, eval.xi); 36 | eval.error.noalias() = mSqrtCov * eval.xi; 37 | 38 | eval.f = eval.error.squaredNorm(); 39 | 40 | eval.status = Status::VALID; 41 | 42 | return 0; 43 | } 44 | 45 | int JointFactor::linearize(const AlignedVector &poses, 46 | const AlignedVector &joints, 47 | const Factor::Evaluation &base_eval, 48 | Factor::Linearization &base_lin) const { 49 | auto &eval = dynamic_cast(base_eval); 50 | auto &lin = dynamic_cast(base_lin); 51 | 52 | lin.clear(); 53 | 54 | const auto &i = mJoint; 55 | 56 | assert(i >= 0 && i < joints.size()); 57 | 58 | if (i >= joints.size()) { 59 | LOG(ERROR) << "The pose must be valid." << std::endl; 60 | 61 | exit(-1); 62 | } 63 | 64 | assert(eval.status == Status::VALID); 65 | 66 | if (eval.status != Status::VALID) { 67 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 68 | 69 | exit(-1); 70 | } 71 | 72 | auto &J = lin.jacobian; 73 | 74 | math::SO3::dexpinv(eval.xi, lin.dexpinv); 75 | lin.dexpinvR.noalias() = lin.dexpinv * mMean.transpose(); 76 | 77 | J.noalias() = mSqrtCov * lin.dexpinvR; 78 | 79 | lin.status = Status::VALID; 80 | 81 | return 0; 82 | } 83 | } // namespace Initializer 84 | } // namespace scope 85 | -------------------------------------------------------------------------------- /C++/scope/utils/Initialization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace scope { 8 | namespace Initialization { 9 | namespace SMPL { 10 | int initialize(const MatrixX &ShapeSqrtCov, Scalar FocalLength, 11 | // KeyPoint2D setup 12 | const Matrix3X &Measurements2D, Scalar KeyPoint2DGMSigma, 13 | Scalar KeyPoint2DGMEps, 14 | const VectorX &KeyPoint2DConfidenceThreshold, 15 | // POF setup 16 | const Matrix3X &POFMeasurements, Scalar POFGMSigma, 17 | Scalar POFGMEps, 18 | // KeyPoint3D setup 19 | const Matrix3X &Measurements3D, Scalar KeyPoint3DGMSigma, 20 | Scalar KeyPoint3DGMEps, 21 | // joint limit 22 | const AlignedVector> &JointLimitWeight, 23 | const AlignedVector> &JointLimitBias, 24 | const AlignedVector &JointLimitPRelua, 25 | const AlignedVector &JointLimitPRelub, 26 | const Vector<23> &JointLimitScale, 27 | // initial estimates 28 | Pose &TorsoPose, // torso pose 29 | Matrix3 &SpineJoint, // spine joint 30 | AlignedVector &LeftArmJoint, 31 | AlignedVector &RightArmJoint, 32 | AlignedVector &LeftLegJoint, 33 | AlignedVector &RightLegJoint, VectorX &betas); 34 | 35 | int initializeRootPose(const AlignedVector &Poses, const VectorX &Param, 36 | // SMPL model info 37 | const std::vector &kintree, const MatrixX &JDirs, 38 | const VectorX &J, const MatrixX &KeyPointDirs, 39 | const VectorX &KeyPoints, 40 | // KeyPoint2D setup 41 | Scalar FocalLength, const Matrix3X &Measurements2D, 42 | Scalar KeyPoint2DGMSigma, Scalar KeyPoint2DGMEps, 43 | const VectorX &KeyPoint2DConfidenceThreshold, 44 | // POF setup 45 | const Matrix3X &POFMeasurements, Scalar POFGMSigma, 46 | Scalar POFGMEps, 47 | // initial estimates 48 | Pose &RootPose); 49 | } // namespace SMPL 50 | } // namespace Initialization 51 | } // namespace scope 52 | -------------------------------------------------------------------------------- /C++/scope/factor/VertexExtraPinholeCameraFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | class VertexExtraPinholeCameraFactor : public VertexPinholeCameraFactor, 8 | public ExtraPinholeCamera { 9 | public: 10 | struct Evaluation : public VertexPinholeCameraFactor::Evaluation, 11 | public ExtraPinholeCamera::Evaluation { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | }; 14 | 15 | struct Linearization : public VertexPinholeCameraFactor::Linearization, 16 | public ExtraPinholeCamera::Linearization { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Vector3 gPoseR; 20 | Vector3 gJoint; 21 | VectorX gParam; 22 | }; 23 | 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | SCOAT_FACTOR_EVALUATION_NEW 27 | SCOAT_FACTOR_LINEARIZATION_NEW 28 | 29 | VertexExtraPinholeCameraFactor(int pose, int vParam, const Matrix3X &vDirs, 30 | const Vector3 &v, const Scalar &sigma, 31 | const Scalar &eps, const Pose &extraCamPose, 32 | const Vector2 &measurement, 33 | const Scalar &confidence = 1.0, 34 | const std::string &name = "", int index = -1, 35 | bool active = true); 36 | 37 | virtual int evaluate(const AlignedVector &poses, 38 | const AlignedVector &shapes, 39 | const AlignedVector &joints, 40 | const AlignedVector ¶ms, 41 | Factor::Evaluation &base_eval) const override; 42 | 43 | virtual int linearize(const AlignedVector &poses, 44 | const AlignedVector &shapes, 45 | const AlignedVector &joints, 46 | const AlignedVector ¶ms, 47 | const Factor::Evaluation &base_eval, 48 | Factor::Linearization &base_lin) const override; 49 | 50 | protected: 51 | virtual int evaluate(const Pose &pose, const VectorX &vertexParam, 52 | const Vector2 &measurement, Evaluation &eval) const; 53 | 54 | virtual int linearize(const Pose &pose, const VectorX &vertexParam, 55 | const Vector2 &measurement, const Evaluation &eval, 56 | Linearization &lin) const; 57 | }; 58 | } // namespace scope 59 | -------------------------------------------------------------------------------- /C++/scope/factor/JointLimitFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace scope { 6 | class JointLimitFactor : public Factor { 7 | public: 8 | struct Evaluation : public Factor::Evaluation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | AlignedVector> outputs; 12 | AlignedVector> fin; 13 | AlignedVector> fexp[2]; 14 | AlignedVector> flog; 15 | AlignedVector> selected; 16 | 17 | virtual int clear() override; 18 | }; 19 | 20 | struct Linearization : public Factor::Linearization { 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | AlignedVector derivatives; 24 | AlignedVector slopes; 25 | 26 | virtual int clear() override; 27 | }; 28 | 29 | public: 30 | SCOAT_FACTOR_EVALUATION_NEW 31 | SCOAT_FACTOR_LINEARIZATION_NEW 32 | 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | 35 | JointLimitFactor(int joint, const AlignedVector &NNWeight, 36 | const AlignedVector &NNBias, const VectorX &a, 37 | const VectorX &b, Scalar scale = 1.0, 38 | const std::string &name = "", int index = -1, 39 | bool active = true); 40 | 41 | virtual int evaluate(const AlignedVector &poses, 42 | const AlignedVector &shapes, 43 | const AlignedVector &joints, 44 | const AlignedVector ¶ms, 45 | Factor::Evaluation &base_eval) const override; 46 | 47 | virtual int linearize(const AlignedVector &poses, 48 | const AlignedVector &shapes, 49 | const AlignedVector &joints, 50 | const AlignedVector ¶ms, 51 | const Factor::Evaluation &base_eval, 52 | Factor::Linearization &base_lin) const override; 53 | 54 | protected: 55 | int mnLayers; 56 | int mnInnerLayers; 57 | 58 | // weights for linear transformation 59 | AlignedVector NNWeight; 60 | // biases for linear transformation 61 | AlignedVector NNBias; 62 | // parameters for PRelu 63 | VectorX NNPReLUScale[2]; 64 | VectorX NNPReLURate; 65 | 66 | protected: 67 | virtual int evaluate(const Matrix3 &Omega, Evaluation &eval) const; 68 | 69 | virtual int linearize(const Matrix3 &Omega, const Evaluation &eval, 70 | Linearization &lin) const; 71 | }; 72 | } // namespace scope 73 | -------------------------------------------------------------------------------- /C++/scope/optimizer/SMPLify.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Optimizer { 8 | template 9 | class SMPLify 10 | : public Optimizer::NumJoints, P, SMPL

::NumVertices, CamOpt> { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | using Model = SMPL

; 15 | using Optimization = 16 | Optimizer; 17 | 18 | using Optimization::computeGradient; 19 | using Optimization::linearize; 20 | using Optimization::solveGaussNewton; 21 | 22 | SMPLify(const Model &smpl, const Options &options); 23 | 24 | virtual int solveGaussNewton() const override; 25 | 26 | protected: 27 | std::shared_ptr mSMPL; 28 | 29 | protected: 30 | virtual int setupParameterInfo() override; 31 | 32 | private: 33 | using Optimization::CamParamIndex; 34 | using Optimization::CamParamOffset; 35 | using Optimization::CamParamSize; 36 | using Optimization::DCamParamOffset; 37 | using Optimization::DFaceParamOffset; 38 | using Optimization::DJointOffset; 39 | using Optimization::DJointSize; 40 | using Optimization::DParamOffset; 41 | using Optimization::DParamSize; 42 | using Optimization::DPoseOffset; 43 | using Optimization::DPoseSize; 44 | using Optimization::DSize; 45 | using Optimization::DVertexParamOffset; 46 | using Optimization::FaceParamIndex; 47 | using Optimization::FaceParamOffset; 48 | using Optimization::FaceParamSize; 49 | using Optimization::MaxDSize; 50 | using Optimization::MaxNumCollisions; 51 | using Optimization::NumJoints; 52 | using Optimization::NumParams; 53 | using Optimization::NumPoses; 54 | using Optimization::NumShapes; 55 | using Optimization::ParamSize; 56 | using Optimization::Size; 57 | using Optimization::VertexParamIndex; 58 | using Optimization::VertexParamOffset; 59 | using Optimization::VertexParamSize; 60 | 61 | using Optimization::mvB; 62 | using Optimization::mvBp; 63 | using Optimization::mvBu; 64 | using Optimization::mvH; 65 | using Optimization::mvHuuInv; 66 | using Optimization::mvHxB; 67 | using Optimization::mvKuxp; 68 | using Optimization::mvM; 69 | 70 | using Optimization::mvE; 71 | using Optimization::mvhp; 72 | using Optimization::mvhu; 73 | using Optimization::mvhx; 74 | using Optimization::mvku; 75 | using Optimization::mvmp; 76 | using Optimization::mvmu; 77 | using Optimization::mvmx; 78 | 79 | using Optimization::mParamGN; 80 | using Optimization::mvJointGN; 81 | using Optimization::mvPoseGN; 82 | 83 | using Optimization::mDLambda; 84 | using Optimization::mLambda; 85 | using Optimization::mOptions; 86 | }; 87 | } // namespace Optimizer 88 | } // namespace scope 89 | -------------------------------------------------------------------------------- /C++/scope/factor/JointFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | JointFactor::JointFactor(int joint, const Matrix3 &sqrtCov, const Matrix3 &mean, 8 | const std::string &name, int index, bool active) 9 | : Factor({}, {}, {joint}, {}, name, index, active), 10 | mSqrtCov(sqrtCov), 11 | mMean(mean) { 12 | assert((mean.col(0).cross(mean.col(1)) - mean.col(2)).norm() < 1e-5 && 13 | "mean must be a rotational matrix"); 14 | } 15 | 16 | int JointFactor::evaluate(const AlignedVector &poses, 17 | const AlignedVector &shapes, 18 | const AlignedVector &joints, 19 | const AlignedVector ¶ms, 20 | Factor::Evaluation &base_eval) const { 21 | auto &eval = dynamic_cast(base_eval); 22 | eval.clear(); 23 | 24 | const auto &i = mvJoints[0]; 25 | assert(i >= 0 && i < joints.size()); 26 | 27 | if (i >= joints.size()) { 28 | LOG(ERROR) << "The joint must be valid." << std::endl; 29 | 30 | exit(-1); 31 | } 32 | 33 | const auto &R = joints[i]; 34 | 35 | eval.errorR.noalias() = mMean.transpose() * R; 36 | 37 | math::SO3::log(eval.errorR, eval.xi); 38 | eval.error.noalias() = mSqrtCov * eval.xi; 39 | 40 | eval.f = eval.error.squaredNorm(); 41 | 42 | eval.status = Status::VALID; 43 | 44 | return 0; 45 | } 46 | 47 | int JointFactor::linearize(const AlignedVector &poses, 48 | const AlignedVector &shapes, 49 | const AlignedVector &joints, 50 | const AlignedVector ¶ms, 51 | const Factor::Evaluation &base_eval, 52 | Factor::Linearization &base_lin) const { 53 | auto &eval = dynamic_cast(base_eval); 54 | auto &lin = dynamic_cast(base_lin); 55 | 56 | lin.clear(); 57 | 58 | assert(eval.status == Status::VALID); 59 | 60 | if (eval.status != Status::VALID) { 61 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 62 | 63 | exit(-1); 64 | } 65 | 66 | const auto &i = mvJoints[0]; 67 | assert(i >= 0 && i < joints.size()); 68 | 69 | if (i < 0 || i >= joints.size()) { 70 | LOG(ERROR) << "The joint must be valid." << std::endl; 71 | 72 | exit(-1); 73 | } 74 | 75 | lin.jacobians[2].resize(1); 76 | lin.jacobians[2][0].resize(3, 3); 77 | Eigen::Map J(lin.jacobians[2][0].data()); 78 | 79 | math::SO3::dexpinv(eval.xi, lin.dexpinv); 80 | lin.dexpinvR.noalias() = lin.dexpinv * mMean.transpose(); 81 | 82 | J.noalias() = mSqrtCov * lin.dexpinvR; 83 | 84 | lin.status = Status::VALID; 85 | 86 | return 0; 87 | } 88 | } // namespace scope 89 | -------------------------------------------------------------------------------- /C++/scope/base/Pose.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | class Pose { 8 | public: 9 | Pose() : R(matrix.data()), t(matrix.data() + 9) { 10 | R.setIdentity(); 11 | t.setZero(); 12 | } 13 | 14 | Pose(const Pose& T) 15 | : matrix(T.matrix), R(matrix.data()), t(matrix.data() + 9) {} 16 | 17 | Pose(const Matrix3& R, const Vector3& t): Pose() { 18 | this->R = R; 19 | this->t = t; 20 | } 21 | 22 | Pose& operator=(const Pose& T) { 23 | matrix = T.matrix; 24 | 25 | return *this; 26 | } 27 | 28 | Pose(Pose&& T) : matrix(T.matrix), R(std::move(T.R)), t(std::move(T.t)) {} 29 | 30 | Pose& operator=(Pose&& T) { 31 | matrix = std::move(T.matrix); 32 | R = std::move(T.R); 33 | t = std::move(T.t); 34 | 35 | return *this; 36 | } 37 | 38 | static const Pose& Identity() { 39 | static Pose identity; 40 | 41 | return identity; 42 | } 43 | 44 | template 45 | static int exp(Eigen::MatrixBase const& xi, Pose& res) { 46 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T, 6) 47 | const auto& w = xi.template head<3>(); 48 | const auto& v = xi.template tail<3>(); 49 | 50 | // TODO: Optimize the code 51 | const Scalar th = w.stableNorm(); 52 | Vector3 s = w; 53 | 54 | if (th < 1e-2) { 55 | Scalar c, d, th2, th4, th6, d2, thd2; 56 | th2 = th * th; 57 | th4 = th2 * th2; 58 | th6 = th4 * th2; 59 | d = 1 + th2 / 12 + th4 / 120 + th6 / 1185.88235294117647058823529412; 60 | 61 | d2 = d * d; 62 | thd2 = th2 * d2; 63 | c = 2 / (4 + thd2); 64 | 65 | s *= c * d2; 66 | 67 | res.R.noalias() = s * w.transpose(); 68 | res.R.diagonal().array() += 1 - c * thd2; 69 | 70 | s *= 2 / d; 71 | 72 | res.t = v; 73 | res.t += 0.1666666666666666666666666666667 * w.dot(v) * w; 74 | res.t += 0.5 * w.cross(v); 75 | } else { 76 | Scalar sth, cth; 77 | 78 | math::SO3::fsincos(th, &sth, &cth); 79 | 80 | s /= th; 81 | 82 | res.R.noalias() = (1 - cth) * s * s.transpose(); 83 | res.R.diagonal().array() += cth; 84 | 85 | s *= sth; 86 | 87 | res.t = sth * v; 88 | res.t += (th - sth) * w.dot(v) * w; 89 | res.t += (1 - cth) * w.cross(v); 90 | res.t /= th; 91 | } 92 | 93 | res.R(0, 1) -= s[2]; 94 | res.R(1, 0) += s[2]; 95 | res.R(0, 2) += s[1]; 96 | res.R(2, 0) -= s[1]; 97 | res.R(1, 2) -= s[0]; 98 | res.R(2, 1) += s[0]; 99 | 100 | return 0; 101 | } 102 | 103 | public: 104 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 105 | 106 | Eigen::Matrix matrix; 107 | Eigen::Map R; // rotational part 108 | Eigen::Map t; // translational part 109 | }; 110 | } // namespace scope 111 | -------------------------------------------------------------------------------- /C++/scope/initializer/ArmRefiner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | ArmRefiner::ArmRefiner(const Options &options, 9 | const std::array &RelJointLocations) 10 | : Initializer(2, options), mRelJointLocations(RelJointLocations) {} 11 | 12 | int ArmRefiner::FKintree(int n) const { 13 | auto &poses = mvPoses[n]; 14 | auto &joints = mvJoints[n]; 15 | 16 | poses[1].R.noalias() = poses[0].R * joints[0]; 17 | poses[1].t = poses[0].t; 18 | poses[1].t.noalias() += poses[0].R * mRelJointLocations[0]; 19 | 20 | poses[2].R.noalias() = poses[1].R * joints[1]; 21 | poses[2].t = poses[1].t; 22 | poses[2].t.noalias() += poses[1].R * mRelJointLocations[1]; 23 | 24 | return 0; 25 | } 26 | 27 | int ArmRefiner::DFKintree() const { 28 | const auto &poses = mvPoses[0]; 29 | 30 | mvB[0].topRows<3>() = poses[0].R; 31 | mvB[0].block<3, 1>(3, 0) = poses[1].t.cross(poses[0].R.col(0)); 32 | mvB[0].block<3, 1>(3, 1) = poses[1].t.cross(poses[0].R.col(1)); 33 | mvB[0].block<3, 1>(3, 2) = poses[1].t.cross(poses[0].R.col(2)); 34 | 35 | mvB[1].topRows<3>() = poses[1].R; 36 | mvB[1].block<3, 1>(3, 0) = poses[2].t.cross(poses[1].R.col(0)); 37 | mvB[1].block<3, 1>(3, 1) = poses[2].t.cross(poses[1].R.col(1)); 38 | mvB[1].block<3, 1>(3, 2) = poses[2].t.cross(poses[1].R.col(2)); 39 | 40 | return 0; 41 | } 42 | 43 | int ArmRefiner::updateGaussNewton() const { 44 | Matrix6 temp1; 45 | Matrix63 temp2; 46 | 47 | temp1 = mvMxx[1] + mvMxx[2]; 48 | temp2.noalias() = temp1 * mvB[0]; 49 | 50 | mH.resize(6, 6); 51 | mH.topLeftCorner<3, 3>().noalias() = mvB[0].transpose() * temp2; 52 | 53 | temp2.noalias() = mvMxx[2] * mvB[1]; 54 | 55 | mH.topRightCorner<3, 3>().noalias() = mvB[0].transpose() * temp2; 56 | mH.bottomLeftCorner<3, 3>().noalias() = mH.topRightCorner<3, 3>().transpose(); 57 | 58 | mH.bottomRightCorner<3, 3>().noalias() = mvB[1].transpose() * temp2; 59 | 60 | mH.topLeftCorner<3, 3>().noalias() += mvMuu[0]; 61 | mH.bottomRightCorner<3, 3>().noalias() += mvMuu[1]; 62 | 63 | Vector6 temp3; 64 | temp3 = mvmx[1] + mvmx[2]; 65 | 66 | mh.resize(6); 67 | mh.head<3>() = mvmu[0]; 68 | mh.tail<3>() = mvmu[1]; 69 | 70 | mh.head<3>().noalias() += mvB[0].transpose() * temp3; 71 | mh.tail<3>().noalias() += mvB[1].transpose() * mvmx[2]; 72 | 73 | return 0; 74 | } 75 | 76 | int ArmRefiner::update(Scalar stepsize) const { 77 | assert(stepsize > 0); 78 | 79 | mDJointChange = mhGN * stepsize; 80 | 81 | math::SO3::exp(mDJointChange.head<3>(), mvJointChange[0]); 82 | math::SO3::exp(mDJointChange.tail<3>(), mvJointChange[1]); 83 | 84 | mvJoints[1][0].noalias() = mvJointChange[0] * mvJoints[0][0]; 85 | mvJoints[1][1].noalias() = mvJointChange[1] * mvJoints[0][1]; 86 | 87 | FKintree(1); 88 | 89 | return 0; 90 | } 91 | 92 | } // namespace Initializer 93 | } // namespace scope 94 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Reviatalizing Optimization for 3D Human Pose and Shape Estimation: A Sparse Constrained Formulation 2 | 3 | This is the implementation of the approach described in the paper: 4 | > Taosha Fan, Kalyan Vasudev Alwala, Donglai Xiang, Weipeng Xu, Todd Murphey, Mustafa Mukadam. [Reviatalizing Optimization for 3D Human Pose and Shape Estimation: A Sparse Constrained Formulation](https://arxiv.org/abs/2105.13965). In IEEE/CVF International Conference on Computer Vision (ICCV), 2021. 5 | 6 | More demos are available at https://sites.google.com/view/scope-human 7 | 8 |

9 | 10 | ## Install 11 | ```shell 12 | git clone https://github.com/fantaosha/SCOPE.git 13 | cd SCOPE 14 | export SCOPE_ROOT=$(pwd) 15 | mkdir build 16 | cd build 17 | cmake ../C++ 18 | make -j4 19 | ``` 20 | 21 | ## Usage 22 | 23 | ### SMPL Model 24 | Download the [SMPL model](https://smpl.is.tue.mpg.de/) and extract these```.pkl``` files to [```$SCOPE_ROOT/model```](model). 25 | 26 | 27 | ### Preprocess 28 | ```shell 29 | cd $SCOPE_ROOT/model 30 | python3 preprocess.py smpl_male.npz YOUR_SMPL_MALE.pkl 31 | ``` 32 | 33 | ### Run 34 | ```shell 35 | cd $SCOPE_ROOT 36 | ./build/bin/run --model ./model/smpl_male.npz --prior ./model/joint_prior.json --keypoint ./examples/keypoints.json --result ./examples/results.json 37 | ``` 38 | 39 | ## Dataset 40 | ### 2D and 3D Keypoints 41 | The 2D and 3D keypoints estimates from [AlphaPose](https://github.com/MVIG-SJTU/AlphaPose) and [VideoPose3D](https://github.com/facebookresearch/VideoPose3D) can be downloaded from [Google Drive](https://drive.google.com/drive/folders/1DvRE-G-74vjmQiDXDCgndF-xXvENK5ZH?usp=sharing). 42 | 43 | 44 | ### 2D Keypoint Index 45 | ```shell 46 | 0: nose 47 | 1: left eye 48 | 2: right eye 49 | 3: left ear 50 | 4: right ear 51 | 5: left upper arm 52 | 6: right upper arm 53 | 7: left elbow 54 | 8: right elow 55 | 9: left wrist 56 | 10: right wrist 57 | 11: left hip 58 | 12: right hip 59 | 13: left knee 60 | 14: right knee 61 | 15: left ankle 62 | 16: right ankle 63 | 17: head top 64 | 18: thorax 65 | 19: middle hip 66 | 20: left big toe 67 | 21: right big toe 68 | 22: left small toe 69 | 23: right small toe 70 | 24: left heel 71 | 25: right heel 72 | 26: chest 73 | 27: neck 74 | ``` 75 | 76 | ### 3D Keypoint Index 77 | ```shell 78 | 0: middile hip 79 | 1: left hip 80 | 2: left knee 81 | 3: left ankle 82 | 4: right hip 83 | 5: right knee 84 | 6: right ankle 85 | 7: chest 86 | 8: thorax 87 | 9: neck 88 | 10: head top 89 | 11: left upper arm 90 | 12: left elbow 91 | 13: left wrist 92 | 14: right upper arm 93 | 15: right elbow 94 | 16: right wrist 95 | ``` 96 | 97 | ## Citation 98 | ``` 99 | @article{fan2021revitalizing, 100 | title={Revitalizing Optimization for 3D Human Pose and Shape Estimation: A Sparse Constrained Formulation}, 101 | author={Fan, Taosha and Alwala, Kalyan Vasudev and Xiang, Donglai and Xu, Weipeng and Murphey, Todd and Mukadam, Mustafa}, 102 | journal={Proceedings of the IEEE/CVF International Conference on Computer Vision}, 103 | year={2021} 104 | } 105 | ``` 106 | 107 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/PoseFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | PoseFactor::PoseFactor(int pose, const Matrix6 &sqrtCov, const Pose &mean, 9 | const std::string &name, int index) 10 | : Factor(name, index), mPose(pose), mSqrtCov(sqrtCov), mMean(mean) { 11 | assert((mean.R.col(0).cross(mean.R.col(1)) - mean.R.col(2)).norm() < 1e-5 && 12 | "mean.R must be a rotational matrix"); 13 | } 14 | 15 | int PoseFactor::evaluate(const AlignedVector &poses, 16 | const AlignedVector &joints, 17 | Factor::Evaluation &base_eval) const { 18 | auto &eval = dynamic_cast(base_eval); 19 | eval.clear(); 20 | 21 | const auto &i = mPose; 22 | 23 | assert(i >= 0 && i < poses.size()); 24 | 25 | if (i >= poses.size()) { 26 | LOG(ERROR) << "The pose must be valid." << std::endl; 27 | 28 | exit(-1); 29 | } 30 | 31 | auto &pose = poses[i]; 32 | 33 | eval.errorR.noalias() = mMean.R.transpose() * pose.R; 34 | 35 | eval.error.resize(6); 36 | 37 | Eigen::Map omega(eval.xi.data()); 38 | math::SO3::log(eval.errorR, omega); 39 | eval.xi.tail<3>() = pose.t - mMean.t; 40 | eval.error.noalias() = mSqrtCov * eval.xi; 41 | 42 | eval.f = eval.error.squaredNorm(); 43 | 44 | eval.status = Status::VALID; 45 | 46 | return 0; 47 | } 48 | 49 | int PoseFactor::linearize(const AlignedVector &poses, 50 | const AlignedVector &joints, 51 | const Factor::Evaluation &base_eval, 52 | Factor::Linearization &base_lin) const { 53 | auto &eval = dynamic_cast(base_eval); 54 | auto &lin = dynamic_cast(base_lin); 55 | 56 | lin.clear(); 57 | 58 | const auto &i = mPose; 59 | 60 | assert(i >= 0 && i < poses.size()); 61 | 62 | if (i >= poses.size()) { 63 | LOG(ERROR) << "The pose must be valid." << std::endl; 64 | 65 | exit(-1); 66 | } 67 | 68 | assert(eval.status == Status::VALID); 69 | 70 | if (eval.status != Status::VALID) { 71 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 72 | 73 | exit(-1); 74 | } 75 | 76 | auto &pose = poses[i]; 77 | 78 | auto &J = lin.jacobian; 79 | 80 | math::SO3::dexpinv(eval.xi.head<3>(), lin.dexpinv); 81 | lin.dexpinvR.noalias() = lin.dexpinv * mMean.R.transpose(); 82 | 83 | J.topRightCorner<3, 3>().setZero(); 84 | 85 | const auto &t = pose.t; 86 | 87 | Eigen::Map JR(J.data()); 88 | Eigen::Map Jt(J.data() + 18); 89 | 90 | Jt = mSqrtCov.rightCols<3>(); 91 | 92 | JR.row(0).noalias() = t.cross(Jt.row(0)); 93 | JR.row(1).noalias() = t.cross(Jt.row(1)); 94 | JR.row(2).noalias() = t.cross(Jt.row(2)); 95 | JR.row(3).noalias() = t.cross(Jt.row(3)); 96 | JR.row(4).noalias() = t.cross(Jt.row(4)); 97 | JR.row(5).noalias() = t.cross(Jt.row(5)); 98 | JR.noalias() += mSqrtCov.leftCols<3>() * lin.dexpinvR; 99 | 100 | lin.status = Status::VALID; 101 | 102 | return 0; 103 | } 104 | } // namespace Initializer 105 | } // namespace scope 106 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/POFFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | POFFactor::POFFactor(int pose, const Vector3 &s, const Scalar &sigma, 8 | const Scalar &eps, const Vector3 &measurement, 9 | const Scalar &confidence, const std::string &name, 10 | int index) 11 | : Factor(name, index), mPose(pose), mMeasurement(measurement), 12 | mSigma(sigma), mCon(confidence), mSqrtCon(std::sqrt(confidence)), 13 | mSigmaCon(confidence * sigma), mMinAlpha(eps), mS(s) { 14 | mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma; 15 | mMeasurement.stableNormalize(); 16 | mS.stableNormalize(); 17 | } 18 | 19 | int POFFactor::evaluate(const AlignedVector &poses, 20 | const AlignedVector &joints, 21 | Factor::Evaluation &base_eval) const { 22 | auto &eval = dynamic_cast(base_eval); 23 | eval.clear(); 24 | 25 | const auto &i = mPose; 26 | 27 | assert(i >= 0 && i < poses.size()); 28 | 29 | if (i >= poses.size()) { 30 | LOG(ERROR) << "The pose must be valid." << std::endl; 31 | 32 | exit(-1); 33 | } 34 | 35 | auto &pose = poses[i]; 36 | 37 | eval.error.resize(1); 38 | 39 | eval.d.noalias() = pose.R * mS; 40 | eval.error[0] = 1 - mMeasurement.dot(eval.d); 41 | eval.squaredErrorNorm = eval.error.squaredNorm(); 42 | 43 | #if 0 44 | eval.f = mCon * eval.squaredErrorNorm; 45 | #else 46 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 47 | #endif 48 | 49 | eval.status = Status::VALID; 50 | 51 | return 0; 52 | } 53 | 54 | int POFFactor::linearize(const AlignedVector &poses, 55 | const AlignedVector &joints, 56 | const Factor::Evaluation &base_eval, 57 | Factor::Linearization &base_lin) const { 58 | auto &eval = dynamic_cast(base_eval); 59 | auto &lin = dynamic_cast(base_lin); 60 | 61 | lin.clear(); 62 | 63 | const auto &i = mPose; 64 | 65 | assert(i >= 0 && i < poses.size()); 66 | 67 | if (i >= poses.size()) { 68 | LOG(ERROR) << "The pose must be valid." << std::endl; 69 | 70 | exit(-1); 71 | } 72 | 73 | assert(eval.status == Status::VALID); 74 | 75 | if (eval.status != Status::VALID) { 76 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 77 | 78 | exit(-1); 79 | } 80 | 81 | lin.jacobian.setZero(); 82 | 83 | Eigen::Map JR(lin.jacobian.data()); 84 | 85 | JR.noalias() = mMeasurement.cross(eval.d); 86 | 87 | #if 0 88 | JR *= mSqrtCon; 89 | lin.scaledError = mSqrtCon * eval.error[0]; 90 | #else 91 | lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm); 92 | lin.alpha = eval.squaredErrorNorm > mGMThreshold 93 | ? mMinAlpha 94 | : std::sqrt(1 - 4 * eval.squaredErrorNorm / 95 | (mSigma + eval.squaredErrorNorm)); 96 | 97 | lin.sqrtDrho *= mSqrtCon; 98 | lin.scaledError = lin.sqrtDrho * eval.error[0] / lin.alpha; 99 | 100 | JR *= lin.sqrtDrho * lin.alpha; 101 | #endif 102 | 103 | lin.status = Status::VALID; 104 | 105 | return 0; 106 | } 107 | } // namespace Initializer 108 | } // namespace scope 109 | -------------------------------------------------------------------------------- /C++/scope/factor/PoseFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | PoseFactor::PoseFactor(int pose, const Matrix6 &sqrtCov, const Pose &mean, 8 | const std::string &name, int index, bool active) 9 | : Factor({pose}, {}, {}, {}, name, index, active), mSqrtCov(sqrtCov), 10 | mMean(mean) { 11 | assert((mean.R.col(0).cross(mean.R.col(1)) - mean.R.col(2)).norm() < 1e-5 && 12 | "mean.R must be a rotational matrix"); 13 | } 14 | 15 | int PoseFactor::evaluate(const AlignedVector &poses, 16 | const AlignedVector &shapes, 17 | const AlignedVector &joints, 18 | const AlignedVector ¶ms, 19 | Factor::Evaluation &base_eval) const { 20 | auto &eval = dynamic_cast(base_eval); 21 | eval.clear(); 22 | 23 | const auto &i = mvPoses[0]; 24 | assert(i >= 0 && i < poses.size()); 25 | 26 | if (i >= poses.size()) { 27 | LOG(ERROR) << "The pose must be valid." << std::endl; 28 | 29 | exit(-1); 30 | } 31 | 32 | auto &pose = poses[i]; 33 | 34 | eval.errorR.noalias() = mMean.R.transpose() * pose.R; 35 | 36 | eval.error.resize(6); 37 | 38 | Eigen::Map omega(eval.xi.data()); 39 | math::SO3::log(eval.errorR, omega); 40 | eval.xi.tail<3>() = pose.t - mMean.t; 41 | eval.error.noalias() = mSqrtCov * eval.xi; 42 | 43 | eval.f = eval.error.squaredNorm(); 44 | 45 | eval.status = Status::VALID; 46 | 47 | return 0; 48 | } 49 | 50 | int PoseFactor::linearize(const AlignedVector &poses, 51 | const AlignedVector &shapes, 52 | const AlignedVector &joints, 53 | const AlignedVector ¶ms, 54 | const Factor::Evaluation &base_eval, 55 | Factor::Linearization &base_lin) const { 56 | auto &eval = dynamic_cast(base_eval); 57 | auto &lin = dynamic_cast(base_lin); 58 | 59 | lin.clear(); 60 | 61 | assert(eval.status == Status::VALID); 62 | 63 | if (eval.status != Status::VALID) { 64 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 65 | 66 | exit(-1); 67 | } 68 | 69 | const auto &i = mvPoses[0]; 70 | assert(i >= 0 && i < poses.size()); 71 | 72 | if (i < 0 || i >= poses.size()) { 73 | LOG(ERROR) << "The joint must be valid." << std::endl; 74 | 75 | exit(-1); 76 | } 77 | 78 | lin.jacobians[0].resize(1); 79 | 80 | auto &J = lin.jacobians[0][0]; 81 | J.resize(6, 6); 82 | 83 | math::SO3::dexpinv(eval.xi.head<3>(), lin.dexpinv); 84 | lin.dexpinvR.noalias() = lin.dexpinv * mMean.R.transpose(); 85 | 86 | J.topRightCorner<3, 3>().setZero(); 87 | 88 | const auto &t = poses[i].t; 89 | 90 | Eigen::Map JR(J.data()); 91 | Eigen::Map Jt(J.data() + 18); 92 | 93 | Jt = mSqrtCov.rightCols<3>(); 94 | 95 | JR.row(0).noalias() = t.cross(Jt.row(0)); 96 | JR.row(1).noalias() = t.cross(Jt.row(1)); 97 | JR.row(2).noalias() = t.cross(Jt.row(2)); 98 | JR.row(3).noalias() = t.cross(Jt.row(3)); 99 | JR.row(4).noalias() = t.cross(Jt.row(4)); 100 | JR.row(5).noalias() = t.cross(Jt.row(5)); 101 | JR.noalias() += mSqrtCov.leftCols<3>() * lin.dexpinvR; 102 | 103 | lin.status = Status::VALID; 104 | 105 | return 0; 106 | } 107 | } // namespace scope 108 | -------------------------------------------------------------------------------- /C++/scope/factor/UnitPOFFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | UnitPOFFactor::UnitPOFFactor(int pose, const Vector3 &s, const Scalar &sigma, 7 | const Scalar &eps, const Vector3 &measurement, 8 | const Scalar &confidence, const std::string &name, 9 | int index, bool active) 10 | : POFFactor({pose}, {}, {}, {}, sigma, eps, measurement, confidence, name, 11 | index, active), 12 | mS(s) { 13 | mS.stableNormalize(); 14 | } 15 | 16 | int UnitPOFFactor::evaluate(const AlignedVector &poses, 17 | const AlignedVector &shapes, 18 | const AlignedVector &joints, 19 | const AlignedVector ¶ms, 20 | Factor::Evaluation &base_eval) const { 21 | auto &eval = dynamic_cast(base_eval); 22 | eval.clear(); 23 | 24 | const auto &i = mvPoses[0]; 25 | assert(i >= 0 && i < poses.size()); 26 | 27 | if (i >= poses.size()) { 28 | LOG(ERROR) << "The pose must be valid." << std::endl; 29 | 30 | exit(-1); 31 | } 32 | 33 | auto &pose = poses[i]; 34 | eval.error.resize(3); 35 | 36 | eval.d.noalias() = pose.R * mS; 37 | eval.error = eval.d - mMeasurement; 38 | eval.squaredErrorNorm = eval.error.squaredNorm(); 39 | 40 | #if not ROBUST_POF 41 | eval.f = mCon * eval.squaredErrorNorm; 42 | #else 43 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 44 | #endif 45 | 46 | eval.status = Status::VALID; 47 | 48 | return 0; 49 | } 50 | 51 | int UnitPOFFactor::linearize(const AlignedVector &poses, 52 | const AlignedVector &shapes, 53 | const AlignedVector &joints, 54 | const AlignedVector ¶ms, 55 | const Factor::Evaluation &base_eval, 56 | Factor::Linearization &base_lin) const { 57 | auto &eval = dynamic_cast(base_eval); 58 | auto &lin = dynamic_cast(base_lin); 59 | 60 | lin.clear(); 61 | 62 | assert(eval.status == Status::VALID); 63 | 64 | if (eval.status != Status::VALID) { 65 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 66 | 67 | exit(-1); 68 | } 69 | 70 | const auto &i = mvPoses[0]; 71 | assert(i >= 0 && i < poses.size()); 72 | 73 | if (i < 0 || i >= poses.size()) { 74 | LOG(ERROR) << "The joint must be valid." << std::endl; 75 | 76 | exit(-1); 77 | } 78 | 79 | lin.jacobians[0].resize(1); 80 | lin.jacobians[0][0].setZero(3, 6); 81 | 82 | Eigen::Map JR(lin.jacobians[0][0].data()); 83 | 84 | JR(0, 1) = eval.d[2]; 85 | JR(0, 2) = -eval.d[1]; 86 | JR(1, 0) = -eval.d[2]; 87 | JR(1, 2) = eval.d[0]; 88 | JR(2, 0) = eval.d[1]; 89 | JR(2, 1) = -eval.d[0]; 90 | 91 | lin.gPose.setZero(); 92 | 93 | Eigen::Map gR(lin.gPose.data()); 94 | 95 | gR.noalias() = JR.transpose() * eval.error; 96 | 97 | #if not ROBUST_POF 98 | JR *= mSqrtCon; 99 | gR *= mCon; 100 | #else 101 | preLinearizeRobustKernel(eval, lin); 102 | 103 | JR.noalias() -= lin.scaledError * gR.transpose(); 104 | 105 | JR *= lin.sqrtDrho; 106 | gR *= lin.Drho; 107 | #endif 108 | 109 | lin.status = Status::VALID; 110 | 111 | return 0; 112 | } 113 | } // namespace scope 114 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/JointConstFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | JointConstFactor::JointConstFactor(int joint, const Vector3 &weight, 9 | const Matrix3 &mean, const Vector3 &lbnd, 10 | const Vector3 &ubnd, const std::string &name, 11 | int index) 12 | : Factor(name, index), mWeight(weight), mJoint(joint), mMean(mean), 13 | mLowerBnd(lbnd), mUpperBnd(ubnd) { 14 | assert((mMean.col(0).cross(mMean.col(1)) - mMean.col(2)).norm() < 1e-5 && 15 | "mean.R must be a rotational matrix"); 16 | assert((lbnd.cwiseMin(ubnd) - lbnd).norm() < 1e-7); 17 | assert((ubnd.cwiseMax(lbnd) - ubnd).norm() < 1e-7); 18 | } 19 | 20 | int JointConstFactor::evaluate(const AlignedVector &poses, 21 | const AlignedVector &joints, 22 | Factor::Evaluation &base_eval) const { 23 | auto &eval = dynamic_cast(base_eval); 24 | eval.clear(); 25 | 26 | const auto &i = mJoint; 27 | 28 | assert(i >= 0 && i < joints.size()); 29 | 30 | if (i >= joints.size()) { 31 | LOG(ERROR) << "The pose must be valid." << std::endl; 32 | 33 | exit(-1); 34 | } 35 | 36 | const auto &R = joints[i]; 37 | 38 | eval.errorR.noalias() = mMean.transpose() * R; 39 | 40 | math::SO3::log(eval.errorR, eval.xi); 41 | 42 | eval.errorL = eval.xi - mLowerBnd; 43 | eval.errorL = eval.errorL.cwiseMin(mBnd); 44 | 45 | eval.errorU = mUpperBnd - eval.xi; 46 | eval.errorU = eval.errorU.cwiseMin(mBnd); 47 | 48 | eval.errorLInv = mScale * eval.errorL.cwiseInverse(); 49 | eval.errorUInv = mScale * eval.errorU.cwiseInverse(); 50 | 51 | eval.expL = eval.errorLInv.array().exp(); 52 | eval.expU = eval.errorUInv.array().exp(); 53 | 54 | eval.error = eval.errorL.cwiseProduct(eval.expL); 55 | eval.error += eval.errorU.cwiseProduct(eval.expU); 56 | 57 | eval.error.noalias() = mWeight.asDiagonal() * eval.error; 58 | 59 | eval.f = eval.error.squaredNorm(); 60 | 61 | eval.status = Status::VALID; 62 | 63 | return 0; 64 | } 65 | 66 | int JointConstFactor::linearize(const AlignedVector &poses, 67 | const AlignedVector &joints, 68 | const Factor::Evaluation &base_eval, 69 | Factor::Linearization &base_lin) const { 70 | auto &eval = dynamic_cast(base_eval); 71 | auto &lin = dynamic_cast(base_lin); 72 | 73 | lin.clear(); 74 | 75 | const auto &i = mJoint; 76 | 77 | assert(i >= 0 && i < joints.size()); 78 | 79 | if (i >= joints.size()) { 80 | LOG(ERROR) << "The pose must be valid." << std::endl; 81 | 82 | exit(-1); 83 | } 84 | 85 | assert(eval.status == Status::VALID); 86 | 87 | if (eval.status != Status::VALID) { 88 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 89 | 90 | exit(-1); 91 | } 92 | 93 | auto &J = lin.jacobian; 94 | 95 | math::SO3::dexpinv(eval.xi, lin.DF); 96 | J.noalias() = lin.DF * mMean.transpose(); 97 | 98 | lin.Derror = eval.expL; 99 | lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv); 100 | lin.Derror -= eval.expU; 101 | lin.Derror += eval.expU.cwiseProduct(eval.errorUInv); 102 | 103 | J.noalias() = lin.Derror.asDiagonal() * J; 104 | J.noalias() = mWeight.asDiagonal() * J; 105 | 106 | lin.status = Status::VALID; 107 | 108 | return 0; 109 | } 110 | } // namespace Initializer 111 | } // namespace scope 112 | -------------------------------------------------------------------------------- /C++/scope/model/SMPL.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace scope { 4 | template 5 | SMPL

::SMPL(const MatrixX& RelJDirs, const VectorX& RelJ, 6 | const MatrixX& vDirs, const VectorX& v) 7 | : Model<23, P, 6890>(HumanModel::SMPL, RelJDirs, RelJ, vDirs, v) { 8 | setKinematicsTree(); 9 | } 10 | 11 | template 12 | const std::vector& SMPL

::getKinematicsTree() const { 13 | static const std::vector kintree = { 14 | -1, // 0 15 | 0, // 1 16 | 0, // 2 17 | 0, // 3 18 | 1, // 4 19 | 2, // 5 20 | 3, // 6 21 | 4, // 7 22 | 5, // 8 23 | 6, // 9 24 | 7, // 10 25 | 8, // 11 26 | 9, // 12 27 | 9, // 13 28 | 9, // 14 29 | 12, // 15 30 | 13, // 16 31 | 14, // 17 32 | 16, // 18 33 | 17, // 19 34 | 18, // 20 35 | 19, // 21 36 | 20, // 22 37 | 21 // 23 38 | }; 39 | 40 | return kintree; 41 | } 42 | 43 | template 44 | int SMPL

::setKinematicsTree() { 45 | auto& links = this->mvLinks; 46 | 47 | links.resize(this->NumJoints + 1); 48 | 49 | static const std::string names[] = { 50 | "pelvis", // 0 51 | "leftThigh", // 1 52 | "rightThigh", // 2 53 | "spine", // 3 54 | "leftCalf", // 4 55 | "rightCalf", // 5 56 | "spine1", // 6 57 | "leftFoot", // 7 58 | "rightFoot", // 8 59 | "spine2", // 9 60 | "leftToes", // 10 61 | "rightToes", // 11 62 | "neck", // 12 63 | "leftShoulder", // 13 64 | "rightShoulder", // 14 65 | "head", // 15 66 | "leftUpperArm", // 16 67 | "rightUpperArm", // 17 68 | "leftForeArm", // 18 69 | "rightForeArm", // 19 70 | "leftHand", // 20 71 | "rightHand", // 21 72 | "leftFingers", // 22 73 | "rightFingers" // 23 74 | }; 75 | 76 | const auto& parents = getKinematicsTree(); 77 | 78 | for (int i = 0; i <= this->NumJoints; i++) { 79 | assert(parents[i] < i); 80 | 81 | links[i].mId = i; 82 | links[i].mName = names[i]; 83 | links[i].mParent = parents[i]; 84 | links[i].mJoint = i - 1; 85 | } 86 | 87 | for (int i = 1; i <= this->NumJoints; i++) { 88 | links[links[i].mParent].mvChildren.push_back(i); 89 | } 90 | 91 | return 0; 92 | } 93 | 94 | template 95 | const std::vector SMPL

::mvLeftLeg = std::vector{1, 4, 7, 10}; 96 | template 97 | const std::vector SMPL

::mvRightLeg = std::vector{2, 5, 8, 11}; 98 | template 99 | const std::vector SMPL

::mvHead = std::vector{12, 15}; 100 | template 101 | const std::vector SMPL

::mvLeftArm = 102 | std::vector{13, 16, 18, 20, 22}; 103 | template 104 | const std::vector SMPL

::mvRightArm = 105 | std::vector{14, 17, 19, 21, 23}; 106 | template 107 | const std::vector SMPL

::mvBack = std::vector{3, 6, 9}; 108 | 109 | // const std::vector SMPL::mvLeftLeg = std::vector{10, 7, 4, 1}; 110 | // const std::vector SMPL::mvRightLeg = std::vector{11, 8, 5, 2}; 111 | // const std::vector SMPL::mvHead = std::vector{15, 12}; 112 | // const std::vector SMPL::mvLeftArm = std::vector{22, 20, 18, 16, 113 | // 13}; const std::vector SMPL::mvRightArm = std::vector{23, 21, 19, 114 | // 17, 14}; const std::vector SMPL::mvBack = std::vector{9, 6, 3}; 115 | 116 | template class SMPL<10>; 117 | template class SMPL<11>; 118 | } // namespace scope 119 | -------------------------------------------------------------------------------- /C++/scope/factor/Factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define SCOAT_FACTOR_EVALUATION_NEW \ 8 | virtual std::shared_ptr newEvaluation() const override { \ 9 | return std::make_shared(); \ 10 | } 11 | 12 | #define SCOAT_FACTOR_LINEARIZATION_NEW \ 13 | virtual std::shared_ptr newLinearization() \ 14 | const override { \ 15 | return std::make_shared(); \ 16 | } 17 | 18 | namespace scope { 19 | class Factor : public std::enable_shared_from_this { 20 | protected: 21 | std::vector mvPoses; 22 | std::vector mvShapes; 23 | std::vector mvJoints; 24 | std::vector mvParams; 25 | 26 | std::string mName; 27 | 28 | int mIndex; 29 | 30 | bool mActive; 31 | 32 | public: 33 | enum class Status { INVALID = -1, VALID = 0 }; 34 | 35 | struct Evaluation : std::enable_shared_from_this { 36 | Status status; 37 | 38 | VectorX error; 39 | 40 | Scalar f; 41 | 42 | Evaluation(); 43 | 44 | virtual int reset(); 45 | 46 | virtual int clear(); 47 | }; 48 | 49 | struct Linearization : std::enable_shared_from_this { 50 | Status status; 51 | 52 | // Jacobians[0]: linearization w.r.t. poses 53 | // Jacobians[1]: linearization w.r.t. shapes 54 | // Jacobians[2]: linearization w.r.t. joint states 55 | // Jacobians[3]: linearization w.r.t. implicit parameters 56 | AlignedVector jacobians[4]; 57 | 58 | Linearization(); 59 | 60 | virtual int reset(); 61 | 62 | virtual int clear(); 63 | }; 64 | 65 | public: 66 | Factor(const std::vector &poses, const std::vector &shapes, 67 | const std::vector &joints, const std::vector ¶ms, 68 | const std::string &name = "", int index = -1, bool active = true); 69 | 70 | virtual int evaluate(const AlignedVector &poses, 71 | const AlignedVector &shapes, 72 | const AlignedVector &joints, 73 | const AlignedVector ¶ms, 74 | Evaluation &eval) const = 0; 75 | 76 | virtual int linearize(const AlignedVector &poses, 77 | const AlignedVector &shapes, 78 | const AlignedVector &joints, 79 | const AlignedVector ¶ms, 80 | const Evaluation &eval, Linearization &lin) const = 0; 81 | 82 | virtual std::shared_ptr newEvaluation() const { 83 | return std::make_shared(); 84 | } 85 | 86 | virtual std::shared_ptr newLinearization() const { 87 | return std::make_shared(); 88 | } 89 | 90 | const std::vector &getPoses() const { return mvPoses; } 91 | 92 | const std::vector &getShapes() const { return mvShapes; } 93 | 94 | const std::vector &getJoints() const { return mvJoints; } 95 | 96 | const std::vector &getParams() const { return mvParams; } 97 | 98 | void setID(int index) { mIndex = index; } 99 | 100 | int getID() const { return mIndex; } 101 | 102 | const std::string &getName() const { return mName; } 103 | 104 | bool isActive() const { return mActive; } 105 | 106 | void activate() { mActive = true; } 107 | 108 | void deactivate() { mActive = false; } 109 | }; 110 | } // namespace scope 111 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/EulerAngleConstFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | EulerAngleConstFactor::EulerAngleConstFactor(int joint, const Vector3 &weight, 9 | const Matrix3 &mean, 10 | const Vector3 &lbnd, 11 | const Vector3 &ubnd, 12 | const std::string &name, int index) 13 | : JointConstFactor(joint, weight, mean, lbnd, ubnd, name, index){ 14 | } 15 | 16 | int EulerAngleConstFactor::evaluate(const AlignedVector &poses, 17 | const AlignedVector &joints, 18 | Factor::Evaluation &base_eval) const { 19 | auto &eval = dynamic_cast(base_eval); 20 | eval.clear(); 21 | 22 | const auto &i = mJoint; 23 | 24 | assert(i >= 0 && i < joints.size()); 25 | 26 | if (i >= joints.size()) { 27 | LOG(ERROR) << "The pose must be valid." << std::endl; 28 | 29 | exit(-1); 30 | } 31 | 32 | const auto &R = joints[i]; 33 | 34 | eval.errorR.noalias() = mMean.transpose() * R; 35 | 36 | math::SO3::Rot2XYZ(eval.errorR, eval.xi); 37 | 38 | eval.errorL = eval.xi - mLowerBnd; 39 | eval.errorL = eval.errorL.cwiseMin(mBnd); 40 | 41 | eval.errorU = mUpperBnd - eval.xi; 42 | eval.errorU = eval.errorU.cwiseMin(mBnd); 43 | 44 | eval.errorLInv = mScale * eval.errorL.cwiseInverse(); 45 | eval.errorUInv = mScale * eval.errorU.cwiseInverse(); 46 | 47 | eval.expL = eval.errorLInv.array().exp(); 48 | eval.expU = eval.errorUInv.array().exp(); 49 | 50 | eval.error = eval.errorL.cwiseProduct(eval.expL); 51 | eval.error += eval.errorU.cwiseProduct(eval.expU); 52 | 53 | eval.error.noalias() = mWeight.asDiagonal() * eval.error; 54 | 55 | eval.f = eval.error.squaredNorm(); 56 | 57 | eval.status = Status::VALID; 58 | 59 | return 0; 60 | } 61 | 62 | int EulerAngleConstFactor::linearize(const AlignedVector &poses, 63 | const AlignedVector &joints, 64 | const Factor::Evaluation &base_eval, 65 | Factor::Linearization &base_lin) const { 66 | auto &eval = dynamic_cast(base_eval); 67 | auto &lin = dynamic_cast(base_lin); 68 | 69 | lin.clear(); 70 | 71 | const auto &i = mJoint; 72 | 73 | assert(i >= 0 && i < joints.size()); 74 | 75 | if (i >= joints.size()) { 76 | LOG(ERROR) << "The pose must be valid." << std::endl; 77 | 78 | exit(-1); 79 | } 80 | 81 | assert(eval.status == Status::VALID); 82 | 83 | if (eval.status != Status::VALID) { 84 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 85 | 86 | exit(-1); 87 | } 88 | 89 | auto &J = lin.jacobian; 90 | 91 | lin.DF.setIdentity(); 92 | lin.DF(1, 1) = cos(eval.xi[0]); 93 | lin.DF(2, 1) = sin(eval.xi[0]); 94 | lin.DF.col(2) = eval.errorR.col(2); 95 | 96 | lin.H.noalias() = lin.DF.transpose() * lin.DF; 97 | lin.H.diagonal().array() += 1e-4; 98 | 99 | lin.D.noalias() = lin.H.inverse() * lin.DF.transpose(); 100 | 101 | J.noalias() = lin.D * mMean.transpose(); 102 | 103 | lin.Derror = eval.expL; 104 | lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv); 105 | lin.Derror -= eval.expU; 106 | lin.Derror += eval.expU.cwiseProduct(eval.errorUInv); 107 | 108 | J.noalias() = lin.Derror.asDiagonal() * J; 109 | J.noalias() = mWeight.asDiagonal() * J; 110 | 111 | lin.status = Status::VALID; 112 | 113 | return 0; 114 | } 115 | } // namespace Initializer 116 | } // namespace scope 117 | -------------------------------------------------------------------------------- /C++/scope/model/Model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | template 8 | class Model { 9 | public: 10 | // number of joints 11 | const static int NumJoints = K; 12 | // number of implicit parameters 13 | const static int NumParams = P; 14 | // number of vertices 15 | const static int NumVertices = N; 16 | 17 | protected: 18 | // human model 19 | HumanModel mHuman; 20 | 21 | const MatrixX mRawRelJDirs; 22 | const VectorX mRawRelJ; 23 | const MatrixX mRawVDirs; 24 | const VectorX mRawV; 25 | 26 | // linear map from shape parameters to joint locations 27 | const Eigen::Map> mRelJDirs; 28 | const Eigen::Map> mRelJ; 29 | 30 | AlignedVector> mvRelJDirs; 31 | AlignedVector mvRelJ; 32 | 33 | // linear map from shape parameters to mesh vertices 34 | const Eigen::Map> mVDirs; 35 | const Eigen::Map> mV; 36 | 37 | AlignedVector> mvVDirs; 38 | AlignedVector mvV; 39 | 40 | // body parts 41 | std::vector mvLinks; 42 | 43 | // pairs of body parts to check collisions 44 | std::vector> mvCollisionPairs; 45 | 46 | protected: 47 | Model(const HumanModel& human, const MatrixX& RelJDirs, const VectorX& RelJ, 48 | const MatrixX& VDirs, const VectorX& v); 49 | 50 | Model(const Model& model); 51 | Model(Model&& model); 52 | Model& operator=(const Model& model) = delete; 53 | Model& operator=(Model&& model) = delete; 54 | 55 | virtual int setKinematicsTree() = 0; 56 | 57 | virtual int FKIterate(AlignedVector& T, int i, const Pose& T0, 58 | const AlignedVector& Omega, 59 | const Eigen::Map>& mu) const; 60 | 61 | virtual int DFKIterate(AlignedVector>& Bp, 62 | AlignedVector& Bu, int i, 63 | const AlignedVector& T) const; 64 | 65 | virtual int DFKIterate(AlignedVector>>& B, int i, 66 | const AlignedVector& T) const; 67 | 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | const HumanModel& getHumanModel() const; 72 | 73 | const Eigen::Map>& getRelJDirs() const; 74 | const Eigen::Map>& getRelJ() const; 75 | 76 | const Eigen::Map>& getVDirs() const; 77 | const Eigen::Map>& getV() const; 78 | 79 | const std::vector& getLinks() const; 80 | 81 | const std::vector>& getCollisionsPairs() const; 82 | 83 | virtual const std::vector & getKinematicsTree() const = 0; 84 | 85 | // --------------------------------------------------------- 86 | // forward kinematics 87 | // --------------------------------------------------------- 88 | // T: body part poses 89 | // T0: the root pose 90 | // Omega: joint states 91 | // mu: joint locations 92 | virtual int FK(AlignedVector& T, Eigen::Map>& mu, 93 | const Pose& T0, const AlignedVector& Omega, 94 | const VectorX& beta) const; 95 | 96 | // --------------------------------------------------------- 97 | // linearization of forward kinematics 98 | // --------------------------------------------------------- 99 | virtual int DFK(AlignedVector>& Bp, AlignedVector& Bu, 100 | const AlignedVector& T) const; 101 | 102 | virtual int DFK(AlignedVector>>& B, 103 | const AlignedVector& T) const; 104 | }; 105 | 106 | extern template class Model<2, 2, 20>; 107 | extern template class Model<23, 10, 6890>; 108 | extern template class Model<51, 10, 6890>; 109 | } // namespace scope 110 | -------------------------------------------------------------------------------- /C++/scope/factor/EulerAngleConstFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | EulerAngleConstFactor::EulerAngleConstFactor( 8 | int joint, const Vector3 &weight, const Matrix3 &mean, const Vector3 &lbnd, 9 | const Vector3 &ubnd, const std::string &name, int index, bool active) 10 | : JointConstFactor(joint, weight, mean, lbnd, ubnd, name, index, active) {} 11 | 12 | int EulerAngleConstFactor::evaluate(const AlignedVector &poses, 13 | const AlignedVector &shapes, 14 | const AlignedVector &joints, 15 | const AlignedVector ¶ms, 16 | Factor::Evaluation &base_eval) const { 17 | auto &eval = dynamic_cast(base_eval); 18 | eval.clear(); 19 | 20 | const auto &i = mvJoints[0]; 21 | assert(i >= 0 && i < joints.size()); 22 | 23 | if (i >= joints.size()) { 24 | LOG(ERROR) << "The joint must be valid." << std::endl; 25 | 26 | exit(-1); 27 | } 28 | 29 | const auto &R = joints[i]; 30 | 31 | eval.errorR.noalias() = mMean.transpose() * R; 32 | 33 | math::SO3::Rot2XYZ(eval.errorR, eval.xi); 34 | 35 | eval.errorL = eval.xi - mLowerBnd; 36 | eval.errorL = eval.errorL.cwiseMin(mBnd); 37 | 38 | eval.errorU = mUpperBnd - eval.xi; 39 | eval.errorU = eval.errorU.cwiseMin(mBnd); 40 | 41 | eval.errorLInv = mScale * eval.errorL.cwiseInverse(); 42 | eval.errorUInv = mScale * eval.errorU.cwiseInverse(); 43 | 44 | eval.expL = eval.errorLInv.array().exp(); 45 | eval.expU = eval.errorUInv.array().exp(); 46 | 47 | eval.error = eval.errorL.cwiseProduct(eval.expL); 48 | eval.error += eval.errorU.cwiseProduct(eval.expU); 49 | 50 | eval.error.noalias() = mWeight.asDiagonal() * eval.error; 51 | 52 | eval.f = eval.error.squaredNorm(); 53 | 54 | eval.status = Status::VALID; 55 | 56 | return 0; 57 | } 58 | 59 | int EulerAngleConstFactor::linearize(const AlignedVector &poses, 60 | const AlignedVector &shapes, 61 | const AlignedVector &joints, 62 | const AlignedVector ¶ms, 63 | const Factor::Evaluation &base_eval, 64 | Factor::Linearization &base_lin) const { 65 | auto &eval = dynamic_cast(base_eval); 66 | auto &lin = dynamic_cast(base_lin); 67 | 68 | lin.clear(); 69 | 70 | assert(eval.status == Status::VALID); 71 | 72 | if (eval.status != Status::VALID) { 73 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 74 | 75 | exit(-1); 76 | } 77 | 78 | const auto &i = mvJoints[0]; 79 | assert(i >= 0 && i < joints.size()); 80 | 81 | if (i < 0 || i >= joints.size()) { 82 | LOG(ERROR) << "The joint must be valid." << std::endl; 83 | 84 | exit(-1); 85 | } 86 | 87 | lin.jacobians[2].resize(1); 88 | lin.jacobians[2][0].resize(3, 3); 89 | Eigen::Map J(lin.jacobians[2][0].data()); 90 | 91 | lin.DF.setIdentity(); 92 | lin.DF(1, 1) = cos(eval.xi[0]); 93 | lin.DF(2, 1) = sin(eval.xi[0]); 94 | lin.DF.col(2) = eval.errorR.col(2); 95 | 96 | lin.H.noalias() = lin.DF.transpose() * lin.DF; 97 | lin.H.diagonal().array() += 1e-4; 98 | 99 | lin.D.noalias() = lin.H.inverse() * lin.DF.transpose(); 100 | 101 | J.noalias() = lin.D * mMean.transpose(); 102 | 103 | lin.Derror = eval.expL; 104 | lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv); 105 | lin.Derror -= eval.expU; 106 | lin.Derror += eval.expU.cwiseProduct(eval.errorUInv); 107 | 108 | J.noalias() = lin.Derror.asDiagonal() * J; 109 | J.noalias() = mWeight.asDiagonal() * J; 110 | 111 | lin.status = Status::VALID; 112 | 113 | return 0; 114 | } 115 | } // namespace scope 116 | -------------------------------------------------------------------------------- /C++/scope/factor/JointConstFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | JointConstFactor::JointConstFactor(int joint, const Vector3& weight, const Matrix3 &mean, 8 | const Vector3 &lbnd, const Vector3 &ubnd, 9 | const std::string &name, int index, 10 | bool active) 11 | : Factor({}, {}, {joint}, {}, name, index, active), 12 | mWeight(weight), 13 | mMean(mean), 14 | mLowerBnd(lbnd), 15 | mUpperBnd(ubnd) { 16 | assert((mMean.col(0).cross(mMean.col(1)) - mMean.col(2)).norm() < 1e-5 && 17 | "mean.R must be a rotational matrix"); 18 | assert((lbnd.cwiseMin(ubnd) - lbnd).norm() < 1e-7); 19 | assert((ubnd.cwiseMax(lbnd) - ubnd).norm() < 1e-7); 20 | } 21 | 22 | int JointConstFactor::evaluate(const AlignedVector &poses, 23 | const AlignedVector &shapes, 24 | const AlignedVector &joints, 25 | const AlignedVector ¶ms, 26 | Factor::Evaluation &base_eval) const { 27 | auto &eval = dynamic_cast(base_eval); 28 | eval.clear(); 29 | 30 | const auto &i = mvJoints[0]; 31 | assert(i >= 0 && i < joints.size()); 32 | 33 | if (i >= joints.size()) { 34 | LOG(ERROR) << "The joint must be valid." << std::endl; 35 | 36 | exit(-1); 37 | } 38 | 39 | const auto &R = joints[i]; 40 | 41 | eval.errorR.noalias() = mMean.transpose() * R; 42 | 43 | math::SO3::log(eval.errorR, eval.xi); 44 | 45 | eval.errorL = eval.xi - mLowerBnd; 46 | eval.errorL = eval.errorL.cwiseMin(mBnd); 47 | 48 | eval.errorU = mUpperBnd - eval.xi; 49 | eval.errorU = eval.errorU.cwiseMin(mBnd); 50 | 51 | eval.errorLInv = mScale * eval.errorL.cwiseInverse(); 52 | eval.errorUInv = mScale * eval.errorU.cwiseInverse(); 53 | 54 | eval.expL = eval.errorLInv.array().exp(); 55 | eval.expU = eval.errorUInv.array().exp(); 56 | 57 | eval.error = eval.errorL.cwiseProduct(eval.expL); 58 | eval.error += eval.errorU.cwiseProduct(eval.expU); 59 | 60 | eval.error.noalias() = mWeight.asDiagonal() * eval.error; 61 | 62 | eval.f = eval.error.squaredNorm(); 63 | 64 | eval.status = Status::VALID; 65 | 66 | return 0; 67 | } 68 | 69 | int JointConstFactor::linearize(const AlignedVector &poses, 70 | const AlignedVector &shapes, 71 | const AlignedVector &joints, 72 | const AlignedVector ¶ms, 73 | const Factor::Evaluation &base_eval, 74 | Factor::Linearization &base_lin) const { 75 | auto &eval = dynamic_cast(base_eval); 76 | auto &lin = dynamic_cast(base_lin); 77 | 78 | lin.clear(); 79 | 80 | assert(eval.status == Status::VALID); 81 | 82 | if (eval.status != Status::VALID) { 83 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 84 | 85 | exit(-1); 86 | } 87 | 88 | const auto &i = mvJoints[0]; 89 | assert(i >= 0 && i < joints.size()); 90 | 91 | if (i < 0 || i >= joints.size()) { 92 | LOG(ERROR) << "The joint must be valid." << std::endl; 93 | 94 | exit(-1); 95 | } 96 | 97 | lin.jacobians[2].resize(1); 98 | lin.jacobians[2][0].resize(3, 3); 99 | Eigen::Map J(lin.jacobians[2][0].data()); 100 | 101 | math::SO3::dexpinv(eval.xi, lin.DF); 102 | J.noalias() = lin.DF * mMean.transpose(); 103 | 104 | lin.Derror = eval.expL; 105 | lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv); 106 | lin.Derror -= eval.expU; 107 | lin.Derror += eval.expU.cwiseProduct(eval.errorUInv); 108 | 109 | J.noalias() = lin.Derror.asDiagonal() * J; 110 | J.noalias() = mWeight.asDiagonal() * J; 111 | 112 | lin.status = Status::VALID; 113 | 114 | return 0; 115 | } 116 | } // namespace scope 117 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/DepthCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | DepthCameraFactor::DepthCameraFactor(int pose, const Vector3 &point, 8 | const Scalar &sigma, const Scalar &eps, 9 | const Vector3 &measurement, 10 | const Scalar &confidence, 11 | const std::string &name, int index) 12 | : Factor(name, index), 13 | mPose(pose), 14 | mPoint(point), 15 | mMeasurement(measurement), 16 | mSigma(sigma), 17 | mCon(confidence), 18 | mSqrtCon(std::sqrt(confidence)), 19 | mSigmaCon(confidence * sigma) { 20 | assert(eps > 1e-5 && eps < 1.0); 21 | assert(sigma > 0); 22 | assert(confidence >= 0); 23 | 24 | mMaxGMScale = 1 - eps; 25 | mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma; 26 | } 27 | 28 | int DepthCameraFactor::evaluate(const AlignedVector &poses, 29 | const AlignedVector &joints, 30 | Factor::Evaluation &base_eval) const { 31 | auto &eval = dynamic_cast(base_eval); 32 | eval.clear(); 33 | 34 | const auto &i = mPose; 35 | 36 | assert(i >= 0 && i < poses.size()); 37 | 38 | if (i >= poses.size()) { 39 | LOG(ERROR) << "The pose must be valid." << std::endl; 40 | 41 | exit(-1); 42 | } 43 | 44 | auto &pose = poses[i]; 45 | 46 | eval.point3D = pose.t; 47 | eval.point3D.noalias() += pose.R * mPoint; 48 | 49 | eval.error = eval.point3D - mMeasurement; 50 | eval.squaredErrorNorm = eval.error.squaredNorm(); 51 | 52 | #if 0 53 | eval.f = mCon * eval.squaredErrorNorm; 54 | #else 55 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 56 | #endif 57 | 58 | eval.status = Status::VALID; 59 | 60 | return 0; 61 | } 62 | 63 | int DepthCameraFactor::linearize(const AlignedVector &poses, 64 | const AlignedVector &joints, 65 | const Factor::Evaluation &base_eval, 66 | Factor::Linearization &base_lin) const { 67 | auto &eval = dynamic_cast(base_eval); 68 | auto &lin = dynamic_cast(base_lin); 69 | 70 | lin.clear(); 71 | 72 | const auto &i = mPose; 73 | 74 | assert(i >= 0 && i < poses.size()); 75 | 76 | if (i >= poses.size()) { 77 | LOG(ERROR) << "The pose must be valid." << std::endl; 78 | 79 | exit(-1); 80 | } 81 | 82 | assert(eval.status == Status::VALID); 83 | 84 | if (eval.status != Status::VALID) { 85 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 86 | 87 | exit(-1); 88 | } 89 | 90 | auto &J = lin.jacobian; 91 | 92 | Eigen::Map> JR(J.data()); 93 | Eigen::Map> Jt(J.data() + 9); 94 | 95 | JR.setZero(); 96 | 97 | JR(0, 1) = eval.point3D[2]; 98 | JR(0, 2) = -eval.point3D[1]; 99 | JR(1, 0) = -eval.point3D[2]; 100 | JR(1, 2) = eval.point3D[0]; 101 | JR(2, 0) = eval.point3D[1]; 102 | JR(2, 1) = -eval.point3D[0]; 103 | 104 | Jt.setIdentity(); 105 | Jt.setIdentity(); 106 | 107 | auto &g = lin.g; 108 | 109 | g.noalias() = J.transpose() * eval.error; 110 | 111 | #if 0 112 | J *= mSqrtCon; 113 | g *= mCon; 114 | #else 115 | lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm); 116 | lin.Drho = lin.sqrtDrho * lin.sqrtDrho; 117 | lin.alpha = eval.squaredErrorNorm > mGMThreshold 118 | ? mMaxGMScale 119 | : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm / 120 | (mSigma + eval.squaredErrorNorm)); 121 | 122 | if (lin.alpha > 1e-5) { 123 | lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm; 124 | } else { 125 | lin.scaledError.setZero(); 126 | } 127 | 128 | lin.sqrtDrho *= mSqrtCon; 129 | lin.Drho *= mCon; 130 | 131 | J.noalias() -= lin.scaledError * g.transpose(); 132 | 133 | J *= lin.sqrtDrho; 134 | g *= lin.Drho; 135 | #endif 136 | 137 | lin.status = Status::VALID; 138 | 139 | return 0; 140 | } 141 | } // namespace Initializer 142 | } // namespace scope 143 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/PinholeCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | namespace Initializer { 7 | PinholeCameraFactor::PinholeCameraFactor(int pose, const Vector3 &point, 8 | const Scalar &sigma, const Scalar &eps, 9 | const Vector2 &measurement, 10 | const Scalar &confidence, 11 | const std::string &name, int index) 12 | : Factor(name, index), mPose(pose), mPoint(point), 13 | mMeasurement(measurement), mSigma(sigma), mCon(confidence), 14 | mSqrtCon(std::sqrt(confidence)), mSigmaCon(confidence * sigma) { 15 | assert(eps > 1e-5 && eps < 1.0); 16 | assert(sigma > 0); 17 | assert(confidence >= 0); 18 | 19 | mMaxGMScale = 1 - eps; 20 | mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma; 21 | } 22 | 23 | int PinholeCameraFactor::evaluate(const AlignedVector &poses, 24 | const AlignedVector &joints, 25 | Factor::Evaluation &base_eval) const { 26 | auto &eval = dynamic_cast(base_eval); 27 | eval.clear(); 28 | 29 | const auto &i = mPose; 30 | 31 | assert(i >= 0 && i < poses.size()); 32 | 33 | if (i >= poses.size()) { 34 | LOG(ERROR) << "The pose must be valid." << std::endl; 35 | 36 | exit(-1); 37 | } 38 | 39 | auto &pose = poses[i]; 40 | 41 | eval.pCam = pose.t; 42 | eval.pCam.noalias() += pose.R * mPoint; 43 | 44 | eval.point2D = eval.pCam.head<2>() / eval.pCam[2]; 45 | eval.error = eval.point2D - mMeasurement; 46 | eval.squaredErrorNorm = eval.error.squaredNorm(); 47 | 48 | #if 0 49 | eval.f = mCon * eval.squaredErrorNorm; 50 | #else 51 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 52 | #endif 53 | 54 | eval.status = Status::VALID; 55 | 56 | return 0; 57 | } 58 | 59 | int PinholeCameraFactor::linearize(const AlignedVector &poses, 60 | const AlignedVector &joints, 61 | const Factor::Evaluation &base_eval, 62 | Factor::Linearization &base_lin) const { 63 | auto &eval = dynamic_cast(base_eval); 64 | auto &lin = dynamic_cast(base_lin); 65 | 66 | lin.clear(); 67 | 68 | const auto &i = mPose; 69 | 70 | assert(i >= 0 && i < poses.size()); 71 | 72 | if (i >= poses.size()) { 73 | LOG(ERROR) << "The pose must be valid." << std::endl; 74 | 75 | exit(-1); 76 | } 77 | 78 | assert(eval.status == Status::VALID); 79 | 80 | if (eval.status != Status::VALID) { 81 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 82 | 83 | exit(-1); 84 | } 85 | 86 | auto &J = lin.jacobian; 87 | 88 | J.resize(2, 6); 89 | 90 | Eigen::Map> JR(J.data()); 91 | Eigen::Map> Jt(J.data() + 6); 92 | 93 | Scalar zinv = 1.0 / eval.pCam[2]; 94 | 95 | Jt(0, 0) = zinv; 96 | Jt(0, 1) = 0; 97 | Jt(0, 2) = -eval.point2D[0] * zinv; 98 | Jt(1, 0) = 0; 99 | Jt(1, 1) = zinv; 100 | Jt(1, 2) = -eval.point2D[1] * zinv; 101 | 102 | JR.row(0) = eval.pCam.cross(Jt.row(0)); 103 | JR.row(1) = eval.pCam.cross(Jt.row(1)); 104 | 105 | auto &g = lin.g; 106 | 107 | g.noalias() = J.transpose() * eval.error; 108 | 109 | #if 0 110 | J *= mSqrtCon; 111 | g *= mCon; 112 | #else 113 | lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm); 114 | lin.Drho = lin.sqrtDrho * lin.sqrtDrho; 115 | lin.alpha = eval.squaredErrorNorm > mGMThreshold 116 | ? mMaxGMScale 117 | : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm / 118 | (mSigma + eval.squaredErrorNorm)); 119 | 120 | if (lin.alpha > 1e-5) { 121 | lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm; 122 | } else { 123 | lin.scaledError.setZero(); 124 | } 125 | 126 | lin.sqrtDrho *= mSqrtCon; 127 | lin.Drho *= mCon; 128 | 129 | J.noalias() -= lin.scaledError * g.transpose(); 130 | 131 | J *= lin.sqrtDrho; 132 | g *= lin.Drho; 133 | #endif 134 | 135 | lin.status = Status::VALID; 136 | 137 | return 0; 138 | } 139 | } // namespace Initializer 140 | } // namespace scope 141 | -------------------------------------------------------------------------------- /C++/scope/utils/JointConstInfo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace scope { 9 | namespace JointConstInfo { 10 | namespace SMPL { 11 | const AlignedVector> 12 | JointConstInfo{ 13 | //[0]: joint index 14 | //[1]: weight 15 | //[2]: lower bound 16 | //[3]: upper bound 17 | {3, // left knee 18 | {5, 3, 3}, 19 | (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(), 20 | {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI}, 21 | {+0.85 * M_PI, +0.05 * M_PI, +0.03 * M_PI}}, 22 | {4, // right knee 23 | {5, 3, 3}, 24 | (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(), 25 | {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI}, 26 | {+0.85 * M_PI, +0.05 * M_PI, +0.03 * M_PI}}, 27 | {15, 28 | {3, 3, 3}, 29 | (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(), 30 | {-0.50 * M_PI, -0.75 * M_PI, -0.25 * M_PI}, 31 | {+0.25 * M_PI, +0.00 * M_PI, +0.25 * M_PI}}, 32 | {16, 33 | {3, 3, 3}, 34 | (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(), 35 | {-0.50 * M_PI, -0.00 * M_PI, -0.25 * M_PI}, 36 | {+0.25 * M_PI, +0.75 * M_PI, +0.25 * M_PI}}}; 37 | 38 | const AlignedVector> 39 | EulerAngleConstInfo{ 40 | //[0]: joint index 41 | //[1]: weight 42 | //[2]: lower bound 43 | //[3]: upper bound 44 | {0, // left hip 45 | {10, 3, 3}, 46 | (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(), 47 | {-0.67 * M_PI, -0.25 * M_PI, -0.17 * M_PI}, 48 | {+0.50 * M_PI, +0.48 * M_PI, +0.25 * M_PI}}, 49 | {1, // right hip 50 | {10, 3, 3}, 51 | (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(), 52 | {-0.67 * M_PI, -0.48 * M_PI, -0.25 * M_PI}, 53 | {+0.50 * M_PI, +0.25 * M_PI, +0.17 * M_PI}}, 54 | //{15, // left upper arm 55 | //{3, 3, 3}, 56 | //(Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(), 57 | //{-0.50 * M_PI, -0.33 * M_PI, -0.60 * M_PI}, 58 | //{+0.46 * M_PI, +0.48 * M_PI, +0.40 * M_PI}}, 59 | //{16, // right upper arm 60 | //{3, 3, 3}, 61 | //(Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(), 62 | //{-0.46 * M_PI, -0.33 * M_PI, -0.40 * M_PI}, 63 | //{+0.50 * M_PI, +0.48 * M_PI, +0.60 * M_PI}}, 64 | //{17, // left upper arm 65 | //{3, 3, 3}, 66 | //(Matrix3() << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 67 | // 0.0, 1.0).finished(), 68 | //{-0.00 * M_PI, -0.33 * M_PI, -0.15 * M_PI}, 69 | //{+0.75 * M_PI, +0.33 * M_PI, +0.15 * M_PI}}, 70 | //{18, // right upper arm 71 | //{3, 3, 3}, 72 | //(Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(), 73 | //{-0.75 * M_PI, -0.33 * M_PI, -0.15 * M_PI}, 74 | //{+0.00 * M_PI, +0.33 * M_PI, +0.15 * M_PI}} 75 | }; 76 | } // namespace SMPL 77 | 78 | namespace SMPLH { 79 | const AlignedVector> JointConstInfo{ 80 | //[0]: joint index 81 | //[1]: weight 82 | //[2]: lower bound 83 | //[3]: upper bound 84 | {0, // left hip 85 | {10, 3, 3}, 86 | {-0.67 * M_PI, -0.10 * M_PI, -0.20 * M_PI}, 87 | {+0.50 * M_PI, +0.33 * M_PI, +0.33 * M_PI}}, 88 | {1, // right hip 89 | {10, 3, 3}, 90 | {-0.67 * M_PI, -0.33 * M_PI, -0.33 * M_PI}, 91 | {+0.50 * M_PI, +0.10 * M_PI, +0.20 * M_PI}}, 92 | {3, // left knee 93 | {5, 5, 5}, 94 | {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI}, 95 | {+0.80 * M_PI, +0.05 * M_PI, +0.03 * M_PI}}, 96 | {4, // right knee 97 | {5, 5, 5}, 98 | {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI}, 99 | {+0.80 * M_PI, +0.05 * M_PI, +0.03 * M_PI}}, 100 | {15, 101 | {3, 3, 3}, 102 | {-0.50 * M_PI, -0.75 * M_PI, -0.25 * M_PI}, 103 | {+0.25 * M_PI, +0.05 * M_PI, +0.25 * M_PI}}, 104 | {16, 105 | {3, 3, 3}, 106 | {-0.50 * M_PI, -0.05 * M_PI, -0.25 * M_PI}, 107 | {+0.25 * M_PI, +0.75 * M_PI, +0.25 * M_PI}}}; 108 | } 109 | } // namespace JointConstInfo 110 | } // namespace scope 111 | -------------------------------------------------------------------------------- /C++/scope/factor/JointDepthCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | JointDepthCameraFactor::JointDepthCameraFactor(int pose, const Scalar &sigma, 7 | const Scalar &eps, 8 | const Vector3 &measurement, 9 | const Scalar &confidence, 10 | const std::string &name, 11 | int index, bool active) 12 | : DepthCameraFactor({pose}, {}, {}, {}, sigma, eps, measurement, confidence, 13 | name, index, active) {} 14 | 15 | int JointDepthCameraFactor::evaluate(const AlignedVector &poses, 16 | const AlignedVector &shapes, 17 | const AlignedVector &joints, 18 | const AlignedVector ¶ms, 19 | Factor::Evaluation &base_eval) const { 20 | auto &eval = dynamic_cast(base_eval); 21 | eval.clear(); 22 | 23 | const auto &i = mvPoses[0]; 24 | assert(i >= 0 && i < poses.size()); 25 | 26 | if (i < 0 || i >= poses.size()) { 27 | LOG(ERROR) << "The pose must be valid." << std::endl; 28 | 29 | exit(-1); 30 | } 31 | 32 | evaluate(poses[i], mMeasurement, eval); 33 | 34 | eval.status = Status::VALID; 35 | 36 | return 0; 37 | } 38 | 39 | int JointDepthCameraFactor::linearize(const AlignedVector &poses, 40 | const AlignedVector &shapes, 41 | const AlignedVector &joints, 42 | const AlignedVector ¶ms, 43 | const Factor::Evaluation &base_eval, 44 | Factor::Linearization &base_lin) const { 45 | auto &eval = dynamic_cast(base_eval); 46 | auto &lin = dynamic_cast(base_lin); 47 | 48 | lin.clear(); 49 | 50 | assert(eval.status == Status::VALID); 51 | 52 | if (eval.status != Status::VALID) { 53 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 54 | 55 | exit(-1); 56 | } 57 | 58 | const auto &i = mvPoses[0]; 59 | assert(i >= 0 && i < poses.size()); 60 | 61 | if (i < 0 || i >= poses.size()) { 62 | LOG(ERROR) << "The pose must be valid." << std::endl; 63 | 64 | exit(-1); 65 | } 66 | 67 | lin.jacobians[0].resize(1); 68 | 69 | linearize(poses[i], mMeasurement, eval, lin); 70 | 71 | lin.status = Status::VALID; 72 | 73 | return 0; 74 | } 75 | 76 | int JointDepthCameraFactor::evaluate(const Pose &pose, 77 | const Vector3 &measurement, 78 | Evaluation &eval) const { 79 | eval.error = pose.t - measurement; 80 | eval.squaredErrorNorm = eval.error.squaredNorm(); 81 | 82 | #if not ROBUST_DEPTH 83 | eval.f = mCon * eval.squaredErrorNorm; 84 | #else 85 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 86 | #endif 87 | 88 | return 0; 89 | } 90 | 91 | int JointDepthCameraFactor::linearize(const Pose &pose, 92 | const Vector3 &measurement, 93 | const Evaluation &eval, 94 | Linearization &lin) const { 95 | assert(lin.jacobians[0].size() == 1); 96 | 97 | auto &JPose = lin.jacobians[0][0]; 98 | 99 | JPose.resize(3, 6); 100 | 101 | Eigen::Map JR(JPose.data()); 102 | Eigen::Map Jt(JPose.data() + 9); 103 | 104 | const auto &t = pose.t; 105 | 106 | JR.setZero(); 107 | 108 | JR(0, 1) = t[2]; 109 | JR(0, 2) = -t[1]; 110 | JR(1, 0) = -t[2]; 111 | JR(1, 2) = t[0]; 112 | JR(2, 0) = t[1]; 113 | JR(2, 1) = -t[0]; 114 | 115 | Jt.setIdentity(); 116 | 117 | auto &gPose = lin.gPose; 118 | gPose.noalias() = JPose.transpose() * eval.error; 119 | 120 | #if not ROBUST_DEPTH 121 | JPose *= mSqrtCon; 122 | gPose *= mCon; 123 | #else 124 | preLinearizeRobustKernel(eval, lin); 125 | 126 | JPose.noalias() -= lin.scaledError * gPose.transpose(); 127 | 128 | JPose *= lin.sqrtDrho; 129 | gPose *= lin.Drho; 130 | #endif 131 | 132 | return 0; 133 | } 134 | } // namespace scope 135 | -------------------------------------------------------------------------------- /C++/scope/factor/JointExtraPinholeCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | JointExtraPinholeCameraFactor::JointExtraPinholeCameraFactor( 7 | int pose, const Scalar &sigma, const Scalar &eps, const Pose &extraCamPose, 8 | const Vector2 &measurement, const Scalar &confidence, 9 | const std::string &name, int index, bool active) 10 | : JointPinholeCameraFactor(pose, sigma, eps, measurement, confidence, name, 11 | index, active), 12 | ExtraPinholeCamera(extraCamPose) {} 13 | 14 | int JointExtraPinholeCameraFactor::evaluate( 15 | const AlignedVector &poses, const AlignedVector &shapes, 16 | const AlignedVector &joints, const AlignedVector ¶ms, 17 | Factor::Evaluation &base_eval) const { 18 | auto &eval = dynamic_cast(base_eval); 19 | eval.clear(); 20 | 21 | const auto &i = mvPoses[0]; 22 | assert(i >= 0 && i < poses.size()); 23 | 24 | if (i < 0 || i >= poses.size()) { 25 | LOG(ERROR) << "The pose must be valid." << std::endl; 26 | 27 | exit(-1); 28 | } 29 | 30 | evaluate(poses[i], mMeasurement, eval); 31 | 32 | eval.status = Status::VALID; 33 | 34 | return 0; 35 | } 36 | 37 | int JointExtraPinholeCameraFactor::linearize( 38 | const AlignedVector &poses, const AlignedVector &shapes, 39 | const AlignedVector &joints, const AlignedVector ¶ms, 40 | const Factor::Evaluation &base_eval, 41 | Factor::Linearization &base_lin) const { 42 | auto &eval = dynamic_cast(base_eval); 43 | auto &lin = dynamic_cast(base_lin); 44 | 45 | lin.clear(); 46 | 47 | assert(eval.status == Status::VALID); 48 | 49 | if (eval.status != Status::VALID) { 50 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 51 | 52 | exit(-1); 53 | } 54 | 55 | const auto &i = mvPoses[0]; 56 | assert(i >= 0 && i < poses.size()); 57 | 58 | if (i < 0 || i >= poses.size()) { 59 | LOG(ERROR) << "The pose must be valid." << std::endl; 60 | 61 | exit(-1); 62 | } 63 | 64 | lin.jacobians[0].resize(1); 65 | 66 | linearize(poses[i], mMeasurement, eval, lin); 67 | 68 | lin.status = Status::VALID; 69 | 70 | return 0; 71 | } 72 | 73 | int JointExtraPinholeCameraFactor::evaluate(const Pose &pose, 74 | const Vector2 &measurement, 75 | Evaluation &eval) const { 76 | eval.pExtraCam = mExtraCamPose.t; 77 | eval.pExtraCam.noalias() += mExtraCamPose.R * pose.t; 78 | 79 | eval.point2D = eval.pExtraCam.head<2>() / eval.pExtraCam[2]; 80 | eval.error = eval.point2D - measurement; 81 | eval.squaredErrorNorm = eval.error.squaredNorm(); 82 | 83 | #if not ROBUST_PINHOLE 84 | eval.f = mCon * eval.squaredErrorNorm; 85 | #else 86 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 87 | #endif 88 | 89 | return 0; 90 | } 91 | 92 | int JointExtraPinholeCameraFactor::linearize(const Pose &pose, 93 | const Vector2 &measurement, 94 | const Evaluation &eval, 95 | Linearization &lin) const { 96 | assert(lin.jacobians[0].size() == 1); 97 | 98 | Scalar zinv = (Scalar)1.0 / eval.pExtraCam[2]; 99 | 100 | lin.JPoint(0, 0) = zinv; 101 | lin.JPoint(0, 1) = 0; 102 | lin.JPoint(0, 2) = -eval.point2D[0] * zinv; 103 | lin.JPoint(1, 0) = 0; 104 | lin.JPoint(1, 1) = zinv; 105 | lin.JPoint(1, 2) = -eval.point2D[1] * zinv; 106 | 107 | auto &JPose = lin.jacobians[0][0]; 108 | 109 | JPose.resize(2, 6); 110 | 111 | Eigen::Map> JR(JPose.data()); 112 | Eigen::Map> Jt(JPose.data() + 6); 113 | 114 | Jt.noalias() = lin.JPoint * mExtraCamPose.R; 115 | 116 | JR.row(0) = pose.t.cross(Jt.row(0)); 117 | JR.row(1) = pose.t.cross(Jt.row(1)); 118 | 119 | auto &gPose = lin.gPose; 120 | gPose.noalias() = JPose.transpose() * eval.error; 121 | 122 | #if not ROBUST_PINHOLE 123 | JPose *= mSqrtCon; 124 | gPose *= mCon; 125 | #else 126 | preLinearizeRobustKernel(eval, lin); 127 | 128 | JPose.noalias() -= lin.scaledError * gPose.transpose(); 129 | 130 | JPose *= lin.sqrtDrho; 131 | gPose *= lin.Drho; 132 | #endif 133 | 134 | return 0; 135 | } 136 | } // namespace scope 137 | -------------------------------------------------------------------------------- /C++/scope/initializer/factor/PoseConstFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | PoseConstFactor::PoseConstFactor(int pose, const Vector6 &weight, 9 | const Pose &mean, const Vector6 &lbnd, 10 | const Vector6 &ubnd, const std::string &name, 11 | int index) 12 | : Factor(name, index), mPose(pose), mWeight(weight), mMean(mean), 13 | mLowerBnd(lbnd), mUpperBnd(ubnd) { 14 | assert((mean.R.col(0).cross(mean.R.col(1)) - mean.R.col(2)).norm() < 1e-5 && 15 | "mean.R must be a rotational matrix"); 16 | assert((lbnd.cwiseMin(ubnd) - lbnd).norm() < 1e-7); 17 | assert((ubnd.cwiseMax(lbnd) - ubnd).norm() < 1e-7); 18 | } 19 | 20 | int PoseConstFactor::evaluate(const AlignedVector &poses, 21 | const AlignedVector &joints, 22 | Factor::Evaluation &base_eval) const { 23 | auto &eval = dynamic_cast(base_eval); 24 | eval.clear(); 25 | 26 | const auto &i = mPose; 27 | 28 | assert(i >= 0 && i < poses.size()); 29 | 30 | if (i >= poses.size()) { 31 | LOG(ERROR) << "The pose must be valid." << std::endl; 32 | 33 | exit(-1); 34 | } 35 | 36 | auto &pose = poses[i]; 37 | 38 | eval.errorR.noalias() = mMean.R.transpose() * pose.R; 39 | 40 | eval.error.resize(6); 41 | 42 | Eigen::Map omega(eval.xi.data()); 43 | math::SO3::log(eval.errorR, omega); 44 | eval.xi.tail<3>() = pose.t - mMean.t; 45 | 46 | eval.errorL = eval.xi - mLowerBnd; 47 | eval.errorL = eval.errorL.cwiseMin(mBnd); 48 | 49 | eval.errorU = mUpperBnd - eval.xi; 50 | eval.errorU = eval.errorU.cwiseMin(mBnd); 51 | 52 | eval.errorLInv = mScale * eval.errorL.cwiseInverse(); 53 | eval.errorUInv = mScale * eval.errorU.cwiseInverse(); 54 | 55 | eval.expL = eval.errorLInv.array().exp(); 56 | eval.expU = eval.errorUInv.array().exp(); 57 | 58 | eval.error = eval.errorL.cwiseProduct(eval.expL); 59 | eval.error += eval.errorU.cwiseProduct(eval.expU); 60 | 61 | eval.error.noalias() = mWeight.asDiagonal() * eval.error; 62 | 63 | eval.f = eval.error.squaredNorm(); 64 | 65 | eval.status = Status::VALID; 66 | 67 | return 0; 68 | } 69 | 70 | int PoseConstFactor::linearize(const AlignedVector &poses, 71 | const AlignedVector &joints, 72 | const Factor::Evaluation &base_eval, 73 | Factor::Linearization &base_lin) const { 74 | auto &eval = dynamic_cast(base_eval); 75 | auto &lin = dynamic_cast(base_lin); 76 | 77 | lin.clear(); 78 | 79 | const auto &i = mPose; 80 | 81 | assert(i >= 0 && i < poses.size()); 82 | 83 | if (i >= poses.size()) { 84 | LOG(ERROR) << "The pose must be valid." << std::endl; 85 | 86 | exit(-1); 87 | } 88 | 89 | assert(eval.status == Status::VALID); 90 | 91 | if (eval.status != Status::VALID) { 92 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 93 | 94 | exit(-1); 95 | } 96 | 97 | auto &pose = poses[i]; 98 | 99 | auto &J = lin.jacobian; 100 | 101 | math::SO3::dexpinv(eval.xi.head<3>(), lin.dexpinv); 102 | 103 | lin.Derror = eval.expL; 104 | lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv); 105 | lin.Derror -= eval.expU; 106 | lin.Derror += eval.expU.cwiseProduct(eval.errorUInv); 107 | 108 | J.topRightCorner<3, 3>().setZero(); 109 | 110 | const auto &t = poses[i].t; 111 | 112 | Eigen::Map JR(J.data()); 113 | Eigen::Map Jt(J.data() + 18); 114 | 115 | JR.topRows<3>().noalias() = lin.dexpinv * mMean.R.transpose(); 116 | JR(3, 0) = 0; 117 | JR(3, 1) = t[2]; 118 | JR(3, 2) = -t[1]; 119 | JR(4, 0) = -t[2]; 120 | JR(4, 1) = 0; 121 | JR(4, 2) = t[0]; 122 | JR(5, 0) = t[1]; 123 | JR(5, 1) = -t[0]; 124 | JR(5, 2) = 0; 125 | JR.noalias() = lin.Derror.asDiagonal() * JR; 126 | JR.noalias() = mWeight.asDiagonal() * JR; 127 | 128 | Jt.topRows<3>().setZero(); 129 | Jt.bottomRows<3>() = lin.Derror.tail<3>().asDiagonal(); 130 | Jt.bottomRows<3>() = 131 | mWeight.tail<3>().cwiseProduct(lin.Derror.tail<3>()).asDiagonal(); 132 | 133 | lin.status = Status::VALID; 134 | 135 | return 0; 136 | } 137 | } // namespace Initializer 138 | } // namespace scope 139 | -------------------------------------------------------------------------------- /C++/scope/factor/JointPinholeCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | JointPinholeCameraFactor::JointPinholeCameraFactor( 7 | int pose, const Scalar &sigma, const Scalar &eps, 8 | const Vector2 &measurement, const Scalar &confidence, 9 | const std::string &name, int index, bool active) 10 | : PinholeCameraFactor({pose}, {}, {}, {}, sigma, eps, measurement, 11 | confidence, name, index, active) {} 12 | 13 | int JointPinholeCameraFactor::evaluate(const AlignedVector &poses, 14 | const AlignedVector &shapes, 15 | const AlignedVector &joints, 16 | const AlignedVector ¶ms, 17 | Factor::Evaluation &base_eval) const { 18 | auto &eval = dynamic_cast(base_eval); 19 | eval.clear(); 20 | 21 | const auto &i = mvPoses[0]; 22 | assert(i >= 0 && i < poses.size()); 23 | 24 | if (i < 0 || i >= poses.size()) { 25 | LOG(ERROR) << "The pose must be valid." << std::endl; 26 | 27 | exit(-1); 28 | } 29 | 30 | evaluate(poses[i], mMeasurement, eval); 31 | 32 | eval.status = Status::VALID; 33 | 34 | return 0; 35 | } 36 | 37 | int JointPinholeCameraFactor::linearize(const AlignedVector &poses, 38 | const AlignedVector &shapes, 39 | const AlignedVector &joints, 40 | const AlignedVector ¶ms, 41 | const Factor::Evaluation &base_eval, 42 | Factor::Linearization &base_lin) const { 43 | auto &eval = dynamic_cast(base_eval); 44 | auto &lin = dynamic_cast(base_lin); 45 | 46 | lin.clear(); 47 | 48 | assert(eval.status == Status::VALID); 49 | 50 | if (eval.status != Status::VALID) { 51 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 52 | 53 | exit(-1); 54 | } 55 | 56 | const auto &i = mvPoses[0]; 57 | assert(i >= 0 && i < poses.size()); 58 | 59 | if (i < 0 || i >= poses.size()) { 60 | LOG(ERROR) << "The pose must be valid." << std::endl; 61 | 62 | exit(-1); 63 | } 64 | 65 | lin.jacobians[0].resize(1); 66 | 67 | linearize(poses[i], mMeasurement, eval, lin); 68 | 69 | lin.status = Status::VALID; 70 | 71 | return 0; 72 | } 73 | 74 | int JointPinholeCameraFactor::evaluate(const Pose &pose, 75 | const Vector2 &measurement, 76 | Evaluation &eval) const { 77 | const auto &t = pose.t; 78 | 79 | eval.point2D = t.head<2>() / t[2]; 80 | eval.error = eval.point2D - measurement; 81 | eval.squaredErrorNorm = eval.error.squaredNorm(); 82 | 83 | #if not ROBUST_PINHOLE 84 | eval.f = mCon * eval.squaredErrorNorm; 85 | #else 86 | eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm); 87 | #endif 88 | 89 | return 0; 90 | } 91 | 92 | int JointPinholeCameraFactor::linearize(const Pose &pose, 93 | const Vector2 &measurement, 94 | const Evaluation &eval, 95 | Linearization &lin) const { 96 | assert(lin.jacobians[0].size() == 1); 97 | 98 | auto &JPose = lin.jacobians[0][0]; 99 | 100 | JPose.resize(2, 6); 101 | 102 | Eigen::Map> JR(JPose.data()); 103 | Eigen::Map> Jt(JPose.data() + 6); 104 | 105 | Scalar zinv = (Scalar)1.0 / pose.t[2]; 106 | 107 | Jt(0, 0) = zinv; 108 | Jt(0, 1) = 0; 109 | Jt(0, 2) = -eval.point2D[0] * zinv; 110 | Jt(1, 0) = 0; 111 | Jt(1, 1) = zinv; 112 | Jt(1, 2) = -eval.point2D[1] * zinv; 113 | 114 | const auto &t = pose.t; 115 | 116 | JR.row(0) = t.cross(Jt.row(0)); 117 | JR.row(1) = t.cross(Jt.row(1)); 118 | 119 | auto &gPose = lin.gPose; 120 | gPose.noalias() = JPose.transpose() * eval.error; 121 | 122 | #if not ROBUST_PINHOLE 123 | JPose *= mSqrtCon; 124 | gPose *= mCon; 125 | #else 126 | preLinearizeRobustKernel(eval, lin); 127 | 128 | JPose.noalias() -= lin.scaledError * gPose.transpose(); 129 | 130 | JPose *= lin.sqrtDrho; 131 | gPose *= lin.Drho; 132 | #endif 133 | 134 | return 0; 135 | } 136 | } // namespace scope 137 | -------------------------------------------------------------------------------- /C++/scope/factor/PoseConstFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | PoseConstFactor::PoseConstFactor(int pose, const Vector6 &weight, 8 | const Pose &mean, const Vector6 &lbnd, 9 | const Vector6 &ubnd, const std::string &name, 10 | int index, bool active) 11 | : Factor({pose}, {}, {}, {}, name, index, active), 12 | mLowerBnd(lbnd), 13 | mUpperBnd(ubnd), 14 | mWeight(weight), 15 | mMean(mean) { 16 | assert((mean.R.col(0).cross(mean.R.col(1)) - mean.R.col(2)).norm() < 1e-5 && 17 | "mean.R must be a rotational matrix"); 18 | assert((lbnd.cwiseMin(ubnd) - lbnd).norm() < 1e-7); 19 | assert((ubnd.cwiseMax(lbnd) - ubnd).norm() < 1e-7); 20 | } 21 | 22 | int PoseConstFactor::evaluate(const AlignedVector &poses, 23 | const AlignedVector &shapes, 24 | const AlignedVector &joints, 25 | const AlignedVector ¶ms, 26 | Factor::Evaluation &base_eval) const { 27 | auto &eval = dynamic_cast(base_eval); 28 | eval.clear(); 29 | 30 | const auto &i = mvPoses[0]; 31 | assert(i >= 0 && i < poses.size()); 32 | 33 | if (i >= poses.size()) { 34 | LOG(ERROR) << "The pose must be valid." << std::endl; 35 | 36 | exit(-1); 37 | } 38 | 39 | auto &pose = poses[i]; 40 | 41 | eval.errorR.noalias() = mMean.R.transpose() * pose.R; 42 | 43 | eval.error.resize(6); 44 | 45 | Eigen::Map omega(eval.xi.data()); 46 | math::SO3::log(eval.errorR, omega); 47 | eval.xi.tail<3>() = pose.t - mMean.t; 48 | 49 | eval.errorL = eval.xi - mLowerBnd; 50 | eval.errorL = eval.errorL.cwiseMin(mBnd); 51 | 52 | eval.errorU = mUpperBnd - eval.xi; 53 | eval.errorU = eval.errorU.cwiseMin(mBnd); 54 | 55 | eval.errorLInv = mScale * eval.errorL.cwiseInverse(); 56 | eval.errorUInv = mScale * eval.errorU.cwiseInverse(); 57 | 58 | eval.expL = eval.errorLInv.array().exp(); 59 | eval.expU = eval.errorUInv.array().exp(); 60 | 61 | eval.error = eval.errorL.cwiseProduct(eval.expL); 62 | eval.error += eval.errorU.cwiseProduct(eval.expU); 63 | 64 | eval.error.noalias() = mWeight.asDiagonal() * eval.error; 65 | 66 | eval.f = eval.error.squaredNorm(); 67 | 68 | eval.status = Status::VALID; 69 | 70 | return 0; 71 | } 72 | 73 | int PoseConstFactor::linearize(const AlignedVector &poses, 74 | const AlignedVector &shapes, 75 | const AlignedVector &joints, 76 | const AlignedVector ¶ms, 77 | const Factor::Evaluation &base_eval, 78 | Factor::Linearization &base_lin) const { 79 | auto &eval = dynamic_cast(base_eval); 80 | auto &lin = dynamic_cast(base_lin); 81 | 82 | lin.clear(); 83 | 84 | assert(eval.status == Status::VALID); 85 | 86 | if (eval.status != Status::VALID) { 87 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 88 | 89 | exit(-1); 90 | } 91 | 92 | const auto &i = mvPoses[0]; 93 | assert(i >= 0 && i < poses.size()); 94 | 95 | if (i < 0 || i >= poses.size()) { 96 | LOG(ERROR) << "The joint must be valid." << std::endl; 97 | 98 | exit(-1); 99 | } 100 | 101 | lin.jacobians[0].resize(1); 102 | 103 | auto &J = lin.jacobians[0][0]; 104 | J.resize(6, 6); 105 | 106 | math::SO3::dexpinv(eval.xi.head<3>(), lin.dexpinv); 107 | 108 | lin.Derror = eval.expL; 109 | lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv); 110 | lin.Derror -= eval.expU; 111 | lin.Derror += eval.expU.cwiseProduct(eval.errorUInv); 112 | 113 | J.topRightCorner<3, 3>().setZero(); 114 | 115 | const auto &t = poses[i].t; 116 | 117 | Eigen::Map JR(J.data()); 118 | Eigen::Map Jt(J.data() + 18); 119 | 120 | JR.topRows<3>().noalias() = lin.dexpinv * mMean.R.transpose(); 121 | JR(3, 0) = 0; 122 | JR(3, 1) = t[2]; 123 | JR(3, 2) = -t[1]; 124 | JR(4, 0) = -t[2]; 125 | JR(4, 1) = 0; 126 | JR(4, 2) = t[0]; 127 | JR(5, 0) = t[1]; 128 | JR(5, 1) = -t[0]; 129 | JR(5, 2) = 0; 130 | JR.noalias() = lin.Derror.asDiagonal() * JR; 131 | JR.noalias() = mWeight.asDiagonal() * JR; 132 | 133 | Jt.topRows<3>().setZero(); 134 | Jt.bottomRows<3>() = 135 | mWeight.tail<3>().cwiseProduct(lin.Derror.tail<3>()).asDiagonal(); 136 | 137 | lin.status = Status::VALID; 138 | 139 | return 0; 140 | } 141 | } // namespace scope 142 | -------------------------------------------------------------------------------- /C++/scope/factor/FullJointPinholeCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | FullJointPinholeCameraFactor::FullJointPinholeCameraFactor( 7 | int pose, int camParam, const Scalar &sigma, const Scalar &eps, 8 | const Vector2 &measurement, const Scalar &confidence, 9 | const std::string &name, int index, bool active) 10 | : JointPinholeCameraFactor(pose, sigma, eps, measurement, confidence, name, 11 | index, active) { 12 | mvParams = {camParam}; 13 | } 14 | 15 | int FullJointPinholeCameraFactor::evaluate( 16 | const AlignedVector &poses, const AlignedVector &shapes, 17 | const AlignedVector &joints, const AlignedVector ¶ms, 18 | Factor::Evaluation &base_eval) const { 19 | auto &eval = dynamic_cast(base_eval); 20 | eval.clear(); 21 | 22 | const auto &pose = mvPoses[0]; 23 | assert(pose >= 0 && pose < poses.size()); 24 | 25 | if (pose < 0 || pose >= poses.size()) { 26 | LOG(ERROR) << "The pose must be valid." << std::endl; 27 | 28 | exit(-1); 29 | } 30 | 31 | const auto &camParam = mvParams[0]; 32 | assert(camParam >= 0 && camParam < params.size()); 33 | 34 | if (camParam < 0 || camParam > params.size()) { 35 | LOG(ERROR) << "The parameter must be valid." << std::endl; 36 | 37 | exit(-1); 38 | } 39 | 40 | evaluate(poses[pose], params[camParam], mMeasurement, eval); 41 | 42 | eval.status = Status::VALID; 43 | 44 | return 0; 45 | } 46 | 47 | int FullJointPinholeCameraFactor::linearize( 48 | const AlignedVector &poses, const AlignedVector &shapes, 49 | const AlignedVector &joints, const AlignedVector ¶ms, 50 | const Factor::Evaluation &base_eval, 51 | Factor::Linearization &base_lin) const { 52 | auto &eval = dynamic_cast(base_eval); 53 | auto &lin = dynamic_cast(base_lin); 54 | 55 | lin.clear(); 56 | 57 | assert(eval.status == Status::VALID); 58 | 59 | if (eval.status != Status::VALID) { 60 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 61 | 62 | exit(-1); 63 | } 64 | 65 | const auto &pose = mvPoses[0]; 66 | assert(pose >= 0 && pose < poses.size()); 67 | 68 | if (pose < 0 || pose >= poses.size()) { 69 | LOG(ERROR) << "The pose must be valid." << std::endl; 70 | 71 | exit(-1); 72 | } 73 | 74 | const auto &camParam = mvParams[0]; 75 | assert(camParam >= 0 && camParam < params.size()); 76 | 77 | if (camParam < 0 || camParam > params.size()) { 78 | LOG(ERROR) << "The parameter must be valid." << std::endl; 79 | 80 | exit(-1); 81 | } 82 | 83 | lin.jacobians[0].resize(1); 84 | lin.jacobians[3].resize(1); 85 | 86 | linearize(poses[pose], params[camParam], mMeasurement, eval, lin); 87 | 88 | lin.status = Status::VALID; 89 | 90 | return 0; 91 | } 92 | 93 | int FullJointPinholeCameraFactor::evaluate(const Pose &pose, 94 | const VectorX &camParam, 95 | const Vector2 &measurement, 96 | Evaluation &eval) const { 97 | assert(camParam.size() == 4); 98 | 99 | eval.camMeasurement = camParam.tail<2>(); 100 | eval.camMeasurement[0] += camParam[0] * measurement[0]; 101 | eval.camMeasurement[1] += camParam[1] * measurement[1]; 102 | 103 | JointPinholeCameraFactor::evaluate(pose, eval.camMeasurement, eval); 104 | 105 | return 0; 106 | } 107 | 108 | int FullJointPinholeCameraFactor::linearize(const Pose &pose, 109 | const VectorX &camParam, 110 | const Vector2 &measurement, 111 | const Evaluation &eval, 112 | Linearization &lin) const { 113 | assert(camParam.size() == 4); 114 | 115 | assert(lin.jacobians[0].size() == 1); 116 | assert(lin.jacobians[3].size() == 1); 117 | 118 | JointPinholeCameraFactor::linearize(pose, eval.camMeasurement, eval, lin); 119 | 120 | auto &JCam = lin.jacobians[3][0]; 121 | 122 | JCam.resize(2, 4); 123 | 124 | JCam(0, 0) = -measurement[0]; 125 | JCam(0, 1) = 0; 126 | JCam(0, 2) = -1; 127 | JCam(0, 3) = 0; 128 | 129 | JCam(1, 0) = 0; 130 | JCam(1, 1) = -measurement[1]; 131 | JCam(1, 2) = 0; 132 | JCam(1, 3) = -1; 133 | 134 | auto &gCam = lin.gCam; 135 | gCam.noalias() = JCam.transpose() * eval.error; 136 | 137 | #if not ROBUST_PINHOLE 138 | JCam *= mSqrtCon; 139 | gCam *= mCon; 140 | #else 141 | JCam.noalias() -= lin.scaledError * gCam.transpose(); 142 | 143 | JCam *= lin.sqrtDrho; 144 | gCam *= lin.Drho; 145 | #endif 146 | 147 | return 0; 148 | } 149 | } // namespace scope 150 | -------------------------------------------------------------------------------- /C++/scope/initializer/ExtTorsoInitializer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | ExtTorsoInitializer::ExtTorsoInitializer(const Options &options, const Vector3 &RelJointLocation) 9 | : Initializer(1, options), mRelJointLocation({RelJointLocation}) {} 10 | 11 | int ExtTorsoInitializer::initialize(const Pose &T0, 12 | const AlignedVector &joints) const { 13 | assert(joints.size() == mNumJoints); 14 | 15 | if (joints.size() != mNumJoints) { 16 | LOG(ERROR) << "There should be " << mNumJoints << " joint states." 17 | << std::endl; 18 | 19 | exit(-1); 20 | } 21 | 22 | Vector3 xi; 23 | Matrix3 R; 24 | 25 | // Scale spine joint to feasbile range 26 | math::SO3::log(joints[0], xi); 27 | xi[0] = std::max(xi[0], SpineLowerBnd); 28 | xi[0] = std::min(xi[0], SpineUpperBnd); 29 | xi[1] = xi[2] = 0; 30 | math::SO3::exp(xi, R); 31 | 32 | mSpineAngle[0] = mSpineAngle[1] = xi[0]; 33 | 34 | Initializer::initialize(T0, {R}); 35 | 36 | return 0; 37 | } 38 | 39 | int ExtTorsoInitializer::initialize(const Pose &T0, const Scalar& SpineAngle) const { 40 | // Scale spine joint to feasbile range 41 | mSpineAngle[0] = SpineAngle; 42 | mSpineAngle[0] = std::max(mSpineAngle[0], SpineLowerBnd); 43 | mSpineAngle[0] = std::min(mSpineAngle[0], SpineUpperBnd); 44 | 45 | mSpineAngle[1] = mSpineAngle[0]; 46 | 47 | Matrix3 R = Matrix3::Identity(); 48 | R(1, 1) = cos(mSpineAngle[0]); 49 | R(2, 1) = sin(mSpineAngle[0]); 50 | R(2, 2) = R(1, 1); 51 | R(1, 2) = -R(2, 1); 52 | 53 | Initializer::initialize(T0, {R}); 54 | 55 | return 0; 56 | } 57 | 58 | int ExtTorsoInitializer::FKintree(int n) const { 59 | auto &poses = mvPoses[n]; 60 | auto &joints = mvJoints[n]; 61 | 62 | poses[1].R.noalias() = poses[0].R * joints[0]; 63 | poses[1].t = poses[0].t; 64 | poses[1].t.noalias() += poses[0].R * mRelJointLocation; 65 | 66 | return 0; 67 | } 68 | 69 | int ExtTorsoInitializer::DFKintree() const { 70 | const auto &poses = mvPoses[0]; 71 | 72 | mB.head<3>() = poses[0].R.col(0); 73 | mB.tail<3>().noalias() = poses[1].t.cross(poses[0].R.col(0)); 74 | 75 | return 0; 76 | } 77 | 78 | int ExtTorsoInitializer::updateGaussNewton() const { 79 | mH.resize(7, 7); 80 | 81 | mH.block<6, 1>(0, 6).noalias() = mvMxx[1] * mB; 82 | mH(6, 6) = mB.dot(mH.block<6, 1>(0, 6)); 83 | mH(6, 6) += mvMuu[0](0, 0); 84 | 85 | mH.topLeftCorner<6, 6>() = mvMxx[0] + mvMxx[1]; 86 | 87 | mH.block<1, 6>(6, 0).noalias() = mH.block<6, 1>(0, 6).transpose(); 88 | 89 | mh.resize(7); 90 | 91 | mh[6] = mB.dot(mvmx[1]); 92 | mh[6] += mvmu[0][0]; 93 | 94 | mh.head<6>() = mvmx[0] + mvmx[1]; 95 | 96 | return 0; 97 | } 98 | 99 | int ExtTorsoInitializer::solveGaussNewton() const { 100 | mLambda = mH.diagonal() * (mDLambda - 1); 101 | mLambda.array() += mOptions.delta; 102 | mH.diagonal() += mLambda; 103 | 104 | mHchol.compute(mH.topLeftCorner<6, 6>()); 105 | 106 | mS.noalias() = -mHchol.solve(mH.block<6, 1>(0, 6)); 107 | ms.noalias() = -mHchol.solve(mh.head<6>()); 108 | 109 | mA = mH(6, 6) + mH.block<6, 1>(0, 6).dot(mS); 110 | ma = mh[6] + mH.block<6, 1>(0, 6).dot(ms); 111 | 112 | mhGN.resize(7); 113 | 114 | // Guarantee mKneeJoint in [0, 0.78 * M_PI] 115 | mhGN[6] = -ma / mA; 116 | mhGN[6] = std::max(mhGN[6], SpineLowerBnd - mSpineAngle[0]); 117 | mhGN[6] = std::min(mhGN[6], SpineUpperBnd - mSpineAngle[0]); 118 | mhGN.head<6>() = ms; 119 | mhGN.head<6>().noalias() += mS * mhGN[6]; 120 | 121 | mSquaredError = mh; 122 | mSquaredError += 0.5 * mH * mhGN; 123 | 124 | mE = mhGN.dot(mSquaredError); 125 | mSquaredError = mhGN.cwiseAbs2(); 126 | mE -= 0.5 * mLambda.dot(mSquaredError); 127 | 128 | return 0; 129 | } 130 | 131 | int ExtTorsoInitializer::update(Scalar stepsize) const { 132 | assert(stepsize > 0); 133 | 134 | mDRootPoseChange = mhGN.head<6>() * stepsize; 135 | Pose::exp(mDRootPoseChange, mRootPoseChange); 136 | 137 | mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R; 138 | mvPoses[1][0].t = mRootPoseChange.t; 139 | mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t; 140 | 141 | mSpineAngle[1] = mSpineAngle[0] + stepsize * mhGN[6]; 142 | 143 | mvJoints[1][0].setIdentity(); 144 | mvJoints[1][0](1, 1) = std::cos(mSpineAngle[1]); 145 | mvJoints[1][0](2, 2) = mvJoints[1][0](1, 1); 146 | mvJoints[1][0](2, 1) = std::sin(mSpineAngle[1]); 147 | mvJoints[1][0](1, 2) = -mvJoints[1][0](2, 1); 148 | 149 | FKintree(1); 150 | 151 | return 0; 152 | } 153 | 154 | int ExtTorsoInitializer::accept() const { 155 | mSpineAngle[0] = mSpineAngle[1]; 156 | 157 | return Initializer::accept(); 158 | } 159 | } // namespace Initializer 160 | } // namespace scope 161 | -------------------------------------------------------------------------------- /C++/scope/factor/FullJointExtraPinholeCameraFactor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace scope { 6 | FullJointExtraPinholeCameraFactor::FullJointExtraPinholeCameraFactor( 7 | int pose, int camParam, const Scalar &sigma, const Scalar &eps, 8 | const Pose &extraCamPose, const Vector2 &measurement, 9 | const Scalar &confidence, const std::string &name, int index, bool active) 10 | : JointExtraPinholeCameraFactor(pose, sigma, eps, extraCamPose, measurement, 11 | confidence, name, index, active) { 12 | mvParams = {camParam}; 13 | } 14 | 15 | int FullJointExtraPinholeCameraFactor::evaluate( 16 | const AlignedVector &poses, const AlignedVector &shapes, 17 | const AlignedVector &joints, const AlignedVector ¶ms, 18 | Factor::Evaluation &base_eval) const { 19 | auto &eval = dynamic_cast(base_eval); 20 | eval.clear(); 21 | 22 | const auto &pose = mvPoses[0]; 23 | assert(pose >= 0 && pose < poses.size()); 24 | 25 | if (pose < 0 || pose >= poses.size()) { 26 | LOG(ERROR) << "The pose must be valid." << std::endl; 27 | 28 | exit(-1); 29 | } 30 | 31 | const auto &camParam = mvParams[0]; 32 | assert(camParam >= 0 && camParam < params.size()); 33 | 34 | if (camParam < 0 || camParam > params.size()) { 35 | LOG(ERROR) << "The parameter must be valid." << std::endl; 36 | 37 | exit(-1); 38 | } 39 | 40 | evaluate(poses[pose], params[camParam], mMeasurement, eval); 41 | 42 | eval.status = Status::VALID; 43 | 44 | return 0; 45 | } 46 | 47 | int FullJointExtraPinholeCameraFactor::linearize( 48 | const AlignedVector &poses, const AlignedVector &shapes, 49 | const AlignedVector &joints, const AlignedVector ¶ms, 50 | const Factor::Evaluation &base_eval, 51 | Factor::Linearization &base_lin) const { 52 | auto &eval = dynamic_cast(base_eval); 53 | auto &lin = dynamic_cast(base_lin); 54 | 55 | lin.clear(); 56 | 57 | assert(eval.status == Status::VALID); 58 | 59 | if (eval.status != Status::VALID) { 60 | LOG(ERROR) << "The evaluation must be valid." << std::endl; 61 | 62 | exit(-1); 63 | } 64 | 65 | const auto &pose = mvPoses[0]; 66 | assert(pose >= 0 && pose < poses.size()); 67 | 68 | if (pose < 0 || pose >= poses.size()) { 69 | LOG(ERROR) << "The pose must be valid." << std::endl; 70 | 71 | exit(-1); 72 | } 73 | 74 | const auto &camParam = mvParams[0]; 75 | assert(camParam >= 0 && camParam < params.size()); 76 | 77 | if (camParam < 0 || camParam > params.size()) { 78 | LOG(ERROR) << "The parameter must be valid." << std::endl; 79 | 80 | exit(-1); 81 | } 82 | 83 | lin.jacobians[0].resize(1); 84 | lin.jacobians[3].resize(1); 85 | 86 | linearize(poses[pose], params[camParam], mMeasurement, eval, lin); 87 | 88 | lin.status = Status::VALID; 89 | 90 | return 0; 91 | } 92 | 93 | int FullJointExtraPinholeCameraFactor::evaluate(const Pose &pose, 94 | const VectorX &camParam, 95 | const Vector2 &measurement, 96 | Evaluation &eval) const { 97 | assert(camParam.size() == 4); 98 | 99 | eval.camMeasurement = camParam.tail<2>(); 100 | eval.camMeasurement[0] += camParam[0] * measurement[0]; 101 | eval.camMeasurement[1] += camParam[1] * measurement[1]; 102 | 103 | JointExtraPinholeCameraFactor::evaluate(pose, eval.camMeasurement, eval); 104 | 105 | return 0; 106 | } 107 | 108 | int FullJointExtraPinholeCameraFactor::linearize(const Pose &pose, 109 | const VectorX &camParam, 110 | const Vector2 &measurement, 111 | const Evaluation &eval, 112 | Linearization &lin) const { 113 | assert(camParam.size() == 4); 114 | 115 | assert(lin.jacobians[0].size() == 1); 116 | assert(lin.jacobians[3].size() == 1); 117 | 118 | JointExtraPinholeCameraFactor::linearize(pose, eval.camMeasurement, eval, 119 | lin); 120 | 121 | auto &JCam = lin.jacobians[3][0]; 122 | 123 | JCam.resize(2, 4); 124 | 125 | JCam(0, 0) = -measurement[0]; 126 | JCam(0, 1) = 0; 127 | JCam(0, 2) = -1; 128 | JCam(0, 3) = 0; 129 | 130 | JCam(1, 0) = 0; 131 | JCam(1, 1) = -measurement[1]; 132 | JCam(1, 2) = 0; 133 | JCam(1, 3) = -1; 134 | 135 | auto &gCam = lin.gCam; 136 | gCam.noalias() = JCam.transpose() * eval.error; 137 | 138 | #if not ROBUST_PINHOLE 139 | JCam *= mSqrtCon; 140 | gCam *= mCon; 141 | #else 142 | JCam.noalias() -= lin.scaledError * gCam.transpose(); 143 | 144 | JCam *= lin.sqrtDrho; 145 | gCam *= lin.Drho; 146 | #endif 147 | 148 | return 0; 149 | } 150 | } // namespace scope 151 | -------------------------------------------------------------------------------- /C++/scope/initializer/ArmInitializer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace scope { 7 | namespace Initializer { 8 | ArmInitializer::ArmInitializer(const Options &options, 9 | const std::array &RelJointLocations, 10 | Arm LR) 11 | : Initializer(2, options), mRelJointLocations(RelJointLocations), mArm(LR), 12 | mElbowLowerBnd(LR == Arm::Left ? -0.85 * M_PI : 0), 13 | mElbowUpperBnd(LR == Arm::Left ? 0 : 0.85 * M_PI) {} 14 | 15 | int ArmInitializer::initialize(const Pose &T0, 16 | const AlignedVector &joints) const { 17 | assert(joints.size() == mNumJoints); 18 | 19 | if (joints.size() != mNumJoints) { 20 | LOG(ERROR) << "There should be " << mNumJoints << " joint states." 21 | << std::endl; 22 | 23 | exit(-1); 24 | } 25 | 26 | Vector3 xi; 27 | Matrix3 R; 28 | 29 | // Scale eblow joint to feasbile range 30 | math::SO3::log(joints[1], xi); 31 | xi[1] = std::max(xi[1], mElbowLowerBnd); 32 | xi[1] = std::min(xi[1], mElbowUpperBnd); 33 | xi[0] = xi[2] = 0; 34 | math::SO3::exp(xi, R); 35 | 36 | mElbowJoint[0] = mElbowJoint[1] = xi[1]; 37 | 38 | Initializer::initialize(T0, {joints[0], R}); 39 | 40 | return 0; 41 | } 42 | 43 | int ArmInitializer::FKintree(int n) const { 44 | auto &poses = mvPoses[n]; 45 | auto &joints = mvJoints[n]; 46 | 47 | poses[1].R.noalias() = poses[0].R * joints[0]; 48 | poses[1].t = poses[0].t; 49 | poses[1].t.noalias() += poses[0].R * mRelJointLocations[0]; 50 | 51 | poses[2].R.noalias() = poses[1].R * joints[1]; 52 | poses[2].t = poses[1].t; 53 | poses[2].t.noalias() += poses[1].R * mRelJointLocations[1]; 54 | 55 | return 0; 56 | } 57 | 58 | int ArmInitializer::DFKintree() const { 59 | const auto &poses = mvPoses[0]; 60 | 61 | mB0.topRows<3>() = poses[0].R; 62 | mB0.block<3, 1>(3, 0).noalias() = poses[1].t.cross(poses[0].R.col(0)); 63 | mB0.block<3, 1>(3, 1).noalias() = poses[1].t.cross(poses[0].R.col(1)); 64 | mB0.block<3, 1>(3, 2).noalias() = poses[1].t.cross(poses[0].R.col(2)); 65 | 66 | mB1.head<3>() = poses[1].R.col(1); 67 | mB1.tail<3>().noalias() = poses[2].t.cross(poses[1].R.col(1)); 68 | 69 | return 0; 70 | } 71 | 72 | int ArmInitializer::updateGaussNewton() const { 73 | mH.resize(4, 4); 74 | 75 | Vector6 temp1; 76 | Matrix6 temp2; 77 | Matrix63 temp3; 78 | 79 | temp1.noalias() = mvMxx[2] * mB1; 80 | mH.topRightCorner<3, 1>().noalias() = mB0.transpose() * temp1; 81 | mH(3, 3) = mB1.dot(temp1); 82 | mH(3, 3) += mvMuu[1](1, 1); 83 | 84 | temp2 = mvMxx[1] + mvMxx[2]; 85 | temp3.noalias() = temp2 * mB0; 86 | mH.topLeftCorner<3, 3>().noalias() = mB0.transpose() * temp3; 87 | mH.topLeftCorner<3, 3>().noalias() += mvMuu[0]; 88 | 89 | mH.bottomLeftCorner<1, 3>().noalias() = mH.topRightCorner<3, 1>().transpose(); 90 | 91 | mh.resize(4); 92 | 93 | mh[3] = mB1.dot(mvmx[2]); 94 | mh[3] += mvmu[1][1]; 95 | 96 | temp1 = mvmx[1] + mvmx[2]; 97 | mh.head<3>().noalias() = mB0.transpose() * temp1; 98 | mh.head<3>() += mvmu[0]; 99 | 100 | return 0; 101 | } 102 | 103 | int ArmInitializer::solveGaussNewton() const { 104 | mLambda = mH.diagonal() * (mDLambda - 1); 105 | mLambda.array() += mOptions.delta; 106 | mH.diagonal() += mLambda; 107 | 108 | mHchol.compute(mH.topLeftCorner<3, 3>()); 109 | 110 | mS.noalias() = -mHchol.solve(mH.block<3, 1>(0, 3)); 111 | ms.noalias() = -mHchol.solve(mh.head<3>()); 112 | 113 | mA = mH(3, 3) + mH.block<3, 1>(0, 3).dot(mS); 114 | ma = mh[3] + mH.block<3, 1>(0, 3).dot(ms); 115 | 116 | mhGN.resize(4); 117 | 118 | // Guarantee mKneeJoint in [0, 0.78 * M_PI] 119 | mhGN[3] = -ma / mA; 120 | mhGN[3] = std::max(mhGN[3], mElbowLowerBnd - mElbowJoint[0]); 121 | mhGN[3] = std::min(mhGN[3], mElbowUpperBnd - mElbowJoint[0]); 122 | mhGN.head<3>() = ms; 123 | mhGN.head<3>().noalias() += mS * mhGN[3]; 124 | 125 | mSquaredError = mh; 126 | mSquaredError += 0.5 * mH * mhGN; 127 | 128 | mE = mhGN.dot(mSquaredError); 129 | mSquaredError = mhGN.cwiseAbs2(); 130 | mE -= 0.5 * mLambda.dot(mSquaredError); 131 | 132 | return 0; 133 | } 134 | 135 | int ArmInitializer::update(Scalar stepsize) const { 136 | assert(stepsize > 0); 137 | 138 | mDJointChange = mhGN.head<3>() * stepsize; 139 | math::SO3::exp(mDJointChange, mJointChange); 140 | mvJoints[1][0].noalias() = mJointChange * mvJoints[0][0]; 141 | 142 | mElbowJoint[1] = mElbowJoint[0] + stepsize * mhGN[3]; 143 | 144 | mvJoints[1][1].setIdentity(); 145 | mvJoints[1][1](0, 0) = std::cos(mElbowJoint[1]); 146 | mvJoints[1][1](2, 2) = mvJoints[1][1](0, 0); 147 | mvJoints[1][1](0, 2) = std::sin(mElbowJoint[1]); 148 | mvJoints[1][1](2, 0) = -mvJoints[1][1](0, 2); 149 | 150 | FKintree(1); 151 | 152 | return 0; 153 | } 154 | 155 | int ArmInitializer::accept() const { 156 | mElbowJoint[0] = mElbowJoint[1]; 157 | 158 | return Initializer::accept(); 159 | } 160 | } // namespace Initializer 161 | } // namespace scope 162 | --------------------------------------------------------------------------------